A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

TL;DR

The paper tackles the drift inherent in local pose estimation by fusing local VO/VIO with global sensors in a unified optimization framework. It formulates a global pose graph where local constraints from VO/VIO are augmented with global-factor constraints from GPS, magnetometers, barometers, and other sensors, solved via nonlinear least-squares. The approach demonstrates improved long-range translation accuracy and drift elimination on KITTI data and real-world experiments, with an open-source implementation. This general framework enables seamless integration of diverse global sensors, enhancing robust, drift-free 6-DoF localization for autonomous robots.

Abstract

Accurate state estimation is a fundamental problem for autonomous robots. To achieve locally accurate and globally drift-free state estimation, multiple sensors with complementary properties are usually fused together. Local sensors (camera, IMU, LiDAR, etc) provide precise pose within a small region, while global sensors (GPS, magnetometer, barometer, etc) supply noisy but globally drift-free localization in a large-scale environment. In this paper, we propose a sensor fusion framework to fuse local states with global sensors, which achieves locally accurate and globally drift-free pose estimation. Local estimations, produced by existing VO/VIO approaches, are fused with global sensors in a pose graph optimization. Within the graph optimization, local estimations are aligned into a global coordinate. Meanwhile, the accumulated drifts are eliminated. We evaluate the performance of our system on public datasets and with real-world experiments. Results are compared against other state-of-the-art algorithms. We highlight that our system is a general framework, which can easily fuse various global sensors in a unified pose graph optimization. Our implementations are open source\footnote{https://github.com/HKUST-Aerial-Robotics/VINS-Fusion}.

Figures (10)

• Figure 1: KITTI dataset results of the proposed sensor fusion framework (VO + GPS). The top of this figure is a pair of stereo images. The left bottom part is the estimated trajectory and feature points, while the right bottom part is the estimated global trajectory aligned with Google map.
• Figure 2: An illustration of the proposed framework structure. The global estimator fuses local estimations with various global sensors to achieve locally accurate and globally drift-free pose estimation.
• Figure 3: An illustration of the global pose graph structure. Every node represents one pose in world frame, which contains position and orientation. The edge between two consecutive nodes is a local constraint, which is from local estimation (VO/VIO). Other edges are global constraints, which come from global sensors.
• Figure 4: Rotation error and translation error plot in 10_03_drive_0042.
• Figure 5: Rotation error and translation error plot in 09_30_drive_0033.