Source-linked AI summary
A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors
Tong Qin, Jie Pan, Shaozu Cao, Shaojie Shen
TL;DR
Robotic state-estimation algorithms often support only one sensor or a fixed sensor suite, despite platforms using changing combinations. The paper proposes a factor-based optimization framework that fuses multiple sensors and demonstrates it with three visual-inertial suites. Evaluations on public datasets and real-world experiments validate fusion across different settings.
Problem
Existing state-estimation algorithms usually target a single sensor or specific sensor suite, while platforms use varied and changing sensor combinations.
Method
The paper treats each sensor measurement as a general factor and sums factors sharing state variables within an optimization framework, demonstrated with visual and inertial sensors.
Results
Evaluations on public datasets and real-world experiments validate the framework with stereo cameras, monocular camera plus IMU, and stereo cameras plus IMU.
Takeaways & Limitations
The framework can fuse sensor data under different settings and supports switching among multiple sensor combinations.
Abstract
from arXiv · showhide
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}.
I. INTRODUCTION
The paper addresses the need for state-estimation algorithms that can support changing sensor suites across robotic platforms. It proposes and demonstrates a general optimization-based framework for multiple visual and inertial combinations.
- Real-time 6-DoF state estimation supports robotics applications including exploration, autonomous driving, VR, and AR.
- Existing algorithms typically target a single sensor or specific sensor suite, limiting portability across platforms and sensor combinations.
- The proposed optimization-based framework supports multiple sensor combinations and can switch between them.
- Visual and inertial demonstrations cover stereo cameras, monocular camera with an IMU, and stereo cameras with an IMU.
- The system was evaluated on public datasets and real experiments, with open-source code released for the community.
II. RELATED WORK
Related work spans filter-based and optimization-based approaches to multi-sensor fusion. The paper adopts sliding-window optimization while representing measurements and states as a pose graph.
- Multi-sensor fusion approaches are broadly divided into filter-based and optimization-based methods.
- Filter-based methods propagate states with high-rate inertial measurements and update them with visual measurements, but are sensitive to early linearization and time synchronization.
- Optimization-based methods jointly optimize multiple variables and measurements, offering buffering for disordered sensor arrivals at higher computational cost.
- In the pose graph, nodes represent states at moments and edges represent measurement-derived factors constraining one or more states.
- Sliding-window methods bound the pose graph size, and the paper adopts a sliding-window optimization framework for state estimation.
- The visual-inertial demonstration supports stereo cameras, monocular camera with an IMU, and stereo cameras with an IMU, with the IMU and one camera optional.
A. Problem Definition
The framework estimates robot pose together with optional camera and IMU-related states. Its visual-inertial formulation includes landmark depths, velocity, and sensor biases, while calibration is assumed known.
- The main estimated states are the robot’s 3D position and orientation, with additional sensor-related states when required.
- Camera-related states include depths or 3D locations of visual landmarks observed by the system.
- IMU-related states include velocity, time-varying acceleration bias, and gyroscope bias.
- The IMU-related state block can be omitted when only stereo cameras are used.
- Sensor-center-to-body translations and camera calibration parameters are assumed known from offline calibration.
2) Cost Function:
The cost function formulates state estimation as maximum likelihood under independent, Gaussian-distributed measurements, then converts it into a nonlinear least-squares problem.
- State estimation is formulated as maximum likelihood estimation over the joint distribution of robot poses across time.
- Under measurement independence, the likelihood is expressed through the product of individual measurement probabilities.
- The measurement set S includes observations from cameras, IMU, and other sensors, with measurement uncertainty assumed Gaussian.
- The negative log-likelihood uses the sensor model h(·) and converts estimation into a nonlinear least-squares problem known as Bundle Adjustment.
1) Camera Factor:
The camera factor models feature reprojection for monocular and stereo cameras, using tracked feature associations, calibrated camera geometry, and pixel-coordinate uncertainty.
- The framework supports both monocular and stereo cameras, detecting and tracking corner features to construct camera factors.Stereo tracking additionally matches features between the left and right images.
- Each camera factor projects a feature from its first observation into subsequent frames according to feature associations.
- Projection and back-projection functions depend on the camera model, while each camera uses its corresponding calibrated extrinsic transformation.The extrinsic transformation maps the body center to the camera center and is calibrated offline.
- The reprojection-error covariance is constant in pixel coordinates and derives from the camera’s intrinsic calibration results.
- The reprojection factor supports temporal projection within a camera and spatial projection between the left and right cameras.
2) IMU Factor:
The IMU factor uses preintegration to summarize high-rate inertial measurements between frames while propagating uncertainty in motion and bias variables.
- The framework constructs the IMU factor with a standard IMU preintegration algorithm.Multiple high-frequency IMU measurements between two frames are pre-integrated before forming the factor.
- Acceleration and gyroscope noise are modeled as Gaussian white noise, while time-varying biases follow random-walk processes.
- Preintegration propagates covariance for relative position, velocity, rotation, and bias.
- Every pair of adjacent frames contributes one IMU factor to the cost function.The rotation residual uses manifold subtraction, and gravity is treated as a known vector with norm around 9.81.
3) Other Factors:
The framework accommodates sensors beyond cameras and IMUs by representing their measurements as residual factors within the same nonlinear optimization process.
- Wheel speedometers, LiDAR, and Radar can be incorporated by modeling their measurements as general residual factors.These residuals are added to the cost function alongside existing sensor factors.
- The nonlinear least-squares problem is linearized around an initial state estimate using each factor’s Jacobian matrix.
- A Newton-Gauss or Levenberg-Marquardt approach solves the linearized problem for the state increment δX.
- The current state is updated with manifold addition and the procedure iterates until convergence.The implementation uses the Ceres solver to obtain stable and efficient results.
D. Marginalization
Marginalization bounds the growing computational cost by converting information from past states into a prior over the remaining states. The resulting MAP problem adds this prior term while retaining past information.
- Marginalization converts previous measurements into a prior term, bounding computational complexity as the number of states grows over time.The paper states that unbounded state growth would increase complexity quadratically.
- The marginalized states Xm and remaining states Xr are separated, and the Schur complement produces a new prior Hp, bp for Xr.The prior represents information from the marginalized states.
- Within the sliding window, the system keeps ten spatial camera frames and marginalizes factors associated with the first frame when a new keyframe arrives.
- After marginalization, state estimation is formulated as a MAP problem combining the likelihood of measurements with the prior over current states.The states before m are marginalized and converted into the prior term.
- The resulting optimization differs from the original formulation only by the added prior term and is solved with the Ceres solver.
E. Discussion
The framework treats sensors as interchangeable factors, allowing sensor combinations to be changed and failed sensors to be replaced. The discussion also evaluates the system on public datasets and outdoor experiments.
- Sensors can be added when their measurements are represented as general residual factors, while inactive sensors can be removed and replaced after failure.
- Relative pose error plots report translation and rotation errors for the MH 05 difficult and V2 02 medium sequences.
- The framework is evaluated with visual and inertial sensors on public datasets and in a large-scale outdoor environment.The evaluation includes comparison with another state-of-the-art algorithm and numerical accuracy analysis.
A. Datasets
The dataset evaluation uses EuRoC sequences with stereo, monocular-inertial, and stereo-inertial configurations, comparing the framework with OKVIS using trajectory and relative-pose errors. IMU-assisted methods improve robustness when visual tracking fails, while calibration affects stereo-inertial performance.
- EuRoC provides stereo images at 20 FPS, synchronized IMU measurements at 200 Hz, and ground truth from VICON and Leica MS50.
- The experiments separately evaluate stereo cameras, a monocular camera with an IMU, and stereo cameras with an IMU across EuRoC sequences.
- The proposed results are compared with OKVIS using Relative Pose Errors and Absolute Trajectory Errors, with trajectories aligned to ground truth by Horn’s method.
- Stereo-only estimation fails on V1 03 difficult and V2 03 difficult, whereas methods involving the IMU succeed on all sequences.The paper attributes the improvement to the IMU bridging visual-tracking failures caused by illumination change, textureless areas, or motion blur.
- Stereo-only estimation performs worst in most sequences, while the authors report that their results outperform OKVIS in most sequences.The IMU suppresses roll and pitch drift through gravity measurements, but stereo cameras with an IMU require more accurate calibration than monocular-inertial estimation.
- The outdoor sensor suite combines stereo cameras, a 200 Hz IMU, and GPS, and the outdoor experiment estimates trajectories for comparison with GPS.
B. Real-world experiment
The outdoor experiment evaluates three sensor combinations against GPS and shows that adding an IMU substantially improves accuracy over stereo-only estimation. The framework is demonstrated in real-world settings, while globally aware estimation remains future work.
- The sensor suite combines stereo cameras at 20Hz with an onboard IMU at 200Hz and uses GPS position as ground truth.
- The outdoor evaluation compares stereo cameras, a monocular camera with an IMU, and stereo cameras with an IMU.
- Stereo-only estimation shows noticeable position drift, while methods involving the IMU achieve better accuracy in the outdoor experiments.The comparison uses two walked circles, relative pose error, and RMSE results against GPS.
- The framework fuses sensor data under different settings and is presented as supporting multiple sensor combinations for local pose estimation.
- Future work will add global sensors such as GPS to achieve locally accurate and globally aware pose estimation.