ethz-asl/wavemap

Fast, efficient and accurate multi-resolution, multi-sensor 3D occupancy mapping

Wavemap

Hierarchical, multi-resolution volumetric mapping

Wavemap achieves state-of-the-art memory and computational efficiency by combining Haar wavelet compression and a coarse-to-fine measurement integration scheme. Advanced measurement models allow it to attain exceptionally high recall rates on challenging obstacles like thin objects.

The framework is very flexible and supports several data structures, measurement integration methods, and sensor models out of the box. The ROS interface can, for example, easily be configured to fuse multiple sensor inputs, such as a LiDAR configured with a range of 20m and several depth cameras up to a resolution of 1cm, into a single map.

⭐ If you find wavemap useful, star it on GitHub to get notified of new releases!

Documentation

The framework's documentation is hosted on GitHub Pages.

Table of contents

• Installation
• Demos
• Configuration
• Usage examples
• Contributing
• Library API
• FAQ

Paper

A technical introduction to the theory behind wavemap is provided in our open-access RSS paper, available here. For a quick overview, watch the accompanying 5-minute presentation here.

Abstract

Volumetric maps are widely used in robotics due to their desirable properties in applications such as path planning, exploration, and manipulation. Constant advances in mapping technologies are needed to keep up with the improvements in sensor technology, generating increasingly vast amounts of precise measurements. Handling this data in a computationally and memory-efficient manner is paramount to representing the environment at the desired scales and resolutions. In this work, we express the desirable properties of a volumetric mapping framework through the lens of multi-resolution analysis. This shows that wavelets are a natural foundation for hierarchical and multi-resolution volumetric mapping. Based on this insight we design an efficient mapping system that uses wavelet decomposition. The efficiency of the system enables the use of uncertainty-aware sensor models, improving the quality of the maps. Experiments on both synthetic and real-world data provide mapping accuracy and runtime performance comparisons with state-of-the-art methods on both RGB-D and 3D LiDAR data. The framework is open-sourced to allow the robotics community at large to explore this approach.

Please cite this paper when using wavemap for research.

APA-style:

Reijgwart, V., Cadena, C., Siegwart, R., & Ott, L. (2023). Efficient volumetric mapping of multi-scale environments using wavelet-based compression. Proceedings of Robotics: Science and Systems XIX. https://doi.org/10.15607/RSS.2023.XIX.065

BibTeX:

@INPROCEEDINGS{reijgwart2023wavemap,
    author = {Reijgwart, Victor and Cadena, Cesar and Siegwart, Roland and Ott, Lionel},
    journal = {Robotics: Science and Systems. Online Proceedings},
    title = {Efficient volumetric mapping of multi-scale environments using wavelet-based compression},
    year = {2023-07},
}

Note that the code has significantly improved since the paper was written. Wavemap is now up to 10x faster, thanks to new multi-threaded measurement integrators, and uses up to 50% less RAM, by virtue of new memory efficient data structures inspired by OpenVDB.

wavemap's Issues

Feature request: Combining wavemaps expressed in different reference frames

Hi @victorreijgwart!

Thank you for this great library, we have been testing it with the output of the lidar mounted on our quadruped robot and we are very happy with the results :)

However, there is a small functionality that we think is missing and would probably be very helpful for other projects too.

Context
Currently, our mapping pipeline has two main components:

• an odometry module that takes in lidar measurements and outputs the estimated pose for each new scan (after point cloud processing and fusion with other sensor modalities), and
• a mapping module that takes in the odometry estimation and refines it with loop closures and absolute pose measurements.

Being (very) conservative, the drift of our odometry output is negligible for travelled distances of 15-20 m. However, loop closures do improve the global map in our typical operations (with distances in the order of hundreds of meters or kilometers).

Our goal is to obtain a consistent global map that can be consumed by downstream navigation applications. We think that the way forward would be to create a set of submaps referenced to the poses in our SLAM graph and to combine them all together at the end of the mapping session using the final optimized SLAM poses.

