RootSLAM: Square Root Bundle Adjustment for Visual-Inertial SLAM
Authors: Tomás Santucci, Mateo de Mayo, Daniel Cremers, Taihú Pire
Organizations: National University of Rosario, Rosario, Argentina · Technical University of Munich and the Munich Center for Machine Learning, Munich, Germany · CIFASIS (CONICET-UNR), Rosario, Argentina
Efficient perception is a prerequisite for the small, low-power devices targeted by robotics, mixed reality, autonomous driving, and drone applications, all of which rely on Simultaneous Localization and Mapping (SLAM) to estimate their motion within their surroundings. Learning-based SLAM systems achieve state-of-the-art accuracy but are demanding in compute, whereas classical geometry-based methods retain a strong efficiency advantage with little to no loss in accuracy. In this work we present RootSLAM, a fully-featured visual-inertial SLAM system for multi-camera setups built on Basalt. RootSLAM achieves accuracy competitive with the state of the art while running up to 20 times faster, facilitating the deployment of perception systems in resource constrained environments. We adapt the square-root bundle adjustment formulation from structure from motion into the multi-camera visual-inertial SLAM setting through a novel reformulation that leverages the system's pose-graph, letting the global optimizer run in single-precision floating point, thus doubling its performance. We provide a thorough benchmark against the state of the art together with an ablation study of our contributions. Our new implementation, including its map management and loop detection module, pose-graph optimizer, and global mapping components, will be open-sourced.
Figures & tables
Fig. 1: RootSLAM running on the long sequence MOO12. Top: the resulting map, with the estimated trajectory in gray and the landmarks in blue. Bottom: moving average of the per-frame processing time throughout the sequence, which stays under the real-time budget of 33 ms for 99.99% of the frames, averaging 7.2 ms with a standard deviation of 1.6 ms. Run on an Intel Core Ultra 9 285H.
Fig. 2: Architecture of RootSLAM. We introduce the map database, loop closing and global bundle adjustment components and add them on top of Basalt’s front-end and local optimizer.
Fig. 3: Loop closures found by RootSLAM on (a) TUM-VI magistrale 1 and (b) corridor 1. The trajectory is drawn with time on the vertical axis, so every near-vertical segment links two visits to the same place: in red the loops that were detected and validated, and in green the subset whose estimated drift exceeded 10 cm and therefore triggered a pose-graph optimization. The insets show the 3D-2D correspondences of one such loop, with the observations of the candidate landmarks, the keypoints of the new keyframe, and the inlier matches that survive the geometric validation.
Fig. 4: Structure of a landmark block. (a) Original RootBA: per-camera state of dimension dp=9 , including the intrinsic parameters xi in orange. Rows correspond to the observations of the landmark. (b) RootSLAM removes the intrinsics, so dp=6 , and accumulates observations from multiple cameras of the rig in the same body pose variable xb .
ATE [cm]
RTE [cm]
Seq.
#
BSL
+LC
+GBA
RS
BSL
+LC
+GBA
RS
MIO
16
33.0
9.3
8.3
7.0
1.38
1.48
1.88
1.26
MIP
17
23.6
4.0
2.6
2.3
0.62
0.79
0.94
0.70
MG
15
21.9
4.5
4.1
3.4
1.35
1.47
1.73
1.21
MO
16
19.0
5.7
5.7
4.6
1.15
0.98
1.30
0.89
EMH
5
9.1
8.1
8.1
7.0
0.89
1.03
0.91
0.88
TABLE I: Ablation study over the components of RootSLAM
ATE [cm]
RTE [cm]
Total time* [s]
Sequence
#
double
float
double
float
double
float
MIO
16
7.0
7.0
1.27
1.26
4.59
2.03
MIP
17
2.3
2.3
0.71
0.70
15.19
7.74
MG
15
3.5
3.4
1.21
1.21
2.85
1.39
MO
16
4.9
4.6
0.91
0.89
3.68
2.07
EMH
5
7.0
7.0
0.88
0.88
2.69
1.29
TABLE II: Numerical precision of global optimizer
ATE [cm]
RTE [cm]
Succ.
Seq.
#
BSL
RS
OK2
OS3
BSL
RS
OK2
OS3
OS3 †
MIO
16
33.0
7.0
451.7
39890
1.38
1.26
5.97
184.1
87.1
MIP
17
23.6
2.3
2.8
42.7
0.62
0.70
0.45
1.63
94.2
MG
15
21.9
3.4
10.5
827.1
1.35
1.21
0.89
11.54
98.4
MO
16
19.0
4.6
42.9
21.7
1.15
0.89
0.83
7.89
91.5
EMH
5
9.1
7.0
3.2
3.4
0.89
0.88
0.94
0.97
91.8
TABLE III: Accuracy against the state of the art
Fig. 5: Estimated trajectories against ground truth on three segments of MIPP01, TR5 and EV103, with a frame of the scene from a highlighted section. RootSLAM stays clearly ahead on the MSD segment, where the fast head motion of MIPP01 degrades both references.
Mean frame time [ms]
Global BA [s]
Sequence
Dur. [s]
BSL
RS
OK2
OS3
RS
OK2
EMH02
152.0
11.8
11.5
191.5 †
40.9
2.83
6.35
EV102
85.5
8.9
9.3
132.5 †
38.9
1.22
6.07
EV202
117.4
9.4
9.0
132.6 †
40.3
3.26
6.87
Group Mean
118.3
10.1
9.9
152.2 †
40.0
2.44
6.43
MIO02
87.3
15.2
15.1
80.7 †
40.0 †
0.77
13.16
TABLE IV: System times on representative sequences
Fig. 6: Per-stage timings of RootSLAM on MOO12, a 42 min stereo sequence at 30 Hz, run on an Intel Core Ultra 9 285H. (a) Latency of the VIO pipeline, accounting for front-end and local optimizer threads, needed to estimate a pose. (b) Pending messages of the queues feeding the loop closer and the map. (c) Work spent per keyframe on each loop closing stage, in log scale. Solid lines are a moving average over a 6 s window, with the raw data in the background. For (b) we show the peak size of each window. In parentheses, the mean time of each stage.
Fig. 7: Per-frame processing time on MOO02, from MSD at 30Hz , in log scale. Solid lines are a moving average over a 2 s window, with the raw data in the background. The dashed line is the real-time budget of the sequence. RootSLAM tracks the Basalt VIO it extends within a fraction of a millisecond and stays far below the budget.
Simultaneous localization and mapping (SLAM) is one of the services running on an autonomous robot. It is typically run to assist other tasks such as planning, manipulation, etc. All these tasks are run on edge hardware and are subject to severe resource constraints. However, most SLAM systems are built and tested in isolation, and their performance is reported as if they are the only task running on a system. We observe that existing benchmarks lack a common mechanism for comparing SLAM systems under realistic resource constraints. To address this limitation, we have developed SLAMSqueezeBench, a framework that allows testing of SLAM systems under realistic workloads on edge hardware. It does so by imposing constraints on compute and memory resources available for the SLAM system during execution. It also simulates realistic camera frame acquisition with frame drops when a finite buffer is full. Using SLAMSqueezeBench, we compare nine SLAM systems spanning classical systems, learning-based systems, and approaches for Gaussian splatting. Our testing framework will be available for use by the community upon publication.
The tracking module of a visual-inertial SLAM system processes incoming image frames and IMU data to estimate the position of the frame in relation to the map. It is important for the tracking to complete in a timely manner for each frame to avoid poor localization or tracking loss. We therefore present a new approach which leverages GPU computing power to accelerate time-consuming components of tracking in order to improve its performance. These components include stereo feature matching and local map tracking. We implement our design inside the ORB-SLAM3 tracking process using CUDA. Our evaluation demonstrates an overall improvement in tracking performance of up to 2.8x on a desktop and Jetson Xavier NX board in stereo-inertial mode, using the well-known SLAM datasets EuRoC and TUM-VI.
Kimia Khabiri, Parsa Hosseininejad, Shishir Gopinath +2
We present AMB3R-SLAM, a real-time monocular SLAM system capable of reconstructing kilometer-scale trajectories over 10k frames on a single consumer-grade GPU. Our model couples a lightweight front-end for low-latency online tracking with a hierarchical backend that progressively enforces local, mid-level, and global consistency. By avoiding bundle adjustment that relies on the static world assumption, our system naturally handles complex dynamic scenes out of the box. Furthermore, we demonstrate that our method can be extended to leverage stereo, RGB-D, and LiDAR as additional inputs. AMB3R-SLAM achieves strong camera tracking performance across 9 datasets, reducing the absolute trajectory error (ATE) of previous state-of-the-art methods on VBR and Oxford Spires by over 70%. With additional LiDAR input, our model further reduces ATE to sub-meter level on KITTI and VBR datasets.
Hengyi Wang, Lourdes Agapito
Department of Computer Science, University College London