Usage examples

Serialization

Files

Saving maps to files:

#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
  wavemap::io::mapToFile(map, "/some/path/to/your/map.wvmp");
}

Loading maps from files:

#include <wavemap_io/file_conversions.h>

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

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

ROS msgs

Receiving maps over ROS topics:

#include <ros/ros.h>
#include <wavemap_ros_conversions/map_msg_conversions.h>

// Create a smart pointer that will own the received map
wavemap::VolumetricDataStructureBase::Ptr loaded_map;

// Define the map callback
void mapCallback(const wavemap_msgs::Map& msg) {
  // Load the received map
  wavemap::convert::rosMsgToMap(msg, loaded_map);
}

// NOTE: We only define loaded_map and mapCallback globally for illustration
//       purposes. In practice, we recommend defining them as class members.

int main(int argc, char** argv) {
  // Register your node with ROS
  ros::init(argc, argv, "your_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  // Subscribe to the ROS topic
  ros::Subscriber map_sub = nh.subscribe("map", 1, &mapCallback);
}

Sending maps over ROS topics:

#include <ros/ros.h>
#include <wavemap_ros_conversions/map_msg_conversions.h>

int main(int argc, char** argv) {
  // Register your node with ROS
  ros::init(argc, argv, "your_node");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  // Create an empty map for illustration
  wavemap::HashedWaveletOctreeConfig config;
  wavemap::HashedWaveletOctree map(config);

  // Advertise the ROS topic
  const std::string ros_topic = "map";
  const int queue_size = 1;
  ros::Publisher map_pub =
      nh_private.advertise<wavemap_msgs::Map>(ros_topic, queue_size);

  // Convert the map to a ROS msg
  wavemap_msgs::Map map_msg;
  const std::string map_frame = "odom";
  const ros::Time stamp = ros::Time::now();
  wavemap::convert::mapToRosMsg(map, map_frame, stamp, map_msg);

  // Publish the ROS message
  map_pub.publish(map_msg);
}

Queries

Fixed resolution

#include <wavemap/data_structure/volumetric/volumetric_data_structure_base.h>

#include "wavemap_examples/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.
  VolumetricDataStructureBase::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

#include <wavemap/data_structure/volumetric/hashed_wavelet_octree.h>
#include <wavemap/indexing/index_conversions.h>

#include "wavemap_examples/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

#include <wavemap/data_structure/volumetric/hashed_wavelet_octree.h>
#include <wavemap/utils/iterate/grid_iterator.h>
#include <wavemap/utils/query/query_accelerator.h>

#include "wavemap_examples/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);
  }
}

Interpolation

Nearest neighbor interpolation:

#include <wavemap/data_structure/volumetric/volumetric_data_structure_base.h>
#include <wavemap/utils/query/map_interpolator.h>

#include "wavemap_examples/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.
  VolumetricDataStructureBase::Ptr map;

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

  // Compute the index that's nearest to the query point
  const FloatingPoint min_cell_width_inv = 1.f / map->getMinCellWidth();
  const Index3D nearest_neighbor_index =
      convert::pointToNearestIndex(query_point, min_cell_width_inv);

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

Trilinear interpolation:

#include <wavemap/data_structure/volumetric/volumetric_data_structure_base.h>
#include <wavemap/utils/query/map_interpolator.h>

#include "wavemap_examples/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.
  VolumetricDataStructureBase::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);
}

Classification

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

#include "wavemap_examples/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);
  }
}