Feature request: Add traversability information

[miguelcastillon]

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

[victorreijgwart]

Hi Miguel,

Thank you for this question! This feature would definitely be great to have.

As you suggested, the steps would, in general, be very similar to integrating color information.

I have a few questions to make sure I understand your setup correctly.

• Do you perform the traversability classification in image space (e.g. pixel-wise predictions based on RGB-D images)?
• How do you usually filter the traversability estimates? Assuming you store the posterior per-voxel, do you apply the measurement updates using log odds? Or using a weighted average?

Regarding your questions:

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.

This is correct. The structure of the map and the information it stores is identical to Octomap. The key difference is that while octomap stores the occupancy probability of each node as an absolute value, we store the occupancy probabilities using differences. The main advantage of doing this is that when you update the map at a lower resolution, all the affected high-resolution cells are updated implicitly. So when you update the map at a resolution of 20cm (e.g. for LiDAR), in an area with a maximum resolution of 1cm (e.g. from RGB-D), the probabilities of all the low and high-res cells are updated correctly even though you only pay for a 20cm update.

Although you could simply store the difference between a node and each of its 8 children using 8 floats, some of this information is redundant. Specifically, the average value of the 8 children is identical to the value of the parent. Haar wavelets, therefore, only use 7 difference values. In wavelet terminology, these differences are called detail coefficients and a node's own value is called its scale coefficient. For the Haar transform, a node's scale coefficient directly corresponds to the average value over the volume covered by the node. In code, the InverseHaar transform takes 1 scale coefficient (the value of parent) + 7 detail coefficients (differences) and returns the values of the 8 children.

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.

Yes, that's right :)

Would we like (is it even possible?) the traversability information to also be expressed via Haar coefficients?

Yes, this would be possible. If you query traversability information at a lower resolution, would you like the map to return the average traversability over the subvolume covered by the node? Or would you rather have the minimum/maximum traversability over the given subvolume? The Haar transform is only suitable if you want the lower resolutions to capture averages. In case you'd rather have the lower resolutions perform min/max reductions or another non-linear operation, it'd be better to store the values of interest directly as absolute values (at the loss of implicit multi-res updates).

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?

They're completely independent. Each octree has a few private variables (e.g. the max tree height). These get passed in through the constructor whenever a new block is created. Let me know if this answers your question.

I can't really see where or how the traversability information would be stored.

In the code, the type of the data stored in each octree node is specified here. You can change it by supplying another type as the Ndtree's first template argument.

Best wishes,
Victor

[miguelcastillon]

Hi Victor,

Answering your questions:

Do you perform the traversability classification in image space (e.g. pixel-wise predictions based on RGB-D images)?

Our current classificator works with the 3D point cloud, but that may change in the near future.

How do you usually filter the traversability estimates? Assuming you store the posterior per-voxel, do you apply the measurement updates using log odds? Or using a weighted average?

This may also change, but we currently treat traversability similar to occupancy (log odds and threshold for classification).

If you query traversability information at a lower resolution, would you like the map to return the average traversability over the subvolume covered by the node?

Yes, I think this is quite reasonable, and --if I have understood it correctly-- that way we would be using full potential of the Haar transforms.

Thank you very much for the clarifications, I appreciate them. Unfortunately, I think I overestimated my current availability to work on this in the coming weeks, but it is definitely a feature of great interest and I'll try to come back to it when I have some more bandwidth.

Best regards,
Miguel