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

In plain words

A robot or handheld rig that moves through a building needs to know where it is and how it is turning. Three sensors help with that. A spinning or solid-state laser scanner (LiDAR) measures distances to surfaces, but fails in long, bare corridors where every wall looks the same. A camera sees texture, but fails when the image is blurred, dark or blocked. An inertial sensor (IMU) measures acceleration and rotation very often, but drifts if it is trusted alone.

R2LIVE uses all three at once. It predicts the motion from the inertial sensor at every sample and corrects that prediction with each laser scan and each camera frame. This keeps the estimate smooth at high rate. A second, slower stage then tunes the recent camera poses and the 3D points seen by the camera, so the points line up better. The authors tested it in a long subway tunnel full of walking people, with the camera covered and with the laser covered, and at high rotation speeds. They report that it stayed usable in each case and beat FAST-LIO and VINS-Mono in their tests.

TL;DR

R2LIVE is a tightly coupled LiDAR-inertial-visual system with two parts: an error-state iterated Kalman filter (ESIKF) that runs odometry and updates on each LiDAR frame and camera frame §IV-I, and a sliding-window factor graph over image keyframes that refines visual landmarks, keyframe poses and the camera-LiDAR time offset §V. The IMU is propagated between measurements §IV-D. The paper reports real-time operation on an embedded computer (i7-8550U) and a desktop tab. I. The code is open under GPL-2.0 and the paper's numbers are author-reported. Read this card if you need a LiDAR-inertial-visual estimator design, the exact filter update, or to reproduce its update rules.

Problem

LiDAR-only odometry degenerates where there are few geometric features, which is most critical for small-field-of-view LiDARs §I. Prior work adds an IMU to LiDAR odometry §II-B, or also a camera §II-C. The paper targets a tightly coupled fusion of all three sensors that stays real-time on onboard hardware §I, §VI-F. Assumptions: the time offsets between the three sensors are pre-calibrated §IV-B; the LiDAR-IMU extrinsic is known §IV-B; LiDAR points within a frame are treated as measured at one instant after IMU-based motion compensation §IV-B.

Method

The system has two stages §III, fig. 2.

1. Filter-based odometry (front end, §IV). The state holds the IMU pose, velocity, the camera-IMU extrinsic, and the gyro and accelerometer biases §IV-C. Between sensor events, the state is propagated with the IMU readings held constant over each sample period (zero-order hold) §IV-C. The error state has 21 dimensions and lives on the tangent space of the manifold, with the paper's boxplus and boxminus operators §IV-A, eq. 1 §IV-D, eq. 4. At each LiDAR frame or camera frame the propagated prior is fused with the measurements in an iterated MAP update §IV-E, §IV-F, eq. 8-11.

2. Factor graph optimization (back end, §V). Visual landmarks triangulated in the front end are noisy because keyframe pose error propagates into them §V. The back end optimizes camera poses and landmark positions in a sliding window of image keyframes, adds IMU preintegration factors between keyframes, and adds pose constraints from the LiDAR. LiDAR poses stay fixed and raw LiDAR points are not used in the graph §V. The graph is similar to VINS-Mono's, and it also estimates the camera-LiDAR time offset §V.

Continuous model. The IMU kinematics use gravity in the global frame, and the biases follow a random walk §IV-B, eq. 2:

\begin{aligned} {}^G\dot{\mathbf p}_I &= {}^G\mathbf v_I, \\ {}^G\dot{\mathbf v}_I &= {}^G\mathbf R_I(\mathbf a_m - \mathbf b_a - \mathbf n_a) + {}^G\mathbf g, \\ {}^G\dot{\mathbf R}_I &= {}^G\mathbf R_I\,[\boldsymbol\omega_m - \mathbf b_g - \mathbf n_g]_\times, \\ \dot{\mathbf b}_g &= \mathbf n_{bg}, \qquad \dot{\mathbf b}_a = \mathbf n_{ba} \end{aligned} eq. 2

