LiDAR-inertial odometry

Overview

LiDAR-only odometry degenerates where there are few geometric features, which matters most for small-field-of-view LiDARs 2102.12400 §I. Long corridors and open fields are the usual examples 2104.10831 §1. An IMU alone drifts when trusted by itself, so the two sensors are combined.

The shared idea: the IMU propagates the state between scans, and each LiDAR scan is matched against a local or global map to correct that prediction. The scan enters either as a filter update, as in R2LIVE 2102.12400 §IV-D 2102.12400 §IV-I, and R3LIVE 2109.07982 §IV, or as a constraint in a factor graph, as in the LiDAR-inertial part of LVI-SAM 2104.10831 §2.3. All three cards describe systems that also use a camera. The variants below are therefore compared on their LiDAR-inertial parts where the paper gives them. For R3LIVE, the reported numbers belong to the full LiDAR-inertial-visual system.

Variants

Iterated error-state Kalman filter (R2LIVE)

The state holds the IMU pose, velocity, the camera-IMU extrinsic and the gyro and accelerometer biases. Between sensor events it is propagated through the IMU readings 2102.12400 §IV-C. At each LiDAR frame the point-to-plane residuals against nearby map planes are linearized and fused in an iterated MAP update 2102.12400 §IV-G 2102.12400 §IV-I. The error state lives on the manifold's tangent space 2102.12400 §IV-A. The authors report a median relative pose error of 0.27° and 0.21% on a 300 m D-GPS sequence, against 0.41° and 0.34% for FAST-LIO 2102.12400 fig. 11. Card: R2LIVE.

Iterated error-state Kalman filter with a global colour map (R3LIVE)

R3LIVE splits the work into a LiDAR-inertial odometry (LIO) and a visual-inertial odometry (VIO). The LIO minimises point-to-plane residuals in an error-state iterated Kalman filter and appends each scan to a global map of voxels 2109.07982 §II 2109.07982 §III-B1. The paper refers to the authors' earlier work for the LIO equations, so the card does not specify them 2109.07982 §IV. The VIO then colours the map. It runs a reprojection update on tracked points, then a photometric update against the stored map colours, and fuses the observed colours into each map point 2109.07982 §V-A 2109.07982 §V-B 2109.07982 §V-C. The VIO projects points from the map that the LIO builds, so it needs the LIO to run first 2109.07982 §II.

The authors report translation drift of 0.093 to 0.164 m and rotation drift of 0.285 to 3.925° over four campus trajectories of 1191 to 1524 m, with no loop-closure processing 2109.07982 tab. II. In a seaport test on a 300 m sequence, R3LIVE's high-resolution setting is reported at 0.21° and 0.17% 2109.07982 tab. III. The LIO takes 18.40 ms per frame on a desktop and 39.56 ms on the onboard UAV computer 2109.07982 tab. IV. Card: R3LIVE.

