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.