A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors

TL;DR

The paper introduces a general optimization-based framework for local odometry that supports multiple sensor suites by modeling each sensor as a factor in a pose graph. It employs a sliding-window nonlinear least-squares approach with IMU preintegration and marginalization to fuse visual and inertial data across stereo, monocular+IMU, and stereo+IMU configurations. The method is validated on public datasets (e.g., EuRoC) and real-world outdoor tests, showing robustness and often outperforming a state-of-the-art baseline when IMU data is included. The framework’s generality enables easy sensor reconfiguration and resilience to sensor failure, with open-source code released for the community.

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}.

Figures (8)

• Figure 1: An illustration of the proposed framework for state estimation, which supports multiple sensor choices, such as stereo cameras, a monocular camera with an IMU, and stereo cameras with an IMU. Each sensor is treated as a general factor. Factors which share common state variables are summed together to build the optimization problem.
• Figure 2: A graphic illustration of the pose graph. Each node represents states (position, orientation, velocity and so on) at one moment. Each edge represents a factor, which is derived by one measurement. Edges constrain one state, two states or multiple states.
• Figure 3: A graphic illustration of the proposed framework with visual and inertial sensors. The IMU and one of cameras are optional. Therefore, it forms three types (stereo cameras, a monocular camera with an IMU, and stereo cameras with an IMU). Several camera poses, IMU measurements and visual measurements exist in the pose graph.
• Figure 4: Relative pose error geiger2012we in MH_05_difficult. Two plots are relative errors in translation and rotation respectively.
• Figure 5: Relative pose error geiger2012we in V2_02_medium. Two plots are relative errors in translation and rotation respectively.