RGBD-SLAM-V2

Endres 2013 · Paper

One-line summary — A complete graph-based RGB-D SLAM system (“3-D Mapping With an RGB-D Camera”, T-RO 2014) using visual features back-projected to 3D, a beam-based transformation validation model, g2o pose-graph optimization, and OctoMap output — evaluated exhaustively on the TUM RGB-D benchmark.

Problem

Early RGB-D SLAM approaches lacked a standardized, modular pipeline and reproducible evaluation. Beyond that, feature-based RANSAC and ICP both lack reliable failure detection: a low inlier count may just mean low overlap, while repetitive man-made structure (identical chairs, wallpapers) produces confidently wrong transformations. The community needed a robust open baseline that combines color features with dense depth in a principled graph back-end, plus a rigorous benchmark-driven characterization of what actually matters for accuracy.

Method & architecture

The system splits into the classic frontend / backend / map-representation triad.

Frontend: egomotion from features + depth. Keypoints are extracted from the RGB image (SIFT via GPU, SURF, or ORB; descriptor d∈R64\mathbf{d} \in \mathbb{R}^{64} for SURF) and localized in 3D through the depth image. Matching uses Lowe’s nearest/second-nearest ratio test (Hamming distance for ORB; Euclidean or the better-performing Hellinger distance for SIFT/SURF). The relative transformation between two frames is estimated by RANSAC: three 3D-3D correspondences initialize an estimate, inliers are counted by Mahalanobis distance, and the estimate is recursively refined with a decreasing inlier threshold via a closed-form least-squares solution.

Environment measurement model (EMM). To validate a candidate transformation independently of how it was estimated, depth points of one frame are projected into the other; each depth pixel implicitly defines a beam whose free space observed points must respect. Modeling both measurements of a common surface point with sensor-noise covariances Σi,Σj\Sigma_i, \Sigma_j, the paper derives

p(yi∣yj)=N(yi; yj, Σij),Σij=Σi+Σjp(\mathbf{y}_i \mid \mathbf{y}_j) = \mathcal{N}(\mathbf{y}_i;\, \mathbf{y}_j,\, \Sigma_{ij}), \qquad \Sigma_{ij} = \Sigma_i + \Sigma_j

Points within 3 sigma count as inliers; points projected far behind the associated beam are “occluded” and ignored; points landing in observed free space are outliers. A transformation is rejected unless the quality q=I/(I+O)q = I/(I+O) (inliers over inliers-plus-outliers) exceeds a threshold and inliers are at least 25% of observed points — a robust per-point hypothesis test instead of a brittle joint χ3N2\chi^2_{3N} test.

Loop-closure search. Candidate frames for pairwise matching combine (i) nn immediate predecessors, (ii) kk frames sampled from the geodesic (graph) neighborhood of the previous frame via a limited-depth spanning tree — so found loop closures guide the search for more — and (iii) ll frames sampled from designated keyframes for large loops.

Backend: pose-graph optimization with g2o. Validated transformations zijz_{ij} with information matrices Ωij\Omega_{ij} form edges of a pose graph over sensor poses X\mathbf{X}, optimized by minimizing

F(X)=∑⟨i,j⟩∈Ce(xi,xj,zij)⊤ Ωij e(xi,xj,zij)F(\mathbf{X}) = \sum_{\langle i,j\rangle \in \mathcal{C}} e(\mathbf{x}_i, \mathbf{x}_j, z_{ij})^{\top}\, \Omega_{ij}\, e(\mathbf{x}_i, \mathbf{x}_j, z_{ij})

After optimization, edges whose Mahalanobis discrepancy is too large are pruned — a second robustness layer against bogus loop closures from repetitive structure.

Map: OctoMap. The optimized trajectory projects the clouds into a probabilistic octree occupancy map that explicitly represents free and unknown space (2 cm resolution maps of the test sequences take only 4.2-25 MB) and is directly usable for navigation, unlike raw point clouds.

Results

Evaluated on the TUM RGB-D benchmark (which the authors co-created) using ATE: ATERMSE=1n∑i∥trans(x^i)−trans(xi)∥2\mathrm{ATE}_{\mathrm{RMSE}} = \sqrt{\tfrac{1}{n}\sum_i \|\mathrm{trans}(\hat{\mathbf{x}}_i) - \mathrm{trans}(\mathbf{x}_i)\|^2}. Headline numbers (Intel i7 3.4 GHz + GTX 570): ATE RMSE of 0.026 m on fr1/desk (15.2 Hz), 0.087 m on fr1/room, 0.057 m on fr2/desk, 0.86 m on fr2/large_no_loop, and 1.65 m on a 229 m MIT Stata Center sequence — better than the respective best previously published results on all but fr2/large_no_loop. Feature study: GPU-SIFT is the most accurate (median RMSE 0.04 m over the fr1 sequences, ~600-700 features per frame sufficing), while ORB and Shi-Tomasi+SURF are fast options whose ~15 cm average error suits only benign scenarios; the Hellinger distance improved matching by up to 25.8% on some datasets at no runtime cost. On the challenging “Robot SLAM” Pioneer sequences, geodesic-neighborhood sampling cut average error by 26%, and the EMM (0.82 ms average evaluation) plus edge pruning drastically reduced error, with best results using both. The system is fully open source.

Why it matters for SLAM

RGBD-SLAM-V2 established the feature-based frontend + pose-graph backend pipeline as the standard baseline for RGB-D SLAM, in contrast to the dense GPU-fusion line started by KinectFusion, and its EMM introduced principled, estimator-independent failure detection using dense depth. Its influence is arguably even larger through the TUM RGB-D benchmark and the ATE/RPE evaluation tools, which nearly every subsequent SLAM paper reports on. It remains a very readable reference implementation of a full classical SLAM system.