C++ API

In this tutorial, we illustrate how you can use wavemap’s C++ API in your own projects.

Tip

An example package that combines the setup steps and code examples that follow can be found here.

CMake target setup

Once you included wavemap’s C++ library in your CMake project, for example by following our installation instructions, the last remaining step to start using it is to configure your CMake target (e.g. library or executable) to use it.

Wavemap’s C++ library contains three main components:

wavemap_core: The framework’s core algorithms, data structures and utilities
wavemap_io: Functions to read and write maps to streams and files
wavemap_pipeline: A measurement integration and map management pipeline

For an example executable that performs some operations on a map after reading it from a file, you would need to link:

add_executable(your_executable your_code.cc)
target_link_libraries(your_executable
      PUBLIC wavemap::wavemap_core wavemap::wavemap_io)

Note that this will automatically make wavemap’s include directories (headers) available to your_code.cc.

We strongly recommend to also call the set_wavemap_target_properties function to ensure your target’s compilation options are compatible with those of wavemap:

set_wavemap_target_properties(your_executable)

Code examples

In the following sections, you’ll find sample code for common tasks. If you’d like to request examples for additional tasks or contribute new examples, please don’t hesitate to contact us.

Serializing maps

In this section, we’ll demonstrate how to serialize and deserialize maps using wavemap’s lightweight and efficient binary format. This format is consistent across wavemap’s C++, Python, and ROS interfaces. For instance, you can create maps on a robot with ROS and later load them into a rendering engine plugin that only depends on wavemap’s C++ library.

Binary files

Maps can be saved to disk with

#include <wavemap/io/file_conversions.h>

int main(int, char**) {
  // Create an empty map for illustration purposes
  wavemap::HashedWaveletOctreeConfig config;
  wavemap::HashedWaveletOctree map(config);

  // Save the map
  const bool success = wavemap::io::mapToFile(map, "/path/to/your/map.wvmp");
}

and read using

#include <wavemap/io/file_conversions.h>

int main(int, char**) {
  // Create a smart pointer that will own the loaded map
  wavemap::MapBase::Ptr loaded_map;

  // Load the map
  const bool success =
      wavemap::io::fileToMap("/path/to/your/map.wvmp", loaded_map);
}

Byte streams

We also provide an alternative, lower-level interface to convert maps to (byte) streams

#include <strstream>

#include <wavemap/io/stream_conversions.h>

int main(int, char**) {
  // Create an empty map for illustration purposes
  wavemap::HashedWaveletOctreeConfig config;
  wavemap::HashedWaveletOctree map(config);

  // Create an output stream for illustration purposes
  std::ostrstream output_stream;

  // Save the map
  bool success = wavemap::io::mapToStream(map, output_stream);
  output_stream.flush();
  success &= output_stream.good();
}

and read them with

#include <strstream>

#include <wavemap/io/stream_conversions.h>

int main(int, char**) {
  // Create a smart pointer that will own the loaded map
  wavemap::MapBase::Ptr loaded_map;

  // Create an input stream for illustration purposes
  std::istrstream input_stream{""};

  // Load the map
  const bool success = wavemap::io::streamToMap(input_stream, loaded_map);
}

Queries

In this section, we illustrate how you can query the map and classify whether a point or region of interest is occupied.

Node indices

The map models the environment by filling it with cubes of variable sizes, arranged as the nodes of an octree. Node indices are defined as integer [X, Y, Z, height] coordinates, whose XYZ values correspond to the node’s position in the octree’s grid at the given height, or level in the tree. Height 0 corresponds to the map’s maximum resolution, and the grid resolution is halved for each subsequent height level.

Fixed resolution

Querying the value of a single node in the highest resolution grid (height=0) can be done as follows.

#include <wavemap/core/map/map_base.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Declare a map pointer for illustration purposes
  // NOTE: See the other tutorials on how to load maps from files or ROS topics,
  //       such as the map topic published by the wavemap ROS server.
  MapBase::Ptr map;

  // Declare the index to query
  // NOTE: See wavemap/indexing/index_conversions.h for helper methods
  //       to compute and convert indices.
  const Index3D query_index = Index3D::Zero();

  // Query the map
  const FloatingPoint occupancy_log_odds = map->getCellValue(query_index);
  examples::doSomething(occupancy_log_odds);
}

Multi-res averages

It is also possible to query lower resolution nodes, whose values correspond to the average estimated occupancy of the volume they cover.

#include <wavemap/core/indexing/index_conversions.h>
#include <wavemap/core/map/hashed_wavelet_octree.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Declare a map pointer for illustration purposes
  // NOTE: See the other tutorials on how to load maps from files or ROS topics,
  //       such as the map topic published by the wavemap ROS server.
  HashedWaveletOctree::Ptr map;

  // Define the center point and the minimum width of the octree cell to query
  const Point3D query_point = Point3D::Zero();
  const FloatingPoint query_min_cell_width = 0.5f;  // in meters

  // Convert it to an octree node index
  const FloatingPoint map_min_cell_width = map->getMinCellWidth();
  const IndexElement query_height = convert::cellWidthToHeight(
      query_min_cell_width, 1.f / map_min_cell_width);
  const OctreeIndex query_index =
      convert::pointToNodeIndex(query_point, map_min_cell_width, query_height);

  // Query the map
  const FloatingPoint occupancy_log_odds = map->getCellValue(query_index);
  examples::doSomething(occupancy_log_odds);
}