Requested feature
In order to combine submaps referenced wrt different frames, we would need to

1. project all the submaps to world frame, and
2. combine their information.

This could be something like:

Map combineSubmaps(const MapWithPose& map1, const MapWithPose& map2)
{
  return map1.transformToGlobalFrame() + map2.transformToGlobalFrame();
}

So we would need to implement:

1. A sort of MapWithPose class that has the pose of the map in world frame and a function like transformToGlobalFrame.
2. A + operator that combines maps expressed in the same reference frame.

Please let me know what you think of this approach, especially if there are some steps you would do differently.
I'd be happy to work on it myself but I'm sure that some assistance from your side would save me a good amount of time and pain :)
So if you could provide me with some pointers to relevant classes and a brief outline of how you would do it I'd really appreciate it!

Cheers,
Miguel

The size of Map message is too big

Question
Thank you for your marvelous work.
I'm trying to run wavemap in the small area with min_cell_width : 0.05.
If min_cell_width : 0.1, the size of Map message isn't too big, but it has 10 times bigger size about a few MB with min_cell_width:0.05. And It's too big to send from device to ubuntu machine in real time.
Is there way to reduce the size of Map message?
Thanks.

Runtime information:

• Launch file: [Custom]
• Config file: [hasehd_chunked_wavelet_octree, hashed_chunked_wavelet_integrator, continuous_beam]
• Custom setup: # For online use or personal datasets
  • depth sensor: [TOF]
  • pose source: [Odometry from PX4]

System information:

• CPU: [QRB5165]
• RAM: [8GB]
• OS: [e.g. Ubuntu 18.04]
• Wavemap
  • install: [Native]
  • version: [v1.4.0]

Feature Request: Support for integrating color information

Context
For some applications, it'd be helpful if the map contained color information in addition to occupancy.

High-level changes
Wavemap is quite modular and already supports different map data structures and measurement integrators.
The main additions that are needed are:

• a map data structure that stores a color channel alongside the occupancy
• a measurement integrator that also fuses color information when updating the map
• extensions of the ROS interface (wavemap_server) to forward the color images to the integrator

A few different people mentioned interest in this feature. Let's use this GitHub issue to coordinate our efforts.

failed check in morton_encoding.h in Debug mode in panoptic.

Describe the bug
running the demo panoptic database IN DEBUG MODE (not the default release), as described in the demo section, causes a failed DCHECK_GE

To Reproduce
Run the panoptic demo as described in the installation, but with one difference:
instead of "catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release"
replace Release with Debug.

Expected behaviour
There should be no error logs in Debug mode (similar to Release)

