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);
}
}