Accelerators

In case you intend to look up multiple node values, we recommend using wavemap’s query accelerator which traverses the octree significantly faster by caching parent nodes.

#include <wavemap/core/map/hashed_wavelet_octree.h>
#include <wavemap/core/utils/iterate/grid_iterator.h>
#include <wavemap/core/utils/query/query_accelerator.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Declare a map pointer for illustration purposes
  // NOTE: See the other tutorials on how to load maps from files or ROS topics,
  //       such as the map topic published by the wavemap ROS server.
  HashedWaveletOctree::Ptr map;

  // Create the query accelerator
  QueryAccelerator query_accelerator(*map);

  // Query all points within a grid
  for (const auto& query_index :
       Grid<3>(Index3D::Constant(-10), Index3D::Constant(10))) {
    const FloatingPoint occupancy_log_odds =
        query_accelerator.getCellValue(query_index);
    examples::doSomething(occupancy_log_odds);
  }
}

Real coordinates

Many applications require occupancy estimates at arbitrary 3D points, with real-valued coordinates. Such estimates are computed by interpolating the map.

Caution

If your query points are expressed in a different coordinate frame than the map, do not forget to transform them into the map frame before you continue.

Nearest neighbor interpolation

The simplest form of interpolation simply looks up the value of the map node that is closest to the query point.

#include <wavemap/core/map/map_base.h>
#include <wavemap/core/utils/query/map_interpolator.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Create an empty map for illustration purposes
  // NOTE: See the other tutorials on how to load maps from files or ROS topics,
  //       such as the map topic published by the wavemap ROS server.
  MapBase::Ptr map;

  // Declare the point to query [in map frame]
  const Point3D query_point = Point3D::Zero();

  // Query the value of the nearest cell in the map
  const FloatingPoint occupancy_log_odds =
      interpolate::nearestNeighbor(*map, query_point);
  examples::doSomething(occupancy_log_odds);
}

Trilinear interpolation

Another option is to linearly interpolate the map along the x, y, and z axes. This method produces cleaner, more accurate results at the cost of being slightly slower, since it needs to query 8 neighboring map nodes.

#include <wavemap/core/map/map_base.h>
#include <wavemap/core/utils/query/map_interpolator.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Create an empty map for illustration purposes
  // NOTE: See the other tutorials on how to load maps from files or ROS topics,
  //       such as the map topic published by the wavemap ROS server.
  MapBase::Ptr map;

  // Declare the point to query [in map frame]
  const Point3D query_point = Point3D::Zero();

  // Query the map and compute the interpolated value
  const FloatingPoint occupancy_log_odds =
      interpolate::trilinear(*map, query_point);
  examples::doSomething(occupancy_log_odds);
}

Occupancy classification

Once the estimated occupancy at a node or point has been retrieved, it can be classified as follows.

#include <wavemap/core/utils/query/probability_conversions.h>

#include "../common.h"

using namespace wavemap;
int main(int, char**) {
  // Declare a floating point value representing the occupancy posterior in log
  // odds as queried from the map in one of the previous examples
  const FloatingPoint occupancy_log_odds{};

  // A point is considered unobserved if its occupancy posterior is equal to the
  // prior. Wavemap assumes that an unobserved point is equally likely to be
  // free or occupied. In other words, the prior occupancy probability is 0.5,
  // which corresponds to a log odds value of 0.0. Accounting for numerical
  // noise, checking whether a point is unobserved can be done as follows:
  constexpr FloatingPoint kUnobservedThreshold = 1e-3;
  const bool is_unobserved =
      std::abs(occupancy_log_odds) < kUnobservedThreshold;
  examples::doSomething(is_unobserved);

  // In case you would like to convert log odds into probabilities, we provide
  // the following convenience function:
  const FloatingPoint occupancy_probability =
      convert::logOddsToProbability(occupancy_log_odds);
  examples::doSomething(occupancy_probability);

  // To classify whether a point is estimated to be occupied or free, you need
  // to choose a discrimination threshold. A reasonable default threshold is 0.5
  // (probability), which corresponds to 0.0 log odds.
  constexpr FloatingPoint kOccupancyThresholdProb = 0.5;
  constexpr FloatingPoint kOccupancyThresholdLogOdds = 0.0;

  // NOTE: To tailor the threshold, we recommend running wavemap on a dataset
  //       that is representative of your application and analyzing the Receiver
  //       Operating Characteristic curve.

  // Once a threshold has been chosen, you can either classify in log space
  {
    const bool is_occupied = kOccupancyThresholdLogOdds <= occupancy_log_odds;
    const bool is_free = occupancy_log_odds < kOccupancyThresholdLogOdds;
    examples::doSomething(is_occupied);
    examples::doSomething(is_free);
  }

  // Or in probability space
  {
    const bool is_occupied = kOccupancyThresholdProb <= occupancy_probability;
    const bool is_free = occupancy_probability < kOccupancyThresholdProb;
    examples::doSomething(is_occupied);
    examples::doSomething(is_free);
  }
}

Tutorials

Python API