Introduce Bonxai as Map representation #22

tizianoGuadagnino · 2024-11-29T15:29:27Z

Motivation

It is finally time to introduce a proper data structure for representing 3D space instead of a simple hash table. By having a more appropriate and flexible data structure, we could extend the system further, for example, by using some TSDF registration or similar. Of course, this should come at a low cost in terms of performance. Our VoxelHashMap has done wonders for us in KISS, but I intend to move on.

Why Bonxai?

The project is well maintained, does not add additional dependencies, and is header-only. In terms of the build system, it is exactly the same as fossil. The project is already integrated with the ROS ecosystem, so we are also covered on that side. That is a nice cherry on top. When an RViz visualization of the grid is introduced, Kinematic-ICP will immediately benefit.

Why not OpenVDB

I don't want to deal with 20 dependencies and an ugly Cmake script that does madness with FetchContent, Bonxai is much smaller and does precisely what we need it for.

This PR

I keep the same API as the VoxelHashMap but change the type name and the internal map representation. Everything else will be the same for the user.

Please give me feedback on this @benemer @nachovizzo

find a way to remove unused voxels

nachovizzo

I don't mind switching to Bonxai. But did you run some benchmarks? I would expect that it uses much more memory than a simple hash table, as each point would be allocated in a leaf node of 512 voxels, no matter how isolated is this point, which doesn't happen for pain hash tables.

That said I also think it might be slightly slower in terms of runtime. I would probably give it a try with kiss icp, just to try on more datasets, and check the impact on the registration system

cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch

nachovizzo · 2024-11-29T16:17:44Z

And another comment on

the project is already integrated with the ROS ecosystem,

It is as much integrated as Tessil and OpenVDB are 🤣 Actually, OpenVDB is more integrated to ROS due to Steve's https://github.com/SteveMacenski/spatio_temporal_voxel_layer, where we went through big pain making sure OpenVDB is it available as an installable package: https://index.ros.org/p/openvdb_vendor/github-SteveMacenski-spatio_temporal_voxel_layer/#jazzy

tizianoGuadagnino · 2024-11-29T16:42:29Z

I don't mind switching to Bonxai. But did you run some benchmarks? I would expect that it uses much more memory than a simple hash table, as each point would be allocated in a leaf node of 512 voxels, no matter how isolated is this point, which doesn't happen for pain hash tables.

That said I also think it might be slightly slower in terms of runtime. I would probably give it a try with kiss icp, just to try on more datasets, and check the impact on the registration system

We can port it to KISS to test a broader range of datasets. At least on the "Kinematic" sequences, I didn't notice a significant difference in runtime or memory consumption, but as you said, the sample size of our datasets with Kinematic is kind of limited (just 2 sensor configurations).

Remember, although we allocate N voxels in the leaf node, each voxel contains a dynamic data structure (a.k.a. std::vector, so the memory overhead shouldn't be significant.

PS: In terms of "accuracy" it is literally the same number up to the 4th digit, but I guess this doesn't surprise nobody

tizianoGuadagnino · 2024-11-29T16:44:21Z

And another comment on

the project is already integrated with the ROS ecosystem,

It is as much integrated as Tessil and OpenVDB are 🤣 Actually, OpenVDB is more integrated to ROS due to Steve's https://github.com/SteveMacenski/spatio_temporal_voxel_layer, where we went through big pain making sure OpenVDB is it available as an installable package: https://index.ros.org/p/openvdb_vendor/github-SteveMacenski-spatio_temporal_voxel_layer/#jazzy

This was an additional motivation for me. The reason I chose Bonxai is not the ROS integration; this is more of a cherry on top. For me, it needs to be easily installable for the C++ API and ROS so that we can keep these two aspects separate.

facontidavide · 2024-11-30T19:35:31Z

Thanks.

Please run benchmarks.

tizianoGuadagnino · 2024-12-02T09:23:52Z

Thanks.

Please run benchmarks.

Here there is. For this benchmark, I generate a point cloud out of the surface of a cube with a side of 100m. I randomly sampled 10.000 points for each face, so the total points for the point cloud were 60.000. I then simulate a LiDAR 16x1024 moving inside this "box" and benchmark both the GetClosestPoint, AddPoints and Update functions. The code for the benchmark is in the branch benchmark if you are curious (which I will probably merge to master after this).

GetClosestPoint benchmark: I use the full cube point cloud as a map for both Bonxai and VoxelHash and query neighbors for a downsampled version of the scan (the same preprocessing as the original pipeline).

AddPoints: Add the all cube points to the map.

Update benchmark: The map is initialized with all cube points. I simulate a circular trajectory inside the cube, and for each location, I add the points from the downsampled LiDAR and remove the points that are too far away.

Note: We could probably further improve the performance of the Update function by checking a single voxel per leaf grid instead of all the cells.

I will be working on the memory analysis next ;)

