Feature Request: Local mapping via rolling window approach

[astumpf]

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.

[victorreijgwart]

Thank you! I'm glad to hear it's useful.
So far we didn't have plans for a feature like this, but we could add it. I have a few questions to make sure I understand your use case :)

• It would be the most straightforward to add this feature only for the hashed-octree data structures, where we could simply drop blocks when their distance to the robot exceeds a threshold. Would using hashed-octrees be an option for you, or do you need the map as a single octree for your application? The documentation is still behind, but the hashed-octrees generally have better overall performance (less memory usage, faster queries, measurement integration with multi-threading, incremental transmission, faster rendering in Rviz,...). They also let the robot work in a near-infinite workspace, whereas the regular octree's workspace size is limited by the height of the tree (we haven't implemented dynamic height adjustments or rebalancing).
• What is your main motivation to drop distant blocks?
  • Is it to model some kind of uncertainty? If so, do you want to drop nodes/blocks based on a hard distance/time threshold, or would exponential forgetting be better?
  • Or is it to reduce memory usage, speed up iterating over the whole map, or speed up the visualizations?

[astumpf]

Hello and sorry for the late reply. Yes, we have already found that hashed-octree works much better and will very likely use it in the future.

Since my requested feature could obviously be easily implemented for hashed-octree, I already thought of doing it myself. But then I decided to coordinate with you on what your plans are. I assume that you can implement it even faster and cleaner in your framework :-).

What is your main motivation to drop distant blocks?

To answer your questions, yes and yes. We are concerned about the memory consumption due to the small compute units used. Also, there may still be localization drift, so revisiting the same location multiple times would mess up the map without closed loop optimization. So in our case we are more interested in hard distance thresholding, but I can also see interesting use cases for time thresholding (aging out old nodes).

[victorreijgwart]

Hi Alexander,
Great that the hashed-octrees would also work for you. For these data structures, it'd be very easy to drop distant blocks and wavemap already contains some utilities that might be useful. I'll create a branch and link it here. Then we can work on it together.
Out of curiosity, what kind of compute units are you using (unless it's confidential of course)?

[victorreijgwart]

The draft is now available on the feature/local_mapping branch.

I tested it with these general settings:

map:
  general:
    world_frame: "odom"
    body_frame: "body"  # New: Set to a frame that moves with the robot
    thresholding_period: { seconds: 2.0 }
    pruning_period: { seconds: 2.0 }  # Controls how often the pruning incl. block dropping happens
    publication_period: { seconds: 2.0 }
  data_structure:
    type: hashed_chunked_wavelet_octree
    min_cell_width: { meters: 0.1 }
    remove_blocks_beyond_distance: { meters: 25.0 }  # New: Distance beyond which to drop blocks

Let me know how it works for you, and feel free to modify the code :)

[astumpf]

Awesome! Thank you so much for your efforts! I'm currently very busy but definitely will take a look into that asap.

Out of curiosity, what kind of compute units are you using (unless it's confidential of course)?

We are using currently Jetson Xavier as compute units.

[victorreijgwart]

Welcome! No rush :)

We are using currently Jetson Xavier as compute units.

Oh nice, we're using Xavier's on our drones as well.

[victorreijgwart]

Hi Alexander, we're considering merging the local mapping feature into a release sometime in the next two weeks. Did you have a chance to try it? Are there any options that you'd like to use but are still missing?

[astumpf]

My plan is to test this out next week using real robots :-).

[astumpf]

Hi Victor,

Finally, I've tested this branch. Looks great so far! I have already some feedback for you:

1. On which level is the pruning done? It looks like this is done only on a very high level (chunk level?).
2. remove_blocks_beyond_distance should also be checked during integration. Although it may be contradictory, but if integrator_max_range < remove_blocks_beyond_distance, you will see flickering caused by adding and immediately removing nodes.

Thanks for your great work!

[victorreijgwart]

Hi Alexander,
Great that you got to test it!

1. The pruning is currently done at the (hash) block level. The reason for this is that to set an entire block to zero, it can simply be deleted. It's also possible to prune at a higher resolution, and this can be done efficiently by exploiting the Haar wavelet's properties, but it'd take a custom algorithm. Unfortunately, I won't have much time in the coming weeks. Are smooth borders for the sliding window important to you? If they are and you have some time to work on it, we could discuss how to do it.

2. In which situations would the max range need to be larger than the sliding window's radius? Both radii have a similar purpose, and the flickering can be avoided by adjusting either of them s.t. integrator_max_range < remove_blocks_beyond_distance. It's probably cleaner to just print a warning when wavemap starts up and integrator_max_range > remove_blocks_beyond_distance, explaining that it'll cause flickering and how to avoid this.

[astumpf]

1. Thanks for the clarification! As long as the hash block size is reasonably small, there is currently no need to prune at higher resolution. How is the size of the hash block currently determined? Can it be controlled from the outside?
2. As mentioned above, this would be inconsistent and I don't think this is a use case. I made this mistake in my initial testing and was confused at first. A warning is desirable.

[astumpf]

Cannot await to see that feature merged soon! :-)

[victorreijgwart]

1. The block width can be computed from the tree height and the min cell width (max resolution) as:

block_width = min_cell_width * 2^(tree_height)

We don't have a method to introspect this yet. One option would be to add a member method to the data structures along the lines of getBlockWidth() in a similar fashion to the getMinCellWidth() method that already exists. Alternatively, you could calculate the block width based on the min_cell_width and tree_height fields in the ROS messages:

rostopic echo "/wavemap/map/hashed_wavelet_octree[0]" --noarr

3. Ok, we'll add a warning.

Cannot await to see that feature merged soon! :-)

We're planning to review and merge this feature once PR#37 is ready :)

[victorreijgwart]

@astumpf, I just wanted to update you on our timeline. We're planning to refactor the wavemap_server, to make the additional operations it can perform more modular and extendable. This new structure will also provide a cleaner way to incorporate the code and configuration options for the sliding window functionality, so we plan to release the sliding window feature and refactored server together - probably around mid-November.

[astumpf]

I am very excited, looking forward to it!

[victorreijgwart]

The new wavemap_server structure on the feature/plugin_system branch now supports local mapping through a map cropping plugin. It can be enabled and configured by adding an entry of type: crop_map to the operations.

Taking the default Ouster OS0 config for illustration, you could replace the operations array with:

operations:
  - type: threshold_map
    once_every: { seconds: 2.0 }
  - type: prune_map
    once_every: { seconds: 10.0 }
  - type: crop_map
    once_every: { seconds: 2.0 }
    body_frame: "body"
    remove_blocks_beyond_distance: { meters: 25.0 }
  - type: publish_map
    once_every: { seconds: 2.0 }

The main advantage we see is that it groups all the relevant settings in one place and improves the internal structure of the wavemap server. It also makes it easy to add additional plugins in the future, including custom plugins by users. Would this new structure work well for you?

[astumpf]

Definitely yes. I like the new structure. Does that mean we can also add new plugins ourselves?

[victorreijgwart]

Yes, that would be the goal. The idea so far is that users can create classes derived from InputBase or OperationBase, and then register them with the wavemap server by calling something like wavemap_server.addInput(std::move(your_custom_input)) or wavemap_server.addOperation(std::move(your_custom_operation)). This part is not yet implemented though :)