Discrete propagation. The nominal state advances by \hat{\mathbf x}_{i+1} = \hat{\mathbf x}_i \boxplus (\Delta t\, \mathbf f(\hat{\mathbf x}_i, \mathbf u_i, \mathbf 0)) eq. 5 and the error covariance by \boldsymbol\Sigma_{i+1} = \mathbf F_{\delta\hat x}\boldsymbol\Sigma_i \mathbf F_{\delta\hat x}^T + \mathbf F_{\mathbf w}\mathbf Q \mathbf F_{\mathbf w}^T eq. 7. The Jacobians are in app. A of the paper. The paper's text does not define \mathbf Q anywhere; see Pitfalls.

Prior. Propagation stops at the next LiDAR or camera time k+1. The prior is \mathbf x_{k+1} \boxminus \hat{\mathbf x}_{k+1} \sim \mathcal N(\mathbf 0, \boldsymbol\Sigma_{\delta\hat x_{k+1}}) eq. 8. It is re-expressed around the current iterate \check{\mathbf x} with \mathcal H eq. 10, which gives a prior in terms of \delta\check{\mathbf x} eq. 11. \mathcal H is defined in app. B.

Measurements. LiDAR: each planar feature point gives a point-to-plane residual \mathbf r_l = \mathbf u_j^T(\,{}^G\mathbf p_j - \mathbf q_j), with the plane normal \mathbf u_j and a point on the plane \mathbf q_j taken from the nearest map points §IV-G, eq. 12. Its Jacobian is in app. C and its noise term is in eq. 15. Camera: each tracked corner gives a reprojection residual \mathbf r_c = {}^C\mathbf p_s - \boldsymbol\pi({}^C\mathbf P_s), where \boldsymbol\pi is the pinhole projection §IV-H, eq. 16. The corner is tracked with KLT optical flow and FAST corners §IV-H.

Iterated update. The MAP estimate of \delta\check{\mathbf x} minimizes the prior term plus the LiDAR and camera terms eq. 21. The gain is \mathbf K = (\mathbf H^T \mathbf R^{-1}\mathbf H + \mathbf P^{-1})^{-1}\mathbf H^T \mathbf R^{-1} eq. 23, and the iterate is updated as \check{\mathbf x} \leftarrow \check{\mathbf x} \boxplus(-\mathbf K\check{\mathbf z} - (\mathbf I - \mathbf K\mathbf H)\mathcal H^{-1}(\check{\mathbf x}\boxminus\hat{\mathbf x})) §IV-I. The loop repeats until the update is small §IV-I.

# Propagate the state on each IMU sample  [§IV-D] [eq. 5]
def propagate_imu(state, cov, imu_samples, dt):
    for gyro, accel in imu_samples:
        # Mean moves with the zero-noise dynamics  [§IV-D] [eq. 5]
        state = state_plus(state, dt * dynamics(state, gyro, accel))
        # Covariance grows with the state and noise Jacobians  [§IV-D] [eq. 7]
        jac_state = state_jacobian(state, gyro, accel)   # (21, 21)
        jac_noise = noise_jacobian(state, gyro)          # (21, 12)
        cov = jac_state @ cov @ jac_state.T + jac_noise @ noise_q @ jac_noise.T
    return state, cov

# Iterated error-state update at one LiDAR or camera time  [§IV-I] [eq. 21]
def esikf_update(prior_state, prior_cov, measurements):
    # Start the MAP iterate at the propagated prior  [§IV-F]
    iterate = prior_state
    for step in range(max_steps):    # max_steps is not stated in the paper
        # Linearize every measurement at the current iterate  [§IV-G] [§IV-H]
        residuals, H, R_inv = linearize(iterate, measurements)
        # Kalman gain  [§IV-I] [eq. 23]
        gain = solve(H.T @ R_inv @ H + inv(prior_cov), H.T @ R_inv)
        # Correction pulls the iterate toward the measurements and the prior
        # [§IV-I]; prior_jac is the paper's calligraphic H  [eq. 10]
        prior_error = boxminus(iterate, prior_state)
        step_vec = (-gain @ residuals
                    - (eye - gain @ H) @ solve(prior_jac, prior_error))
        iterate = boxplus(iterate, step_vec)
        # Convergence threshold is not stated in the paper  [§IV-I]
        if norm(step_vec) < stop_threshold:
            break
    # Posterior covariance  [§IV-I]
    posterior_cov = (eye - gain @ H) @ prior_cov
    return iterate, posterior_cov

