IROS 2024: DEPTRUM's 3D SLAM Research Accepted at the Top Robotics Conference

From October 14 to 18, 2024, the International Conference on Intelligent Robots and Systems (IROS 2024) was held in Abu Dhabi, UAE. As one of the world's largest and most influential robotics research conferences, IROS attracts researchers, scholars and industry experts from around the world each year to discuss the latest advances in robotics and intelligent systems.

At this conference, DEPTRUM's research — C3P-VoxelMap: Compact, Cumulative and Coalescible Probabilistic Voxel Mapping — was accepted for its innovativeness and presented as an Oral Pitch. The research proposes a solution for real-time 3D SLAM under the limited computing power of embedded platforms, providing a reference design for autonomous navigation of mobile robots.

C3P-VoxelMap video demo

Research background

3D SLAM, as one of the core technologies of robot autonomous navigation, faces the important theoretical and practical question of how to achieve high-precision localization and mapping under limited computing and storage resources.

VoxelMap is a map structure widely used in 3D SLAM: it divides 3D space into voxels of identical structure and indexes them with hash tables, with the advantages of simple maintenance and efficient memory access.

However, conventional VoxelMaps have limitations in practice: they struggle to capture large-scale geometric features of the real world — such as walls, floors and ceilings — so the same physical plane requires many voxels for repeated representation. On the other hand, fixed-size voxels face the bias-variance trade-off. As shown below, small voxels capture map detail better (low bias) but are more susceptible to noise, especially with sparse point clouds (high variance); conversely, large voxels collect more observations to reduce variance (low variance) but may over-smooth the environment, causing representation bias (high bias).

In addition, a Probabilistic VoxelMap must store the historical point cloud of every voxel and re-iterate all points when new observations arrive, to update the uncertainty of the voxel plane. This is because computing the plane's covariance matrix requires the Jacobian matrix of the normal vector with respect to each point. Here, the point p and its covariance are observation-related terms (below, observation terms), while q, F and U are terms related to the overall distribution of the plane point set (distribution terms: q is the point-set mean; F and U are matrices formed from the SVD result — see the paper for details). When a new observation arrives, since the distribution terms in the Jacobian change, every point must be substituted into the formula again for re-differentiation.

This iteration consumes a lot of memory and CPU. On resource-constrained robot platforms it may cause localization latency or insufficient memory to maintain the map, affecting downstream tasks such as obstacle avoidance and path planning.

To solve these pain points, DEPTRUM innovatively proposed C3P-VoxelMap, improving VoxelMap in three aspects: computational efficiency, memory efficiency and algorithmic accuracy.

Method

Incremental update of plane covariance

To avoid repeated differentiation, we want to decouple the observation terms and the distribution terms so that both can be updated incrementally.

Separating the U matrix using the distributive law of matrix computation is relatively easy; the difficulty is separating q and F from the B matrix. Since the B matrix as a whole is not easy to decompose, we examine it element by element based on its expression.

It follows that the element at row m, column n of the B matrix can be decomposed into four summation terms of similar form. Taking one of them as an example:

We use the following two lemmas to separate the observation terms:

The first property helps us extract matrix elements; the second completes the exchange of matrix positions.

Using these two properties, we successfully obtain the accumulated form X containing only observation terms. The derivations of the other terms are similar — see our paper and supplementary materials. When new observation points arrive, we only need to map the points and covariances into the accumulated form above to complete the incremental update. This compact representation avoids re-differentiation for every point, greatly reducing computation and memory usage.

Voxel plane merging

To further solve the bias-variance trade-off, we introduce a voxel merging strategy that dynamically adapts to large planar features in the real world. The difficulty is ensuring merging efficiency — brute-force search would compromise real-time performance.

To this end, we designed a merging method based on Locality Sensitive Hashing. Voxels with similar plane parameters that are physically close are more likely to compute the same hash value, and are pushed into the same bucket to await merging. When a bucket reaches a certain count, merging is triggered: the voxel with the most points is chosen as the reference voxel as the comparison baseline for merging the others, because voxels with more points usually have more accurate plane estimates. If after attempting to merge, the singular values of the point-set distribution still satisfy the plane condition, the voxel is allowed to merge; otherwise it is rejected.

Thanks to the compact form of the plane covariance matrix, we can update merged planes incrementally. Single-voxel and cross-voxel uncertainty updates share a unified incremental form, so the merged plane still has a complete uncertainty representation. Through merging, one large plane feature needs only one or a few voxels, further reducing memory usage. Moreover, at the same noise level, voxel merging estimates planes more accurately, improving SLAM precision (see the schematic below; the theoretical proof is in Section V-C of the paper).

Experimental results

To verify the effectiveness of C3P-VoxelMap, we conducted extensive tests on public datasets.

On the KITTI dataset, C3P-VoxelMap achieved a 70% performance improvement in the map update stage compared with non-incremental update methods.

Furthermore, since each voxel's memory consumption is independent of the point count — i.e., constant complexity — C3P-VoxelMap has higher memory efficiency.

In UTBM dataset tests, C3P-VoxelMap achieved a 20% accuracy improvement over SOTA methods.

This work enables real-time, high-precision 3D SLAM mapping and localization over 500+ m2 on compute-constrained platforms (e.g., Raspberry Pi 4B) using a 3D ToF depth camera, helping robot vacuums and other mobile robots achieve high-precision 3D navigation and obstacle avoidance on embedded platforms.