PaperlayerView exact revision on arXiv

Source-linked AI summary

R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping

Jiarong Lin, Chunran Zheng, Wei Xu, Fu Zhang

arXiv:2102.12400v1cs.RO

TL;DR

The paper addresses robustness and accuracy challenges in LiDAR-based and visual-inertial state estimation by tightly fusing LiDAR, inertial, and camera measurements. It combines high-rate error-state iterated Kalman-filter odometry with factor-graph map refinement, and reports accurate, robust localization and mapping across challenging environments and sensor failures.

  • Problem

    LiDAR-based odometry can degenerate with few geometric features, motivating fusion with inertial and camera measurements for robust state estimation.

  • Method

    The system combines filter-based odometry using an error-state iterated Kalman filter with factor-graph optimization of local keyframe poses and visual landmarks.

  • Results

    Experiments report high accuracy and robustness in large-scale indoor-outdoor and narrow tunnel-like environments, including aggressive motion and sensor failure.

  • Takeaways & Limitations

    Tightly fused LiDAR, inertial, and camera measurements support robust localization and mapping and enable dense, high-precision indoor-outdoor reconstruction.

Abstract

from arXiv · show

In this letter, we propose a robust, real-time tightly-coupled multi-sensor fusion framework, which fuses measurement from LiDAR, inertial sensor, and visual camera to achieve robust and accurate state estimation. Our proposed framework is composed of two parts: the filter-based odometry and factor graph optimization. To guarantee real-time performance, we estimate the state within the framework of error-state iterated Kalman-filter, and further improve the overall precision with our factor graph optimization. Taking advantage of measurement from all individual sensors, our algorithm is robust enough to various visual failure, LiDAR-degenerated scenarios, and is able to run in real-time on an on-board computation platform, as shown by extensive experiments conducted in indoor, outdoor, and mixed environment of different scale. Moreover, the results show that our proposed framework can improve the accuracy of state-of-the-art LiDAR-inertial or visual-inertial odometry. To share our findings and to make contributions to the community, we open source our codes on our Github.

I. INTRODUCTION

The paper introduces a tightly coupled LiDAR-inertial-visual system targeting LiDAR degeneration and combining real-time filtering with factor-graph refinement. It reports robust, accurate state estimation across challenging sensing conditions and supports dense, large-scale indoor-outdoor mapping.

  • Motivation: LiDAR-based SLAM can fail in scenes with few geometric features, especially when LiDARs have a small field of view.The proposed work addresses this degeneration problem by incorporating inertial and camera measurements.
  • Contribution: The framework tightly fuses LiDAR, inertial, and camera measurements for state estimation.This multi-sensor design is intended to improve robustness in challenging environments and sensor-failure cases.
  • Results: Experiments report robustness during aggressive motion, sensor failure, narrow tunnel-like operation, and environments containing moving objects.The system is also reported to outperform state-of-the-art LiDAR-inertial and visual-inertial estimators in overall performance.
  • System overview: A high-rate filter-based odometry uses an error-state iterated Kalman filter, while factor-graph optimization refines visual landmarks and local keyframe poses.The two-part design combines real-time estimation with local map refinement.
  • Results: The method reconstructs dense, high-precision indoor-outdoor 3D maps at large scale, including an HKU building map.The paper also reports open-sourcing the system for the robotics community.
  • Related work: Tightly coupled LiDAR-inertial-visual methods are presented as more robust and accurate than approaches using fewer sensor modalities.The paper positions its system against loosely coupled and LiDAR-inertial alternatives.

B. Continuous-time kinematic model

R2LIVE models rigidly attached IMU, LiDAR, and camera sensors with calibrated timing and extrinsics, using IMU dynamics and an error-state iterated Kalman filter for propagation. IMU-rate discretization supplies predictions between LiDAR or camera updates.

  • The system assumes precalibrated inter-sensor time offsets, known LiDAR–IMU extrinsics, and online estimation of camera–IMU extrinsics.
  • The input sequence uses 200 Hz IMU, 20 Hz camera, and 10 Hz LiDAR measurements, with i indexing IMU data and k indexing LiDAR or camera measurements.
  • IMU kinematics model global position, velocity, attitude, accelerometer bias, and gyroscope bias, with measurement noise and bias random walks.
  • IMU measurements are discretized at the IMU rate under a zero-order-hold assumption.
  • The error-state iterated Kalman filter propagates state and covariance after each IMU measurement until the next LiDAR or camera measurement.

E. The prior distribution

Before each new LiDAR or camera update, propagated state and covariance define a prior. The filter initializes a MAP estimate from this prior and iteratively refines it in the error space.

  • The propagated state estimate and covariance impose a prior distribution for the next state before its LiDAR or camera measurement is fused.
  • The MAP estimate is initialized with the propagated prior estimate and refined iteratively because the estimation problem is nonlinear.
  • The prior in the state variable is transformed into an equivalent prior over the current estimate’s error state before posterior optimization.

G. LiDAR measurement

LiDAR updates extract motion-compensated planar features and constrain the state through point-to-plane residuals. Camera updates track or triangulate visual features and use reprojection residuals from visual landmarks.

  • LiDAR measurement: For LiDAR frames, the method extracts planar feature points from raw scans and compensates their in-frame motion.
  • LiDAR measurement: Each LiDAR feature is transformed into the global frame, where nearby planar map features define a plane constraint and measurement residual.
  • LiDAR measurement: LiDAR measurement noise is incorporated by relating noisy observations to true point locations and enforcing zero residual for the true state.
  • Visual measurement: For camera frames, FAST corners are extracted, KLT optical flow tracks features through the sliding-window keyframes, and new or lost features are triangulated.
  • Visual measurement: Visual landmark–feature correspondences provide reprojection residuals for updating the current state estimate under a pin-hole projection model.