Observed outcome
here is the error log:
_```
F0912 12:48:46.156935 30960 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-2 vs. 0)
*** Check failure stack trace: ***
F0F0F0F0909121121 12:48F:464604.15100:012 012156929 48 146 12: 48:46.156939 30957 309610959 30956n_encoding.h::m50ton_encoding.h50] 50] eck failed: index[dim_idx] >index[dim_idx]
ndex[dim_idx]Chindex[dim_idx] >= 0 (-4 vs. 0)
dex[dim_idx] >= 0 (-2 vs. 0) F0912 12:48:46.156991 30951 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-5 vs. 0) F0912 12:48:46.157212 30953 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-2 vs. 0) F0912 12:48:46.157250 30952 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-4 vs. 0) F0912 12:48:46.157334 F309630 morton_encoding.h 9:12 5012Ch:ck failed: index[dim_idx] >= 0 (-3 vs. 0) 48:46.157335 30949 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-1 vs. 0) F0912 12:48:46.157366 30964 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-3 vs. 0) F0912 12:48:46.157377 30955 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-2 vs. 0) F0912 12:48:46.157466 30950 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-5 vs. 0) F0912 12:48:46.157855 30962 morton_encoding.h:50] Check failed: index[dim_idx] >= 0 (-3 vs. 0)

**System information (please complete if relevant):**
 - CPU:  i9-10980HK
 - RAM: [ 32GB]
 - OS: [ Ubuntu 20.04]
 - Wavemap
   - install: Docker
   - version: [e.g.,  v1.5.1]

Using the map for collision checking/planning

Hello,

I've been playing around with some your code to construct some maps using a Robosense RS-Bpearl and I like the results, thanks for the work.

I wanted to see if i could check for collision based on our robot pose and radius. Reading the documentation and the examples : they show how to check a single cell for occupancy values but how would you recommend to iterate through the map efficiently, and to use multi resolution to optimize the search from coarse to fine ?

Feature Request: Add Native ROS2 Support

Context
With a significant user base transitioning to or already using ROS2, there's a growing demand for native ROS2 interfaces.

Requested features
The integration would require:

1. Native ROS2 version of wavemap's ROS node, the wavemap_server
2. ROS2-Compatible Visualization Plugin: An update or a new plugin for wavemap that works with ROS2's visualization tool, Rviz2
3. Colcon Build System Support

Ongoing discussion
We're currently coordinating this effort through a GitHub discussion and invite interested users and potential contributors to share their insights and suggestions. You can join the conversation here:
GitHub Discussion #41

RViz plugin does not render in topdownortho correctly

Describe the bug
RViz plugin does not render min cell size in topdownortho view.

To Reproduce
Steps to reproduce the behavior:
Set RViz camera view to topdownortho

Expected behavior
The octree should be rendered in min cell size.

Observed outcome
The octree is not rendered in min cell size.

Screenshots
Rendering min cell size:

Not rendering min cell size:

System information (please complete if relevant):

• OS: Ubuntu 18.04
• Wavemap
  • install: native
  • version: v1.5.2

Runtime information (please complete if relevant):

Additional context

Are there any plans to add a function to integrate color information?

Question
Are there any plans to add a feature that integrates the color information from color images into volumetric mapping, similar to the existing feature that integrates depth images into volumetric mapping?

Multiresolution scan integration

Context
Currently, wavemap is updated with a fixed min cell size.

Requested feature
Scan integration with multiple resolutions depending on the distance of the endpoints. The goal would be to use CPU resources for near field integration while saving on long distance updates as these are less informative and generally less dense/accurate.

(Optional) Suggest a solution
Maybe just handle the max tree height for the integrator dynamically?

Additional context

USING without ROS

Hello,
do you have a usage example or file that can guide how to use wavemap directly in c++ without using ROS ?

kind regards,

Feature request: Add traversability information

Hi @victorreijgwart!

Context
Wavemap is a great tool to efficiently store occupation probabilities in 3D. However, some applications (like path planners for ground robots) have the additional need for traversability information. It would be desirable that the traversability information could also be queried at different resolutions for a more efficient path search.

High-level changes
Inspired by your plan to integrate color (#57), I assume the plan would be adding:

• a map data structure that stores traversability alongside the occupancy
• a measurement integrator that also fuses traversability information when updating the map
• extensions of the ROS interface (wavemap_server) to forward the traversability point cloud to the integrator

I would be happy to contribute but I kinda got stuck in the first point (creating a new map data structure) because I can't really see where or how the traversability information would be stored. My (limited) understanding of wavemap is that the occupancy probability of a certain node is given by a linear combination of Haar coefficients from the root node up to the desired termination height.

If we take a volumetric data structure such as HashedWaveletOctree, we see that it stores a std::unordered_map of HashedWaveletOctreeBlock. Each HashedWaveletOctreeBlock has a float scale and a Ndtree of height - 1 where each NdtreeNode is a std::array<float> storing Haar coefficients at each height.

My questions are:

• Would we like (is it even possible?) the traversability information to also be expressed via Haar coefficients? That is, using e.g. HaarCoefficients<CellInfo, 3>, where CellInfo contains two floats (occupancy and traversability).
  • If so, I guess that the Harr functions such as ForwardLifted would change?
  • Otherwise, would HashedWaveletOctreeBlock just have an additional float traversability_; variable? But then I don't see how this info could be expressed in different resolutions.
  • Maybe we would just have another Ndtree that encodes traversability, but then I guess we would need to somehow synchronize both Ndtrees.
• A bit more generic: I understand that each HashedWaveletOctreeBlock has its own Ndtree, is that right? If so, are the Ndtrees somehow connected to each other?

Thank you very much!
Miguel

When scanning column-shaped obstacles, some voxels are not getting marked as occupied.

Describe the bug
Voxels are not getting marked as occupied (never saturate log odds properly?).

To Reproduce
In our setup, we us a velodyne capped to 5m range and min cell size of 0.1. Just get a scan of any small column-shaped object. In our case the robot was steady, but wavemap did never set those voxels to occupied. This issue does not occur with wide obstacles such as walls.

Expected behavior
All endpoints hitting the pillar should be represented in wavemap as occupied voxels.

Observed outcome
See image below

Screenshots

System information (please complete if relevant):

• not relevant

Runtime information (please complete if relevant):

• Custom setup:
  • depth sensor: VLP-16
  • steady robot

Additional context
Add any other context about the problem here.

Missing frame id and stamp in msgs

The latest changes introduced a minor error. When using non-hashed octree the map msg is missing header information.
The code here should be

      if (convert::mapToRosMsg(*occupancy_map_, config_.world_frame,
                              ros::Time::now(), map_msg)) {

Config option to suppress debug output

Context
When running Wavemap we are seeing a lot of output spam

INFO|1695678518.782757590|/wavemap|ros.wavemap_ros: Integrated new pointcloud in 0.00649134s. Total integration time: 12.584s.
INFO|1695678518.826786662|/wavemap|ros.wavemap_ros: Inserting pointcloud with 28800 points. Remaining pointclouds in queue: 0.
INFO|1695678518.833032240|/wavemap|ros.wavemap_ros: Integrated new pointcloud in 0.00619076s. Total integration time: 12.5902s.
INFO|1695678518.976278673|/wavemap|ros.wavemap_ros: Inserting pointcloud with 28800 points. Remaining pointclouds in queue: 0.

Requested feature

Add an option to suppress any debug messages such as those:

wavemap/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc

Line 230 in 7fd41f7

ROS_INFO_STREAM("Inserting pointcloud with "

wavemap/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc

Line 239 in 7fd41f7

ROS_INFO_STREAM("Integrated new pointcloud in "

(Optional) Suggest a solution
Extend the config field by a new bool to enable debug output.

wavemap/ros/wavemap_ros/src/input_handler/pointcloud_input_handler.cc

Line 9 in 7fd41f7

DECLARE_CONFIG_MEMBERS(PointcloudInputHandlerConfig,

Compile error due to tracy

Describe the bug
Seems that the tracy_catkin package does not install headers and libs into a install space as required.

To Reproduce
Steps to reproduce the behavior:

1. Setup catkin workspace in install mode (catkin config --install); may be also reproducable with an extending workspace
2. Install wavemap as described in the tutorial
3. Run catkin build wavemap_all

Expected behavior
No build errors

Observed outcome
Tracy headers are not available.

Screenshots

Errors     << wavemap:make /home/alex/er/logs/wavemap/build.make.229.log
/home/alex/er/src/wavemap/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc:5:10: fatal error: tracy/Tracy.hpp: No such file or directory
    5 | #include <tracy/Tracy.hpp>
      |          ^~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/wavemap.dir/src/data_structure/volumetric/hashed_chunked_wavelet_octree.cc.o] Error 1
make[2]: *** Waiting for unfinished jobs....
/home/alex/er/src/wavemap/libraries/wavemap/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc:3:10: fatal error: tracy/Tracy.hpp: No such file or directory
    3 | #include <tracy/Tracy.hpp>
      |          ^~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/wavemap.dir/src/data_structure/volumetric/hashed_chunked_wavelet_octree_block.cc.o] Error 1
/home/alex/er/src/wavemap/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree.cc:5:10: fatal error: tracy/Tracy.hpp: No such file or directory
    5 | #include <tracy/Tracy.hpp>
      |          ^~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/wavemap.dir/src/data_structure/volumetric/hashed_wavelet_octree.cc.o] Error 1
/home/alex/er/src/wavemap/libraries/wavemap/src/data_structure/volumetric/hashed_wavelet_octree_block.cc:3:10: fatal error: tracy/Tracy.hpp: No such file or directory
    3 | #include <tracy/Tracy.hpp>
      |          ^~~~~~~~~~~~~~~~~
compilation terminated.
make[2]: *** [CMakeFiles/wavemap.dir/src/data_structure/volumetric/hashed_wavelet_octree_block.cc.o] Error 1
make[1]: *** [CMakeFiles/wavemap.dir/all] Error 2
make: *** [all] Error 2
cd /home/alex/er/build/wavemap; catkin build --get-env wavemap | catkin env -si  /usr/bin/make --jobserver-fds=3,4 -j; cd -

System information (please complete if relevant):

• OS: Ubuntu 18.04
• Wavemap
  • install: native build, catkin install space (setup via: catkin config --install)
  • version: v1.5.1
  • cmake: 3.25.2

The livox online config

Thank your great work!
I test your project in lixel mid360. I find you may don't update the config lixel_mind360.yaml in time

`map:
general:
world_frame: "odom"
thresholding_period: { seconds: 2.0 }
pruning_period: { seconds: 10.0 }
visualization_period: { seconds: 2.0 }
data_structure:
type: "hashed_chunked_wavelet_octree"
min_cell_width: { meters: 0.2 }

