R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package

In plain words

R3LIVE is a rig with three sensors: a LiDAR that measures distances to surfaces, an inertial sensor (IMU) that measures motion, and a camera. It works in two linked halves. The first half uses the LiDAR and the IMU to estimate where the rig is and to build a 3D map of the shapes around it. The second half uses the camera to colour that map. Each map point stores a colour, and the system moves its pose estimate until the stored colours match what the camera sees where those points should appear. Because it compares raw colours rather than looking for distinctive corners, it can still track on bare white walls, where feature-based methods struggle. The result is a dense, coloured point cloud that other 3D tools can mesh and use. The authors report results on their own campus and seaport recordings. They also timed the system on a drone's onboard computer.

TL;DR

R3LIVE is an open-source LiDAR-inertial-visual odometry and mapping system. A LiDAR-inertial odometry (LIO) builds the geometry of a global map, and a visual-inertial odometry (VIO) colours the map points by minimising the photometric error of tracked points, with no visual features needed §I. The authors report drift of 0.093 to 0.164 m in translation and 0.285 to 3.925 deg in rotation over four campus trajectories of 1191 to 1524 m tab. II. The official code is GPL-2.0, so this card cites it by path and line only.

Problem

LiDAR-based SLAM fails where the scene has too few geometric features, and solid-state LiDARs with a limited field of view are especially exposed §I. Camera-based methods struggle on texture-less surfaces. Earlier LiDAR-visual fusions either use the visual part loosely coupled or depend on extracted visual features §I. R3LIVE targets real-time localisation, 3D mapping and map colour rendering from a tightly fused LiDAR, IMU and camera §I. The LiDAR is assumed to be synchronised with the IMU, while the time offset between IMU and camera is estimated online §III-A.

Method

System overview. Two subsystems share one global map. The LIO builds the geometry by minimising point-to-plane residuals in an error-state iterated Kalman filter (ESIKF). The VIO renders the texture: it updates the state with frame-to-frame reprojection residuals, then refines it with frame-to-map photometric residuals, and finally renders colours into the map §II §V.

State and map.

• The full state has 29 dimensions: rotation, position and velocity in the global frame, gyroscope and accelerometer biases, gravity, camera-to-IMU rotation and translation, the IMU-camera time offset, and the camera intrinsics (fx, fy, cx, cy) §III-A.
• The map is made of voxels that hold points. The paper gives 0.1 m as the example voxel size. A voxel is "activated" if points were appended to it in the recent window, which the paper states as 1 s §III-B1.
• A map point is a 6-vector of 3D position and RGB colour. Each point also keeps covariances for position and colour, plus the times it was created and last rendered §III-B2.

Procedure.

1. Per LiDAR scan, LIO removes motion distortion by back-propagating IMU measurements, runs an ESIKF on point-to-plane residuals, appends the scan to the map and marks its voxels §IV. The paper refers to its earlier work for the full LIO details §IV.
2. Per image, the state is propagated with the IMU to the image time. Lucas-Kanade optical flow moves the tracked map points into the current image §V-A.
3. Frame-to-frame update: the projection residual of each tracked point is stacked and solved with the ESIKF, iterated until convergence §V-A1 §V-A2.
4. Frame-to-map update: the photometric residual between the stored map colour and the colour interpolated at the predicted pixel is stacked and solved the same way §V-B1 §V-B2.
5. Rendering: each map point in the activated voxels that falls in the image gets its colour fused with the observed colour by a Bayesian update §V-C.
6. Tracked-point set update: points with large residuals or outside the image are removed, and map points with no tracked neighbour are added. The paper gives a 50-pixel radius as an example of "nearby" §V-D.

# Per LiDAR scan: geometry update  [§IV]
# Input:  scan (points), imu (samples), state, global_map
# Output: state, global_map
def lio_step(scan, imu, state, global_map):
    # Remove motion distortion by IMU backward propagation  [§IV]
    scan = undistort_by_imu(scan, imu)
    # Point-to-plane ESIKF against the map  [§IV]
    state = esikf_point_to_plane(state, scan, global_map)
    # Append scan points and mark their voxels as activated  [§III-B1]
    global_map = append_points(global_map, scan, state)
    return state, global_map

