[Paper Review] A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors

Tong Qin, Jie Pan+2 more|arXiv (Cornell University)|Jan 11, 2019

Robotics and Sensor-Based Localization29 references296 citations

TL;DR

Proposes a general optimization-based framework that fuses multiple sensor types (vision and inertial) as residual factors in a pose-graph, enabling flexible sensor suites for local odometry with real-time performance.

ABSTRACT

Nowadays, more and more sensors are equipped on robots to increase robustness and autonomous ability. We have seen various sensor suites equipped on different platforms, such as stereo cameras on ground vehicles, a monocular camera with an IMU (Inertial Measurement Unit) on mobile phones, and stereo cameras with an IMU on aerial robots. Although many algorithms for state estimation have been proposed in the past, they are usually applied to a single sensor or a specific sensor suite. Few of them can be employed with multiple sensor choices. In this paper, we proposed a general optimization-based framework for odometry estimation, which supports multiple sensor sets. Every sensor is treated as a general factor in our framework. Factors which share common state variables are summed together to build the optimization problem. We further demonstrate the generality with visual and inertial sensors, which form three sensor suites (stereo cameras, a monocular camera with an IMU, and stereo cameras with an IMU). We validate the performance of our system on public datasets and through real-world experiments with multiple sensors. Results are compared against other state-of-the-art algorithms. We highlight that our system is a general framework, which can easily fuse various sensors in a pose graph optimization. Our implementations are open source\footnote{https://github.com/HKUST-Aerial-Robotics/VINS-Fusion}.

Motivation & Objective

• Motivate the need for a flexible odometry framework that can accommodate arbitrary sensor combinations to improve robustness and practicality.
• Propose an optimization-based formulation where each sensor yields a residual factor contributing to a unified pose graph.
• Demonstrate the framework with visual and inertial sensors and validate on public datasets and real-world experiments.
• Provide open-source code to facilitate community adoption and extension.

Proposed method

• Model the odometry problem as a maximum likelihood estimation leading to a nonlinear least-squares problem (bundle adjustment) over a sliding window.
• Represent each sensor measurement as a generic factor in a pose graph, with states including robot pose, velocity, biases, and landmark depths.
• Use camera factors (monocular and stereo) for visual reprojection residuals and IMU preintegration factors for inertial constraints between consecutive frames.
• Incorporate marginalization (Schur complement) to bound computation by pruning old states while preserving information as a prior.
• Solve the resulting sparse linear system with a solver (Ceres) in a iterative optimization loop until convergence.
• Note the framework is extendable to other sensors by adding corresponding residual factors.

Experimental results

Research questions

• RQ1 Can a single optimization-based framework support multiple sensor suites for local odometry without redesigning the estimator?
• RQ2 How does incorporating IMU preintegration and camera reprojection factors within a sliding-window BA perform across stereo, monocular+IMU, and stereo+IMU configurations?
• RQ3 What is the effect of marginalization on computational efficiency and estimation accuracy in real-world and public datasets?
• RQ4 How robust is the proposed framework to sensor failure or replacement by alternative sensing modalities?

Key findings

• The framework supports three sensor suites: stereo cameras, monocular camera with an IMU, and stereo cameras with an IMU.
• IMU integration significantly improves motion tracking and reduces drift compared to stereo-only setups.
• Experiments on EuRoC datasets show the framework generally outperforms OKVIS in multiple sequences when using IMU-enabled configurations.
• Real-world outdoor experiments demonstrate improved accuracy with IMU-inclusive configurations over stereo-only setups.
• Open-source implementation is provided (VINS-Fusion), enabling community use and extension.
• The method maintains robustness across different sensor configurations and can accommodate additional sensor types by adding new residual factors.