tizianoGuadagnino · 2024-12-02T13:38:54Z

Note: We could probably further improve the performance of the Update function by checking a single voxel per leaf grid instead of all the cells.

But here I am also removing the dangling keys from the root map once all the leafs from an inner_grid are empty.

tizianoGuadagnino · 2024-12-03T13:24:59Z

Memory Analysis

For this experiment, I follow the God commandment and use heaptrack to perform the measurement. I report two different leaf/inner grid sizes, default (inner_grid_log2_dim=2, leaf_grid_log2_dim=3) and small leaf (inner_grid_log2_dim=2, leaf_grid_log2_dim=2).

Sampling 6.000.000 points from the surface of a cube

VoxelHashMap

BonxaiGrid (default)

BonxaiGrid (small leaf)

Sampling 60.000 points from the surface of a cube

VoxelHashMap

BonxaiGrid (default)

BonxaiGrid (small leaf)

Comments

While using Bonxai increases memory consumption, this overhead seems proportional to the sparsity of the data. This is clearly due to the dense allocation of the leafs as foretold by @nachovizzo. This makes a difference in scenarios with some "random point measurements" due to reflections, occlusions, or sparse structures. In structured environments, the difference in memory consumption might be acceptable, considering that the system halved the time of the nearest neighbor search, which has been one of the bottlenecks for a long time, especially single-threaded.

Give me your thoughts guys ;)

tizianoGuadagnino · 2024-12-03T13:26:38Z

For completeness, I also report the results of the benchmark for the BonxaiGrid (small leaf)

tizianoGuadagnino · 2024-12-03T16:52:52Z

Running the online node on some real data between main and this PR results in the following:

This is a comparison on Heaptrack, it seems VoxelHashMap::GetClosestNeighbor if generating ~18 more allocations per second. I upload here the data so that you guys can have a look.

heaptrack_data.zip

benemer · 2024-12-13T14:14:35Z

To summarize the analysis: At least with the simulated points from the tests, Bonxai is faster for NN query and removing points, whereas the voxel hash map adds points faster. Hard to say how this affects the total runtime because it depends on the actual data rather than the simulated one. I agree we should port this to KISS and run more tests on actual data besides these benchmarks.

Regarding memory consumption, I don't know how severe this increase is. Also, it's again not easy to draw conclusions from simulated data because now we sample from a cube, resulting in more dense surfaces compared to a real-world environment.

But in general, I also like the results so far, and the time we save on the NN search is the most critical.

benemer · 2024-12-13T14:30:51Z

cpp/kinematic_icp/3rdparty/bonxai/bonxai.cmake

+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+# Silence timestamp warning


Is this needed? I think it's coming from the find_dependencies.cmake from KISS, we could move it to the top-level .cmake

benemer · 2024-12-13T14:37:46Z

cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp

+#include "bonxai/grid_coord.hpp"
+
+namespace {
+static constexpr std::array<Bonxai::CoordT, 27> shifts{


To be discussed after resolving PRBonn/kiss-icp#410

benemer · 2024-12-13T15:02:13Z

cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp

+            })) {
+            return;
+        }
+        voxel_points->reserve(max_points_per_voxel_);


There will be duplicated calls to reserve, but since max_points_per_voxel_ is fixed, this should not matter, right?

benemer · 2024-12-13T15:10:08Z

cpp/kinematic_icp/local_map/SparseVoxelGrid.cpp

