PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Tong Qin, Shaozu Cao, Jie Pan, Shaojie Shen

arXiv:1901.03642v1cs.CV

TL;DR

The paper addresses how to combine locally accurate but drifting estimates with globally drift-free sensor measurements for autonomous-robot pose estimation. It proposes an optimization-based pose-graph framework that fuses VO/VIO local factors with global-sensor factors, demonstrating locally accurate and globally drift-free estimation on public datasets and real-world experiments.

  • Problem

    Autonomous robots require precise 6-DoF poses, but local sensors provide accurate incremental estimates while vision-based drift correction struggles in large-scale environments; complementary global sensors are therefore needed.

  • Method

    The framework treats local VO/VIO estimations and global sensor measurements as factors in a unified pose graph, aligning local estimates to a global frame while supporting multiple global sensors.

  • Results

    The system achieves locally accurate and globally drift-free pose estimation, with evaluations on public datasets and real-world experiments demonstrating its performance.

  • Takeaways & Limitations

    A general pose-graph formulation can fuse various global sensors with local estimations in one framework for global pose estimation.

Abstract

from arXiv · show

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

I. INTRODUCTION

Autonomous robots need precise local 6-DoF estimation and globally consistent localization. The proposed optimization framework fuses local estimations with global sensors to achieve locally accurate and globally drift-free pose estimation.

  • Motivation: Local pose estimators provide accurate incremental estimates but lack a fixed global coordinate and accumulate drift over long runs.Loop closure methods may not handle large-scale environments with massive data.
  • Motivation: Global sensors provide drift-free earth-referenced measurements, but their noise, low rate, and limited observability make them insufficient for real-time 6-DoF estimation.GPS measures approximate 3D position without 3D orientation.
  • Framework: The paper proposes fusing local estimations with global sensor measurements in a pose graph optimization.Local and global measurements are represented as factors and summed into one optimization problem.
  • Contributions: The framework supports various global sensors while targeting locally accurate and globally drift-free localization.The evaluation covers public datasets and real-world experiments.

II. RELATED WORK

Prior work addresses local or global-aware localization, but each sensor category has complementary limitations. Optimization-based fusion offers a way to combine them while accommodating late-arriving measurements better than filters.

  • Local localization: Local localization methods use cameras, IMUs, LiDAR, RGB-D, and event sensors for 6-DoF estimation in small environments.Prior work includes visual, LiDAR-based, RGB-D, event-based, visual-inertial, and visual-LiDAR methods.
  • Global-aware localization: Global-aware localization incorporates GPS, magnetometers, and barometers to provide earth-referenced measurements, but these measurements are noisy and low-frequency.Global sensors are therefore usually fused with local sensors rather than used alone.
  • Fusion methods: Filter-based fusion methods are sensitive to time synchronization because late measurements cannot be propagated backward through the filter.Special ordering mechanisms are required to keep measurements from multiple sensors in order.
  • Fusion methods: Optimization-based methods can buffer measurements in a large bundle, allowing delayed measurements to be stored before optimization.This provides an advantage over filter-based methods in handling measurement timing.

1) Local Sensors:

The framework uses existing local VO/VIO estimators to produce local-frame poses, then represents motion and sensor information as factors for global estimation. Local sensors drift with distance from their starting point, whereas global sensors remain earth-referenced but noisy.

  • Local Sensors: Camera, LiDAR, and IMU estimates are initialized in a local reference frame, so accumulated drift grows with distance from the starting point.The first pose is generally used as the local origin.
  • Global Sensors: GPS, magnetometers, and barometers operate in a fixed global frame and provide noisy measurements whose error is independent of traveled distance.GPS measures absolute longitude, latitude, and altitude relative to the earth.
  • Global Estimation: The global estimator converts local results and global sensors into unified factors and produces globally aware 6-DoF pose results.The pose graph uses relative pose constraints between consecutive poses from local estimation.
  • Local Pose Estimation: Existing VO/VIO algorithms supply 6-DoF local poses, and any such algorithm can serve as the local estimator in the framework.The local estimation component is not the paper’s main contribution.

B. Global Pose Graph Structure

The global pose graph represents world-frame poses as nodes and combines local relative-pose constraints with global-sensor constraints. Under independent Gaussian measurement assumptions, estimation becomes a nonlinear least-squares bundle-adjustment problem.

  • Graph structure: Each graph node represents a world-frame pose containing position and orientation, while consecutive nodes are connected by local VO/VIO constraints.The node density is determined by the lowest-frequency sensor.
  • Graph structure: Global-sensor measurements add other edges that constrain the poses in the global frame.The graph thereby combines local relative constraints with global constraints.
  • Optimization formulation: The optimization estimates all global poses as a maximum-likelihood problem over the joint distribution of robot poses.The state is the set of global poses X = {x0, x1, ..., xn}.
  • Optimization formulation: Assuming independent measurement probabilities and Gaussian uncertainty, the state estimation is converted into nonlinear least squares, also known as bundle adjustment.The measurement set includes local VO/VIO and global sensor measurements.

1) Local Factor:

