feat: voxel downsampling mapper module

[nicolaslauzon]

What

I got inspired by kiss-icp to use voxel hashing as a way to downsample point clouds. They use it as storage for their local map.

I tried the robin_map and the std::unordered_map and their performance seems to be similar.

Everything was tested on the demo bag. It would need to be tested on a bigger run.

Pros

• It uses insertion to keep constant times no matter the map size.
• It is faster than the point distance module on everything except small maps.

Cons

• Can't be used with any other mapper modules that modify the map.
• It can be unstable since when the hashMap resizes, the map update takes significantly longer to run. This is observed in the spikes of the timing graph.

Results

Qualitative result of a subsampled map over a dense map

Execution time on a dense map

Execution time on a sparse map in comparaison of a dense map.

[boxanm]

Good job, @nicolaslauzon !
I think there are definitely use cases for this kind of mapping module, especially when we know that there are no dynamic points to be removed, which is actually quite a common scenario. I have a few general remarks and then some to-dos before we merge to develop.

1. Can the main loop of VoxelDownsamplingMapperModule be parallelized?
2. Can we make a guess about the total map size and reserve the needed space in advance to eliminate the sudden jumps in processing time? I suppose some value could be determined from the sensor's max range, the voxel size, and number of points per voxel.

There are a few tasks left:

1. Update the documentation
2. Add support in Python bindings

Also, please change the target branch of this PR from master to develop.

[nicolaslauzon]

Good job, @nicolaslauzon ! I think there are definitely use cases for this kind of mapping module, especially when we know that there are no dynamic points to be removed, which is actually quite a common scenario. I have a few general remarks and then some to-dos before we merge to develop.

1. Can the main loop of VoxelDownsamplingMapperModule be parallelized?

2. Can we make a guess about the total map size and reserve the needed space in advance to eliminate the sudden jumps in processing time? I suppose some value could be determined from the sensor's max range, the voxel size, and number of points per voxel.

There are a few tasks left:

1. Update the documentation

2. Add support in Python bindings

Also, please change the target branch of this PR from master to develop.

1. With a concurrent safe data structure, it would be possible. (Ex)
   I will test if it speeds it up.
2. Good idea! We could make a guess and reserve the space in advance for the whole map. However, if it continues growing over the size of our first guess, I think we should let it rehash on it's own. Here are a few growth policies quickly explained : https://github.com/Tessil/robin-map?tab=readme-ov-file#growth-policy

I will also take care of the doc dans python bindings.