Online and Certifiably Correct Visual Odometry and Mapping

TL;DR

Concisely, the paper tackles certified perception for safety-critical robotics by presenting two algorithms that provide provable error bounds for visual odometry and obstacle mapping using RGB-D data. The Certified Visual Odometry (C-VO) yields relative pose estimates with computable rotation and translation bounds, while the Certified ESDF (CESDF) deflates the SDF to under-approximate obstacle distance, guaranteeing safety despite VO drift. The methods are implemented on hardware to run at 30 frames per second and are demonstrated to produce safe maps and pose estimates, with comparisons to state-of-the-art VO and mapping baselines. Together, these contributions enable end-to-end certified perception that can support safety-critical planners and controllers.

Abstract

This paper proposes two new algorithms for certified perception in safety-critical robotic applications. The first is a Certified Visual Odometry algorithm, which uses a RGBD camera with bounded sensor noise to construct a visual odometry estimate with provable error bounds. The second is a Certified Mapping algorithm which, using the same RGBD images, constructs a Signed Distance Field of the obstacle environment, always safely underestimating the distance to the nearest obstacle. This is required to avoid errors due to VO drift. The algorithms are demonstrated in hardware experiments, where we demonstrate both running online at 30FPS. The methods are also compared to state-of-the-art techniques for odometry and mapping.

Figures (6)

• Figure 1: Block diagram describing our certified perception modules. Given successive RGBD frames, we first use the visual odometry (orange) module to compute a relative pose estimate, and the associated error bounds. These are used in the mapping (purple) module to construct a 3D map of the obstacle geometry. Using the error bounds computed in CVO, we can compute the ESDF deflation that is sufficient to ensure correctness.
• Figure 2: Rotation error bounds due to \ref{['corrollary:rotation_bound']} (orange) are tighter than due to \ref{['lemma:rotation_bound']} (blue). Each line shows the median and interquartile range of 100 trials of rototranslation estimation on synthetic data.
• Figure 3: Diagram demonstrating the CESDF approach. Suppose the robot moves by 0.1 m in the y-axis, but the estimated rototranslation has some error. The approximate approach \ref{['eqn:incorrect_esdf']} is unsafe, estimating the ESDF of $p$ to be 0.526 m, greater than the true ESDF of 0.5 m. In the proposed approach \ref{['eqn:new_radius']}, we calculate a correction $\Delta$, and compute the C-ESDF to be 0.456 m, a safe underestimate.
• Figure 4: Effect of graph fraction on accuracy and runtime of Certified Visual Odometry (CVO). (a) Log-Log plot of the computation time. (b) Log-Linear plot of accuracy. Both graphs were produced by running the algorithm on a recorderd dataset using the Realsense D455 camera, and ground-truth provided by VICON.
• Figure 5: Comparison of the Certified Visual Odometry (C-VO) algorithm with benchmark algorithms. We compare CVO with Nvidia VSlam vslam and VINS-Fusion yu2021vins. To compute the error, we also recorded the ground-truth trajectory using a VICON motion capture system. To enable a fair comparison, each method was run in Visual Odometry only mode, i.e., without loop-closures or IMU measurements. (a) The reconstructed trajectory from each method. (b-g) The relative rotation error and relative translation error for each method. Only our method provides error bounds.

Theorems & Definitions (22)

• Lemma 1
• proof
• Lemma 2
• proof
• Remark 1
• Lemma 3
• proof
• Lemma 4
• proof
• Remark 2