inputs:

• type: "pointcloud"
  general:
  topic_name: "/livox/lidar"
  topic_type: "livox"
  undistort_motion: true
  topic_queue_length: 100
  max_wait_for_pose: { seconds: 1.0 }
  reprojected_pointcloud_topic_name: "/wavemap/reprojected_pointcloud"

projected_range_image_topic_name: "/wavemap/projected_range_image"

integrators:
  - integration_method:
      type: "hashed_chunked_wavelet_integrator"
      min_range: { meters: 1.0 }
      max_range: { meters: 40.0 }
      termination_update_error: 0.1
    projection_model:
      type: "spherical_projector"
      lidar_origin_to_beam_origin: { millimeters: 27.67 }
      lidar_origin_to_sensor_origin_z_offset: { millimeters: 36.18 }
      elevation:
        num_cells: 64
        min_angle: { degrees: -7.0 }
        max_angle: { degrees: 52.0 }
      azimuth:
        num_cells: 1024
        min_angle: { degrees: -180.0 }
        max_angle: { degrees: 180.0 }
    measurement_model:
      type: "continuous_beam"
      angle_sigma: { degrees: 0.035 }
      # NOTE: For best results, we recommend setting range_sigma to
      #       max(min_cell_width / 2, ouster_noise) where ouster_noise = 0.05
      range_sigma: { meters: 0.05 }
      scaling_free: 0.2
      scaling_occupied: 0.4

`
. And I find you forgot to set sensor_frame_id in yaml.

Feature Request: Local mapping via rolling window approach

Thank you very much for the great effort you put into wavemap! It is a great new tool!

I wonder if there are any plans for local mapping, like a rolling window approach. That would be a nice feature where nodes that are too far away from the moving robot are automatically pruned.

rosbag file missed in the wavemap datasets

In the wavemap datasets, there only exist some map file and odometry file. However, in the newer_college_os0_cloister.launch, it misses other rosbag files, including 2021-12-02-10-15-59_0-cloister.bag or 2021-12-02-10-19-05_1-cloister.bag", where can I download them?