+std::vector<Eigen::Vector3d> SparseVoxelGrid::Pointcloud() const {
+    std::vector<Eigen::Vector3d> point_cloud;
+    point_cloud.reserve(map_.activeCellsCount() * max_points_per_voxel_);
+    map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) {


Suggested change

map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) {

map_.forEachCell([&point_cloud, this](const VoxelBlock &block, const auto &) {

tizianoGuadagnino added 12 commits November 22, 2024 11:10

Add Bonxai and move kiss icp cmake

7a19274

Start to work on the local map representation

0084554

Add old API

4adc4e0

At least replicate the functionalities of kiss map, need to test and

01188b4

find a way to remove unused voxels

Just build bonxai_core

be18fae

Add patch

11b9c5a

To fix const correctness

a7ddd20

Remove small vector

7e9726f

Merge branch 'main' into tiziano/bonxai_minimal

6ed4c01

Some renaming and cleaning

c4c1d90

For more clarity

2af1115

Modify bonxai patch

b76abda

tizianoGuadagnino requested review from nachovizzo and benemer as code owners November 29, 2024 15:29

nachovizzo reviewed Nov 29, 2024

View reviewed changes

cpp/kinematic_icp/3rdparty/bonxai/bonxai.patch Show resolved Hide resolved

IWYU

f456e76

Just reset the block

bbb3ecc

Actually remove the keys when leafs are removed, a bit slower

57977f1

Control leaf and grid size

45c1b2b

Forget to push the default config

9859082

tizianoGuadagnino mentioned this pull request Dec 4, 2024

Reduce memory allocation PRBonn/kiss-icp#410

Open

benemer reviewed Dec 13, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Introduce Bonxai as Map representation #22

Introduce Bonxai as Map representation #22

tizianoGuadagnino commented Nov 29, 2024 •

edited

Loading

nachovizzo left a comment

nachovizzo commented Nov 29, 2024

tizianoGuadagnino commented Nov 29, 2024

tizianoGuadagnino commented Nov 29, 2024 •

edited

Loading

facontidavide commented Nov 30, 2024

tizianoGuadagnino commented Dec 2, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 2, 2024

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading

benemer commented Dec 13, 2024

benemer Dec 13, 2024

benemer Dec 13, 2024

benemer Dec 13, 2024

benemer Dec 13, 2024

	map_.forEachCell([&point_cloud, this](VoxelBlock &block, const auto &) {
	map_.forEachCell([&point_cloud, this](const VoxelBlock &block, const auto &) {

Introduce Bonxai as Map representation #22

Are you sure you want to change the base?

Introduce Bonxai as Map representation #22

Conversation

tizianoGuadagnino commented Nov 29, 2024 • edited Loading

Motivation

Why Bonxai?

Why not OpenVDB

This PR

nachovizzo left a comment

Choose a reason for hiding this comment

nachovizzo commented Nov 29, 2024

tizianoGuadagnino commented Nov 29, 2024

tizianoGuadagnino commented Nov 29, 2024 • edited Loading

facontidavide commented Nov 30, 2024

tizianoGuadagnino commented Dec 2, 2024 • edited Loading

tizianoGuadagnino commented Dec 2, 2024

tizianoGuadagnino commented Dec 3, 2024 • edited Loading

Memory Analysis

Sampling 6.000.000 points from the surface of a cube

VoxelHashMap

BonxaiGrid (default)

BonxaiGrid (small leaf)

Sampling 60.000 points from the surface of a cube

VoxelHashMap

BonxaiGrid (default)

BonxaiGrid (small leaf)

Comments

tizianoGuadagnino commented Dec 3, 2024 • edited Loading

tizianoGuadagnino commented Dec 3, 2024 • edited Loading

benemer commented Dec 13, 2024

benemer Dec 13, 2024

Choose a reason for hiding this comment

benemer Dec 13, 2024

Choose a reason for hiding this comment

benemer Dec 13, 2024

Choose a reason for hiding this comment

benemer Dec 13, 2024

Choose a reason for hiding this comment

tizianoGuadagnino commented Nov 29, 2024 •

edited

Loading

tizianoGuadagnino commented Nov 29, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 2, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading

tizianoGuadagnino commented Dec 3, 2024 •

edited

Loading