# Per image: texture update  [§V]
# Input:  image, state, global_map, tracks (points tracked from last frame)
# Output: state, global_map, tracks
def vio_step(image, state, global_map, tracks):
    # Propagate the state to the image time with the IMU  [§V-A]
    state = imu_propagate(state, image.time)
    # Track map points into the image with Lucas-Kanade  [§V-A]
    tracks = track_lucas_kanade(tracks, image)
    # Reject tracking outliers by fundamental-matrix RANSAC (code only)
    tracks = reject_fundamental_ransac(tracks)
    # Frame-to-frame update on the reprojection residuals  [§V-A2]
    state = esikf_update(state, tracks, pnp_residual, image)
    # Frame-to-map update on the photometric residuals  [§V-B2]
    state = esikf_update(state, tracks, photometric_residual, image)
    # Fuse the observed colours into the map  [§V-C]
    global_map = render_colours(global_map, image, state)
    # Add map points that have no tracked neighbour yet  [§V-D]
    tracks = refill_tracks(tracks, global_map, image)
    return state, global_map, tracks

# Predicted pixel of a map point  [§V-A1]
# Input:  point_world (3,), state, pixel_velocity (2,) from the last frame
# Output: pixel (2,)
def project(point_world, state, pixel_velocity):
    # Move the point into the camera frame  [§V-A1]
    point_cam = camera_from_world(point_world, state)
    # Pinhole model with the estimated intrinsics  [§V-A1]
    pixel = pinhole(point_cam, state.intrinsics)
    # Add the time-offset correction on the pixel velocity  [§V-A1]
    return pixel + state.time_offset * pixel_velocity

# Residual of the frame-to-frame update  [§V-A1]
def pnp_residual(state, point, image):
    # Tracked pixel minus predicted pixel  [§V-A1]
    return point.pixel - project(point.position, state, point.pixel_velocity)

# Residual of the frame-to-map update  [§V-B1]
def photometric_residual(state, point, image):
    # Predicted pixel of the point in the current image  [§V-B1]
    pixel = project(point.position, state, point.pixel_velocity)
    # Colour seen there, interpolated from neighbouring pixels  [§V-B1]
    observed = bilinear_colour(image, pixel)
    # Stored map colour minus observed colour  [§V-B1]
    return point.colour - observed

# Iterated ESIKF update, shared by both VIO steps  [§V-A2] [§V-B2]
# Input:  prior (IMU-propagated state), tracks, residual_fn, image
# Output: updated state
def esikf_update(prior, tracks, residual_fn, image):
    # Start the iteration from the IMU-propagated state  [§V-A2]
    state = prior
    for iteration in range(max_iterations):
        # Stack residuals and Jacobians over all tracked points  [§V-A2]
        H, z = stack_residuals(state, tracks, residual_fn, image)
        # Measurement noise of each point  [§V-A2] [§V-B2]
        R = block_diagonal(noise_per_point(tracks))
        # Prior covariance mapped to the tangent space  [§V-A2]
        P_prior = inv(proj_jac) @ sigma_prior @ inv(proj_jac).T
        # Kalman gain  [§V-A2]
        K = inv(H.T @ inv(R) @ H + inv(P_prior)) @ H.T @ inv(R)
        # Error-state correction on the manifold  [§V-A2]
        correction = (
            -K @ z - (I - K @ H) @ inv(proj_jac) @ boxminus(state, prior)
        )
        state = boxplus(state, correction)
        # Stop when the correction is small; threshold not stated  [§V-A2]
        if norm(correction) < threshold:
            break
    return state

# Bayesian colour fusion for one map point seen in the image  [§V-C]
# Element-wise over the three colour channels
def fuse_colour(point, observed, observed_var, elapsed):
    # Inflate the stored variance by the random-walk term  [§V-B1]
    prior_var = point.colour_var + sigma_sq * elapsed
    # Precision-weighted combination of stored and observed colour  [§V-C]
    fused_var = 1 / (1 / prior_var + 1 / observed_var)
    point.colour = fused_var * (
        point.colour / prior_var + observed / observed_var
    )
    point.colour_var = fused_var