Factor graph with scan matching (LVI-SAM's LiDAR-inertial system)

LVI-SAM adapts LIO-SAM. Its factor graph holds IMU preintegration factors, lidar odometry from scan matching, visual odometry and loop closure 2104.10831 §2.3. A new keyframe is added when the pose has changed enough, and the feature map is a sliding window of keyframes, which bounds the cost 2104.10831 §2.3. The graph is solved with iSAM2 2104.10831 §2.1. Scan matching gives a degeneracy flag from the smallest eigenvalue of AᵀA 2104.10831 §2.3.2. Loop candidates come from DBoW2 and are verified by ICP 2104.10831 §2.2.4 2104.10831 §2.3. On its Jackal dataset with loop closure, LVI-SAM reports 0.67 m RMSE against GPS, the lowest of the systems compared there 2104.10831 tab. 2. Card: LVI-SAM.

When to use which

Implementation sketch

Pseudocode in our own words. The common core is one motion propagation and one scan correction. The variants differ in how the correction is applied and in how the map is kept.

# Common core of one LiDAR-inertial step  [arxiv:2102.12400 §IV-D]
# Propagate the state through the IMU readings since the last scan
state, cov = propagate_imu(state, cov, imu_since_last_scan)

# Motion-compensate the scan with the propagated state  [arxiv:2102.12400 §IV-B]
points = deskew(scan, imu_since_last_scan)

# Filter variant: iterate linearize-and-update at this scan time
#   [arxiv:2102.12400 §IV-G] [arxiv:2102.12400 §IV-I]
for _ in range(max_iterations):    # cap not stated in the paper
    residuals, H = point_to_plane(points, state, local_map)
    state, cov = kalman_update(state, cov, residuals, H)
    if update_is_small(state):     # threshold not stated in the paper
        break

# Map variant: append the scan to a global voxel map  [arxiv:2109.07982 §III-B1]
# global_map = append_points(global_map, points, state)

# Graph variant: add factors instead of updating a filter
#   [arxiv:2104.10831 §2.3]
# add_imu_preintegration_factor(graph, keyframe_prev, keyframe_new)
# add_lidar_odometry_factor(graph, scan_match_pose)  # see degeneracy below
# optimize_incrementally(graph)                      # iSAM2 [arxiv:2104.10831 §2.1]

Points to decide when implementing either variant:

• Degeneracy. LVI-SAM's text drops the lidar-odometry factor when a degenerate direction is detected 2104.10831 §2.3.2. Its released code keeps the factor and projects out the degenerate directions instead. Pick one behaviour on purpose.
• Iteration and stop rules. The paper does not state a cap or stop threshold for the filter update 2102.12400 §IV-I. The R2LIVE code uses 4 LiDAR iterations, which is a code value and not a paper value. The R3LIVE paper also gives no convergence threshold for its iterated updates 2109.07982 §V-A2.
• Map lifetime. R3LIVE keeps a global map and marks voxels active for a recent window, which its paper gives as 1 s 2109.07982 §III-B1. The card records that the shipped code defaults to 0.0 and does not set it, so an implementation that follows the paper must keep that window itself.
• Noise densities. R2LIVE does not state its IMU noise values, and its covariance input Q is not defined in the paper 2102.12400 eq. 7. Take them from your own sensor calibration.

Contradictions

No single result is contradicted, but two comparisons point in different directions. Each side is cited, and the disagreement is not resolved here.

• R2LIVE against FAST-LIO on a 300 m sequence. The R2LIVE paper reports R2LIVE at 0.27° and 0.21%, against 0.41° and 0.34% for FAST-LIO 2102.12400 fig. 11. The R3LIVE paper, on its own seaport sequence (a), reports R2LIVE at 0.34° and 0.21%, and FAST-LIO2 at 0.21° and 0.20% 2109.07982 tab. III. The datasets, the baseline version (FAST-LIO against FAST-LIO2) and the error definitions differ, so the two tables do not test the same thing.
• LVI-SAM on its own data against LVI-SAM as a baseline. The LVI-SAM paper reports 0.67 m RMSE against GPS on Jackal 2104.10831 tab. 2. The R3LIVE paper reports LVI-SAM at 0.43° and 2.40% on its seaport sequence (a) 2109.07982 tab. III, and it uses a modified LiDAR front end for that baseline 2109.07982 §VI-D. The metrics differ, so the numbers are not comparable.
• Ablation text against table in LVI-SAM. The text claims that depth reduces error in ablation A3 2104.10831 §3.1.3. The rotation error in that row rises from 6.41° to 7.66° 2104.10831 tab. 1.

Papers

• R2LIVE (Jiarong Lin et al., arXiv:2102.12400): an iterated error-state Kalman filter with a sliding-window back end, tested in a tunnel and a campus building.
• R3LIVE (Jiarong Lin and Fu Zhang, arXiv:2109.07982): a LiDAR-inertial odometry plus a visual-inertial colouring stage on a global voxel map, tested on campus, seaport and UAV runs.
• LVI-SAM (Tixiao Shan et al., arXiv:2104.10831): a factor graph (iSAM2) that combines a visual-inertial and a LiDAR-inertial system, with DBoW2 loop closure verified by ICP.