[Paper Review] A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

Tong Qin, Shaozu Cao+2 more|arXiv (Cornell University)|Jan 11, 2019

Robotics and Sensor-Based LocalizationEngineering205 citations

TL;DR

The paper proposes an optimization-based framework that fuses local VO/VIO estimates with multiple global sensors (GPS, magnetometer, barometer, etc.) in a pose graph to achieve locally accurate and globally drift-free 6-DoF localization, demonstrated on public datasets and real-world experiments with open-source code.

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

Motivation & Objective

• Motivate and build a localization framework that delivers locally accurate and globally drift-free 6-DoF poses by fusing local VO/VIO with global sensors.
• Provide a general, extensible framework that can accommodate multiple global sensors via a unified pose graph optimization.
• Demonstrate robustness and accuracy through public datasets (e.g., KITTI) and real-world experiments, and release open-source code.

Proposed method

• Classify sensors into local and global; use VO/VIO for local pose estimates and treat global sensors as graph factors.
• Formulate pose estimation as a maximum likelihood/ nonlinear least squares optimization (bundle adjustment) over global poses with local and global factors.
• Define local factors from VO/VIO between consecutive frames; model GPS, magnetometer, barometer as global factors with appropriate residuals and covariances.
• Construct a sparse pose graph where nodes are poses in the world frame and edges include local constraints (from VO/VIO) and global constraints (from global sensors).
• Solve using Ceres Solver with Gaussian-Newton/Levenberg-Mardt iterations at low frequency (1 Hz) to obtain global alignment and drift correction while allowing high-rate local poses to be transformed into the global frame.

Experimental results

Research questions

• RQ1 Can multiple global sensors be integrated into a single optimization-based pose graph to achieve drift-free global localization without sacrificing local accuracy?
• RQ2 How does the proposed framework perform when fusing GPS, magnetometer, and barometer with local VO/VIO across datasets and real-world experiments?
• RQ3 Is the framework extensible to include additional global sensors beyond GPS, magnetometer, and barometer?
• RQ4 What are the comparative gains in long-distance trajectories versus VO/VIO alone or other multi-sensor fusion approaches?

Key findings

• The framework achieves locally accurate and globally drift-free 6-DoF poses by fusing local VO/VIO with multiple global sensors in a unified pose graph.
• On KITTI datasets, the proposed method yields lower translation RMSE than ORB-SLAM in several sequences, indicating effective drift elimination by GPS integration.
• Rotation accuracy remains comparable to local VO/VIO, with GPS mainly reducing translation drift over long trajectories.
• Real-world outdoor experiments show the method outperforms MSF and VIO alone in relative pose error, with drift significantly mitigated and trajectories aligned to road maps or RTK ground truth.
• The system is demonstrated as a general framework able to incorporate additional global sensors, and the authors provide open-source code for community use.