Camera residual, per corner §IV-H eq. 16:

# Reprojection residual of one tracked corner  [§IV-H] [eq. 16]
def camera_residual(state, pixel, landmark_world):
    # Landmark in the camera frame through the IMU and the camera extrinsic
    # [§IV-H] [eq. 16]
    point_cam = (R_GI @ R_IC).T @ landmark_world \
        - R_IC.T @ p_GI - p_IC
    # Pinhole projection and the difference to the observed pixel  [§IV-H]
    return pixel - project_pinhole(point_cam)

Hyperparameters

Values stated in the paper:

Not stated in the paper:

Code values (r2live at commit e719ca2, default config config/filr_cam.yaml). These are not in the paper:

Results

Author-reported. Hardware: DJI manifold-2c (Intel i7-8550U, 8 GB) on the handheld rig §VI-A; desktop PC with Intel i7-9700K and 32 GB tab. I. Baselines: VINS-Mono (visual-inertial) and FAST-LIO (LiDAR-inertial), run on the same data §VI-C, §VI-E.

Experiment 4, quantitative accuracy against D-GPS RTK on a 300 m sequence, median relative pose error fig. 11:

Errors are computed over all subsequences of length 50 to 300 m §VI-E.

Run time (average per Experiment, ms, desktop / onboard). LI-Odom is the LiDAR-IMU filter, VI-Odom the visual-inertial filter and FG-OPM the factor graph tab. I:

The back end runs in a separate thread and may run at a lower rate §VI-F.

Qualitative results, not numeric:

• Exp-1, aggressive motion and sensor blocking: the trajectory stays smooth when the camera or the LiDAR is covered §VI-B, fig. 7, fig. 8.
• Exp-2, MTR tunnel up to 190 m with pedestrians: the map matches the station street map, and the paper reports the best overall trajectory among the three methods §VI-C, fig. 9. VINS-Mono stopped midway due to feature-tracking failure fig. 9 caption.
• Exp-3, HKU main building, 876 m: the trajectory closes its loop without any loop-closure step §VI-D, fig. 12.

Paper vs code

Official repository: hku-mars/r2live, linked from the abstract and used at commit e719ca2 §I footnote. The LICENCE file is GPL-2.0, so only the descriptions below are used, not code.

