[feat][perf] Allow to extract only implied map voxels
-
Before, the bounding box of the current scan was used to extract the corresponding submap. In some cases, it was leading to too many extracted points and to very high computation time (heavy kdtree build). It was leading to some local minima issues too (see car_long dataset).
-
Add an overload for the function BuildSubMapKdTree to extract the submap. This submap is now the set of map points that are in the same voxels as the keypoints after ego motion step + their neighboring voxels to allow a motion.
-
Here is an image of the new submap and in green the old submap boundary. The number of points is reduced of about 30% in this case.
-
Results of the SLAM process (no VTK) on 4 threads :
| data | time before (ms) | time now (ms) | visual inspection |
|---|---|---|---|
| car_loop | 26.9 | 28.7 | OK |
| car_long | 22.9 | 24 | fixed |
| housing estate | 29.1 | 32.4 | OK |
| warehouse | 34 | 35.3 | OK |
| edited | 37.3 | 36.5 | OK |
| not_aligned | 52.8 | 46.9 | OK |
-
At the end, it mostly gives worse computation times as the previous version. It is efficient in a dense map not aligned with the voxel grid. There is a trade-off between the voxels extraction and the kdtree operations. Adding time to extract the voxels reduces the time of the kdtree building and for the search.
-
Note that it can solve some local minima issues too as it removes far points.
-
It is also globally more memory efficient.
-
Here is a pipeline example on LidarView