The local factor uses relative pose information from consecutive VO/VIO frames, while GPS measurements are represented in an ENU world frame and constrain graph-node positions.

  • 1) Local Factor:: The local factor uses relative pose between consecutive frames because VO/VIO is accurate within a small region.
  • 1) Local Factor:: Its two rows represent relative position error and relative rotation error between consecutive local-frame poses.
  • 1) Local Factor:: VO/VIO-provided pose covariance is used when available; otherwise, a unified covariance is assigned to local measurements.
  • 1) Local Factor:: GPS longitude, latitude, and altitude are converted into ENU coordinates by setting the first GPS measurement as the origin.
  • 1) Local Factor:: GPS factors directly constrain each node’s position, with covariance determined by the number of received satellites.

3) Magnetometer Factor:

The magnetometer factor uses the direction of the measured magnetic field to constrain world-frame orientation, while adapting covariance to measurement reliability.

  • 3) Magnetometer Factor:: The magnetometer measures magnetic-field intensity whose direction helps determine orientation in the world frame.
  • 3) Magnetometer Factor:: The factor compares the measured magnetic vector with the local ENU magnetic-field vector under the assumption that the field is constant within the region.
  • 3) Magnetometer Factor:: The robot-to-magnetometer transformation is known and calibrated offline.
  • 3) Magnetometer Factor:: Only the normalized magnetic vector is used because environmental effects can easily alter the field magnitude.
  • 3) Magnetometer Factor:: Covariance is increased when measured intensity differs substantially from the local reference and decreased otherwise.

4) Barometer Factor:

The barometer factor represents height residuals from pressure-derived measurements, while the broader framework supports additional global sensors and low-frequency graph optimization.

  • 4) Barometer Factor:: Barometric pressure is converted linearly to height under the assumption that pressure remains constant at one altitude over a time period.
  • 4) Barometer Factor:: The first barometer measurement defines the origin height for subsequent measurements.
  • 4) Barometer Factor:: The barometer factor is a residual of height estimation.
  • 4) Barometer Factor:: Because barometric measurements are noisy, the variance of several short-time measurements is used in the cost function.
  • 5) Other Global Factors:: The framework can model other global or artificial sensors as residual factors under one global frame.
  • 4) Barometer Factor:: Pose graph optimization runs at 1Hz, while subsequent VO/VIO poses can be transformed at 200Hz using the updated local-to-global transformation.

V. EXPERIMENTAL RESULTS

The KITTI evaluation compares the proposed GPS-fused system with ORB-SLAM, showing reduced translation drift and lower absolute trajectory error across all tested sequences, while rotation accuracy is not improved.

  • Datasets: The proposed system is compared with ORB-SLAM on KITTI using stereo cameras and GPS, with ground truth from an inertial navigation system.The evaluation uses RPE and ATE-related trajectory analysis on public sequences.
  • Datasets: The proposed method achieves lower translation error than ORB-SLAM, indicating that GPS effectively eliminates position drift.The comparison covers rotation and translation relative pose errors in two sequences.
  • Datasets: For rotation error, the proposed method is not better than ORB-SLAM because GPS does not directly measure rotation angles.The cited limitation concerns rotational accuracy rather than the system’s global position correction.
  • Datasets: Across all KITTI sequences, the proposed method outperforms ORB-SLAM in RMSE of absolute trajectory error.Estimated trajectories are aligned with ground truth using Horn’s method.
  • Datasets: On KITTI sequence 09 30 drive 0033, ORB-SLAM drifts several meters by the end, whereas the proposed trajectory matches the road network well.The trajectories are aligned with Google map from a bird-eye view.

B. Real-world experiment

Real-world experiments use a multi-sensor suite and RTK ground truth to compare VIO, the proposed system, and MSF. The proposed system substantially reduces translation drift and remains nearly drift-free over a larger outdoor route.

  • Real-world experiment: The outdoor sensor suite combines stereo cameras with an IMU, magnetometer, barometer, GPS receiver, and RTK receiver.RTK provides centimeter-level vertical and horizontal positioning and is treated as ground truth.
  • Real-world experiment: The small-scale experiment compares VIO, the proposed algorithm, and MSF using RTK trajectory ground truth.Trajectories and relative pose errors are evaluated after walking two circles on the ground.
  • Real-world experiment: The proposed system improves VIO accuracy substantially and outperforms MSF in the small-scale outdoor experiment.The reported comparison includes trajectory plots, relative pose error, and RMSE for additional outdoor sequences.
  • Real-world experiment: In the larger outdoor experiment, VIO drifts with distance, while the proposed system is almost drift-free and matches the road map well.The trajectories are compared with Google map using the full outdoor sensor suite.

VI. CONCLUSION

The paper presents an optimization-based framework that fuses local estimations from prior VO/VIO work with global sensors. It reports locally accurate and globally drift-free pose estimation, supported by public-dataset and real-world experiments.

  • The framework fuses local estimations from previous VO/VIO work with global sensors through pose graph optimization.
  • The system achieves locally accurate and globally drift-free 6-DoF pose estimation.
  • Performance is demonstrated on public datasets and through real-world experiments.
Loading 1901.03642v1…