• Visual update iterations. The paper iterates until convergence §IV-I. The camera update runs a fixed two iterations, and its convergence flag is set but never read estimator_node.cpp, estimator_node.cpp.
• LiDAR update iterations. The LiDAR update runs up to max_iteration = 4 and also re-matches correspondences on convergence or on the second-to-last step fast_lio.hpp, fast_lio.hpp. The paper does not mention re-matching.
• Convergence test units. Both branches use the same thresholds of 0.01° and 0.015. The LiDAR branch scales the translation by 100 before comparing fast_lio.hpp. The camera branch does not estimator_node.cpp. Treat these as code choices, not paper values.
• Camera measurement weight. The paper weights the camera residual by its pixel noise covariance \boldsymbol\Sigma_{\beta} §IV-H, eq. 20, but gives no value. The code's gain is (\mathbf H^T\mathbf H + (10^{-3}\,\mathbf P)^{-1})^{-1}\mathbf H^T estimator_node.cpp. This equals the paper's gain eq. 23 with \mathbf R = 1000\,\mathbf I on the residual, so the prior carries more weight than the measurement.
• Visual acceptance gates. The code accepts a camera update only if the rotation change is under 3°, the translation change under 0.5 m and the mean reprojection error under 1.0 estimator_node.cpp. It also reverts to the pre-update state when the change exceeds 0.2 m or 2° estimator_node.cpp. The paper has no such gates.
• Covariance after the camera update. The paper's update gives \hat\Sigma = (\mathbf I - \mathbf K\mathbf H)\check\Sigma §IV-I. In the camera branch the stored covariance is set to the value before the update estimator_node.cpp, estimator_node.cpp. I did not find the covariance update applied there. The LiDAR branch does apply it fast_lio.hpp.
• LiDAR poses in the graph. The paper says LiDAR poses are fixed §V. The code adds a 15-dimensional LiDAR prior factor for each window state estimator_optimization.cpp. These are soft priors, not constants.
• Window size. The paper gives no size §V. The code uses 7 parameters.h.
• Extrinsic. The paper says the camera-IMU extrinsic is estimated online §IV-B. The default launch config fixes it filr_cam.yaml, so the default run does not do what the paper describes.
• Noise values. The paper gives none. The code's defaults in parameters.cpp differ from the yaml, and the yaml is what the default launch file loads parameters.cpp, filr_cam.yaml, demo.launch.
• Gravity. The paper leaves it unstated. The code uses 9.805 from the yaml filr_cam.yaml.
• Index. Eq. 3 writes the next state as t+1, while the rest of the derivation uses i eq. 3 eq. 5.

Pitfalls

• Undefined covariance input. \mathbf Q in eq. 7 is not defined anywhere in the LaTeX source. Take the IMU noise densities from your own sensor calibration. The code's yaml values are the authors' settings, not values the paper derives filr_cam.yaml.
• Iteration and stop rules. The paper gives no iteration cap or stop threshold §IV-I. An implementation must choose both. The code's caps are a reference: 4 on the LiDAR side and 2 on the camera side fast_lio.hpp, estimator_node.cpp.
• Camera measurement scale. The code's camera gain scales the prior by the state covariance times 1e-3 estimator_node.cpp. This is a relative weight, not a pixel noise, so the update strength moves with the state covariance.
• Gating decides whether the update is applied. Camera updates that fail the gates leave the state at its pre-update value estimator_node.cpp. An implementation that skips the gates will apply updates the authors rejected.
• Time offset. The paper estimates the camera-LiDAR time offset in the back end §V but gives no initial value. The code's default is 0.00 s filr_cam.yaml with online estimation on filr_cam.yaml.
• Back-end cost. Landmarks are triangulated from keyframe poses, which the paper says carry error §V. The paper reports the factor graph at 26 to 30 ms on the desktop and 45 to 65 ms onboard tab. I.
• Real-time claim. The paper reports real-time operation on the onboard i7-8550U with 8 GB §VI-F, tab. I. The paper says the factor graph runs on a separate thread at a lower rate §VI-F.
• Figure and table numbers. Figure and table numbers in this card follow the order in the LaTeX source. I did not compile the paper, so check them against the compiled PDF before relying on them. Equation numbers were derived by counting the numbered environments.
• Scope of tests. All experiments use the authors' handheld rig §VI-A. The paper does not test other sensor setups.

Lineage

• Builds on: FAST-LIO, the authors' earlier LiDAR-inertial filter, whose Kalman gain formula the paper uses §IV-I. VINS-Mono provides the factor-graph structure §V and the global-frame convention §IV-B. The error-state iterated filter follows He et al. §IV-D. Lidar feature extraction follows LOAM and FAST-LIO §IV-G.
• Compared against: VINS-Mono and FAST-LIO in Experiments 2 and 4 §VI-C, §VI-E. LIO-SAM, LINS, LIOM and LiLi-OM are described in §II without experiments against them.
• Superseded by: not known from the content of this wiki.