I. Update of error-state iterated Kalman filter

The filter combines the propagated prior with LiDAR and visual posterior information in a joint MAP update. Iteration continues to convergence, after which the state supports mapping, landmark triangulation, and the next propagation cycle.

  • Update of error-state iterated Kalman filter: The MAP update combines the prior distribution with posterior information from LiDAR and visual measurements.
  • Update of error-state iterated Kalman filter: LiDAR and camera measurements need not arrive simultaneously, so either sensor’s residual contribution can be absent from an update.
  • Update of error-state iterated Kalman filter: The measurement residuals and noise covariances are assembled into the joint update, whose Kalman gain is then used to update the state estimate.
  • Update of error-state iterated Kalman filter: The update iterates until the state change falls below a threshold, then projects new LiDAR points into the map, triangulates keyframe landmarks, and starts the next propagation.

V. FACTOR GRAPH OPTIMIZATION

The factor graph optimization refines camera poses and visual landmarks in a sliding window, while incorporating LiDAR and IMU constraints to improve local-map quality and calibrate camera–LiDAR-IMU timing.

  • The optimization refines camera poses and visual landmarks within a sliding window of image keyframes.It addresses low-precision triangulation caused by keyframe pose-estimation error.
  • The factor graph jointly improves visual landmarks and keyframe poses while calibrating the camera–LiDAR-IMU time offset.
  • LiDAR pose constraints and IMU preintegration connect the LiDAR and camera factors in the factor graph.The LiDAR poses are fixed and raw LiDAR points are excluded to keep optimization lightweight.

B. Experiment-1: Robustness evaluation with aggressive motion and sensor failure

Experiments evaluate R2LIVE under aggressive motion, sensor failures, narrow pedestrian-filled tunnels, and large-scale mapping conditions, with results indicating robust, accurate, real-time performance.

  • Robustness evaluation with aggressive motion and sensor failure: A maximum angular speed of 300°/s was used while camera and LiDAR measurements were intentionally blocked to simulate sensor failures.
  • Robustness evaluation with aggressive motion and sensor failure: The estimated trajectory closely tracked actual motion during severe rotation and translation, with no noticeable degradation when camera or LiDAR measurements were unavailable.
  • Robustness evaluation in a narrow tunnel-like environment: The Hong Kong MTR experiment used a tunnel up to 190 meters long with moving pedestrians, challenging both LiDAR- and camera-based SLAM.
  • Robustness evaluation in a narrow tunnel-like environment: R2LIVE achieved the best overall performance against VINS-Mono and Fast-LIO in the narrow tunnel experiment.
  • High precision maps building in large-scale indoor & outdoor urban environment: After traversing 876 meters without loop-closure processing, the system closed the loop and produced dense, high-precision indoor-outdoor maps.
  • Computation and real-time performance: The average running-time results show that R2LIVE achieves real-time performance on both desktop and embedded platforms.The factor graph optimization runs on a separate, lower-rate thread.

B. The computation of H

This subsection presents the mathematical computation of H through the inverse right Jacobian of SO(3).

  • The matrix A is identified as the inverse right Jacobian matrix of SO(3), used in the computation of H.
  • The inverse right Jacobian is expressed using the rotation vector and its norm.

C. The computation of Hlj “

This subsection gives the computation of Hlj and directs readers to supplementary material for its detailed derivation.

  • The computation of Hlj uses the matrix expression involving G, R, P, and identity blocks shown in the subsection.
  • The detailed derivation of Hlj is provided in Section D of the supplementary material.

D. The computation of Hcs and FPs

This section develops the computation of Hcs and FPs using SO(3) perturbation approximations, right-Jacobian relations, and camera projection parameters. Detailed derivations are deferred to supplementary Section E.

  • Camera model: The camera model uses focal lengths fx and fy and principal-point offsets cx and cy.The associated 3 × 3 matrices MA, MB, and MC are introduced for the computation.
  • Derivation scope: The detailed derivation of Hcs and FPs is provided in supplementary Section E.The main text directs readers to the supplementary material for the full computation.
  • SO(3) perturbation: The derivation uses a small-perturbation approximation on SO(3).The approximation is stated for perturbations δr approaching zero.
  • SO(3) perturbation: Jr(r) and Jr^-1(r) are identified as the right Jacobian and inverse right Jacobian of SO(3).These Jacobians support the subsequent perturbation derivations.

B. Computation of Fδx and Fw

This section derives transition and measurement-related Jacobian terms through perturbation approximations and matrix substitutions. It also relates measured inertial quantities to bias- and noise-corrected angular velocity and acceleration.

  • Jacobian derivation: The derivation combines earlier equations with small-perturbation approximations to obtain the required Jacobian terms.The text explicitly applies these approximations before completing the computation.
  • Inertial correction: Measured angular velocity and acceleration are corrected using gyroscope and accelerometer biases and noise terms.The relations are given as ωi = ω̂i − δbgi − ngi and ai = âi − δbai − nai.
  • Matrix construction: The computation introduces Pa and uses it to form a block matrix containing rotational and state-transition terms.The supplied expressions define Pa and include a 3 × 3 rotational block alongside a 3 × 15 block.
  • Camera projection: The camera projection function PC(x̂k+1, GP_s) is differentiated with respect to a perturbed state.The derivation uses the approximation PC(x̂k+1 + δx̂k+1, GP_s) before substituting the resulting expressions.
  • Camera projection: The camera model again uses focal lengths fx and fy together with principal-point offsets cx and cy.These parameters define the image-plane projection used in the Jacobian computation.
Loading 2102.12400v1…