Key equations, without numbers (the paper's equation numbers are not used here):

\mathbf{r} = \boldsymbol{\rho}_{k} - \boldsymbol{\pi}({}^{C}\mathbf{p}, \check{\mathbf{x}}_k) \qquad [\S\text{V-A1}]

\boldsymbol{\pi} = \big[f_x \tfrac{{}^{C}p_x}{{}^{C}p_z} + c_x,\; f_y \tfrac{{}^{C}p_y}{{}^{C}p_z} + c_y\big]^T + \tfrac{{}^{I}t_C}{\Delta t}\,(\boldsymbol{\rho}_{k} - \boldsymbol{\rho}_{k-1}) \qquad [\S\text{V-A1}]

\mathbf{o} = \mathbf{c}_s - \boldsymbol{\gamma}_s \qquad [\S\text{V-B1}]

\tilde{\mathbf{c}}_s = \tilde{\Sigma}\,\big(\Sigma_{c}'^{-1}\mathbf{c}_s + \Sigma_{\gamma}^{-1}\boldsymbol{\gamma}_s\big), \quad \tilde{\Sigma} = \big(\Sigma_{c}'^{-1} + \Sigma_{\gamma}^{-1}\big)^{-1}, \quad \Sigma_c' = \Sigma_{c} + \sigma_s^2 \Delta t \qquad [\S\text{V-C}]

Read literally, the printed colour update takes the inverse of the weighted sum before multiplying by the covariance. The precision-weighted form above is what the code implements, so it is used here; see Pitfalls.

Hyperparameters

Items the paper does not state are marked "not stated". The code values come from the repository at code_commit, cited by path and line.

Results

Author-reported. Setup: the authors' own handheld rig (LiDAR, camera, IMU), with an ArUco board as reference in Experiments 1 and 2 and a D-GPS RTK system in Experiment 3 §VI-A. The data are the authors' own recordings. Baselines are LVI-SAM (with a modified LiDAR front end), R2LIVE, FAST-LIO2 and VINS-Mono, all compared by the authors §VI-D.

Experiment 1, LiDAR-degenerate and texture-less passage. A narrow T-shaped passage, with the sensor sometimes facing bare white walls. Drift at the end of the trajectory, measured with an ArUco board: 1.62 deg rotation and 4.57 cm translation §VI-B fig. 8.

Experiment 2, four campus trajectories. Drift at the ArUco start point tab. II:

Each trajectory closes its loop without loop-closure processing §VI-C.

Experiment 3, relative pose error against RTK ground truth. Two sequences in a seaport with many pedestrians and open sea §VI-D. RRE in deg / RTE in % tab. III:

Sequence (a):

Sequence (b):

HiRes uses 1280 x 1024 images and 0.01 m point spacing; RT uses 320 x 256 and 0.10 m tab. III caption.

Run time, average per frame tab. IV. Times in ms. "PC" is the desktop; "OB" is the onboard UAV computer.

The LIO takes 18.40 ms per frame on PC and 39.56 ms on OB tab. IV.

Applications. Maps export as pcd, ply and obj files, and the authors meshed them offline with CGAL §VII-A. They also used the maps in AirSim and in game prototypes §VII-B. These application results are qualitative.

Paper vs code

Paths are relative to the repository root at code_commit. The code is GPL-2.0, so it is cited by path and line only.

• Frame-to-frame ESIKF. The paper iterates until convergence §V-A2. The code runs esikf_iter_times iterations, default 2 r3live_vio.cpp r3live.hpp.
• Frame-to-map ESIKF. The paper gives no iteration count §V-B2. The code runs a fixed loop of 2 iterations and ignores esikf_iter_times r3live_vio.cpp. It stops early when the mean error is below 10 or the change is below 0.01 r3live_vio.cpp r3live_vio.cpp. The paper states neither rule.
• Robust loss. The paper uses a plain squared Mahalanobis cost §V-A2 §V-B2. The code applies a Huber scale to every residual in both updates r3live_vio.cpp r3live_vio.cpp r3live_vio.cpp.
• Prior weighting. The paper's prior is the IMU-propagated covariance §V-A2. The code scales the state covariance by a clamped measurement weight in the gain r3live_vio.cpp r3live_vio.cpp and sets that weight from the number of newly visited voxels r3live_vio.cpp.
• Observed colour variance. The paper derives it from the interpolation §V-B1. The code uses a constant 15 per channel pointcloud_rgbd.cpp.
• Random walk. The paper adds sigma_s^2 times elapsed time to a variance §V-B1. The code adds 0.1 times elapsed time to a standard deviation pointcloud_rgbd.cpp.
• Observation gate. The code skips an observation taken from more than 1.2 times the distance recorded for that point, a rule the paper does not mention pointcloud_rgbd.cpp.
• Track spacing. The paper uses a 50-pixel neighbourhood §V-D. The code rounds tracked pixel positions to a grid of size track_windows_size (default 40) divided by the scale factor rgbmap_tracker.cpp r3live_vio.cpp. This is a grid, not a circular radius.
• Outlier rejection after tracking. The paper removes points whose residual is too large §V-D. The code calls track_img with -20, and its residual rejection runs only when the argument is positive, so it is skipped r3live_vio.cpp rgbmap_tracker.cpp. A separate PnP RANSAC outlier step does run r3live_vio.cpp.
• Fundamental-matrix RANSAC. The paper does not mention it. The code runs it on tracked points rgbmap_tracker.cpp.
• Activation window. The paper uses 1 s §III-B1. The code default is 0.0, which clears the recent-voxel set on each call r3live.hpp pointcloud_rgbd.cpp. The shipped config does not set it.
• Config matches HiRes. The shipped config uses 1280 x 1024 images and 0.01 m point spacing r3live_config.yaml r3live_config.yaml, so it reproduces the HiRes row, not RT.
• LIO point stride. The config uses every 4th point r3live_config.yaml, the code default is every point r3live.hpp, and the paper does not state it.
• Drift summary. The introduction gives "0.16 m and 3.9 degrees after up to 1.5 km" §I. Those are the largest values in tab. II, taken from different trajectories (translation from Traj-3, rotation from Traj-4).
• Parameters the paper omits. Initial covariances, camera intrinsics and extrinsics are not in the paper. The code and config supply them r3live_vio.cpp r3live_config.yaml.

Pitfalls

• Activation window. The paper's 1 s window does not match the code's default of 0. An implementation that follows the paper must keep recent voxels for 1 s itself; the shipped code does not.
• Track spacing. Implement the 50 px rule as the paper states it, or follow the code's grid. The two give different point densities.
• Random-walk units. The paper's sigma_s^2 is a variance; the code adds to a standard deviation. The paper does not give a value for sigma_s; the code uses 0.1.
• Colour update formula. Read literally, the printed formula inverts the weighted sum and then multiplies by the covariance. The code uses the standard precision-weighted mean instead pointcloud_rgbd.cpp.
• Convergence. Neither the paper nor the code gives a convergence threshold that the paper states. The code uses 0.01 on the change and a mean error of 10, and runs a fixed 2 iterations in the photometric step.
• Robust loss. The Huber term changes the effective weights of outliers. The paper does not mention it, so an implementation that omits it will behave differently on noisy tracks.
• Minimum observations. Points with fewer than 3 colour observations are skipped in the photometric step r3live_vio.cpp.
• Photometric dependency on depth. The VIO projects points from the global map that the LIO builds §II §V. The paper does not say whether the VIO can run without the LIO, so a VIO-only port needs its own depth source.
• Pixel-velocity term. The time-offset correction multiplies the pixel velocity from the last frame §V-A1. If the time offset is wrong, this term biases the projection.
• Cost on the drone computer. The VIO time grows with image size and with smaller point spacing. On the onboard computer it reaches 166 ms at 1280 x 1024 and 0.01 m tab. IV.
• Licence. The code is GPL-2.0 copyleft. Copying code from it into a project brings the GPL terms with it, so implement from the pseudocode and cite the path.
• Not in the paper. The LIO details, including the point-to-plane residual, are deferred to the authors' earlier work §IV, so this card cannot specify them beyond what is written above.

Lineage

• Builds on: R2LIVE, the authors' previous LiDAR-inertial-visual system, for the ESIKF framework and the LIO design §I §IV. The LIO is based on FAST-LIO §IV.
• Compared against: LVI-SAM, R2LIVE, FAST-LIO2 and VINS-Mono §VI-D.
• Superseded by: not stated.