LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

In plain words

A robot that drives or walks around has to know where it is without a map. A camera is good at recognising places, but it struggles in the dark, when the scene is blurry, or when the view has no texture. A laser scanner (lidar) sees the shape of the surroundings well, but it struggles in long corridors or flat open fields where there is little structure to match. An inertial measurement unit (IMU) measures acceleration and rotation, which bridges short gaps but drifts over time.

LVI-SAM uses all three at once. It runs two halves that help each other. The camera-and-IMU half tracks the motion and uses the laser to learn how far away the visual features are. The laser-and-IMU half matches each new scan against a map of recent scans. Each half gives the other a starting guess, and if one half fails, the other keeps the robot localised. A single graph of constraints is optimised together, and when the robot returns to a place it has seen, a loop-closure step pulls the whole trajectory back into line.

The idea matters because each sensor covers the others' weak spots, so the combined estimate can stay accurate and robust across very different environments.

TL;DR

• LVI-SAM is a tightly-coupled lidar-visual-inertial odometry and mapping system built on a factor graph solved with iSAM2 §2.1.
• A visual-inertial system (VIS) and a lidar-inertial system (LIS) run side by side. Each gives the other an initial guess, and each has its own failure detection §2.1 §2.2.3 §2.3.2.
• Loop closure candidates come from DBoW2 in the VIS and are verified by ICP in the LIS §2.2.4 §2.3.
• The author-reported headline is the lowest RMSE against GPS on the Jackal dataset (0.67 m with loop closure), and the lowest errors on Handheld, where LOAM, LIO-mapping and LINS fail tab. 2.
• The paper leaves most thresholds unstated; the official code fixes them, and several code paths differ from the text (see Paper vs code).

Problem

Lidar-only odometry fails in structure-less places, such as long corridors and open fields. Camera-only odometry is sensitive to illumination, rapid motion and initialisation. Lidar-inertial and visual-inertial systems exist, and the lidar-visual systems the authors cite as most accurate are not publicly available §1. The authors also note that their factor graph allows global drift correction through loop closure, which those systems lack §1. The authors target robustness to sensor degradation, real-time operation, and global drift correction through loop closure §1. Non-inertial lidar-visual systems are out of scope §1.

Assumptions: the sensors are a 3D lidar, one monocular camera and one IMU, with the camera-to-lidar and IMU extrinsics known §2.1 fig. 1. The paper states the sensor suite §3 but does not describe an extrinsic calibration procedure.

Method

The system has two sub-systems that share state. Pose estimates are output at the IMU rate fig. 1.

Visual-inertial system (VIS) §2.2. The VIS adapts the VINS-Mono pipeline §2.2 fig. 2. Features are detected with a corner detector and tracked with the Kanade–Lucas–Tomasi method §2.2. The VIS optimises a sliding-window state per frame. The state holds rotation, position, velocity and IMU biases §2.2. Lidar depth is attached to each feature when available §2.2.2.

• Initialisation §2.2.1: the VIS takes the state and bias from the LIS, interpolated to each image keyframe by timestamp. The bias is assumed constant between two keyframes §2.2.1.
• Feature depth §2.2.2 fig. 3: lidar frames are registered to the camera using the VIS odometry and stacked into a sparse-to-dense depth cloud. Features and depth points are projected onto a unit sphere, and the three nearest depth points are found with a 2-D k-d tree in polar coordinates. The feature depth is the distance along the feature ray to the plane through those three points §2.2.2.
• Depth validation §2.2.2 fig. 3: a feature keeps its depth only if the three points are within 2 m of one another. Otherwise it gets no depth §2.2.2.
• VIS failure §2.2.3: the VIS flags a failure if too few features are tracked, or if the IMU bias estimate grows past a limit. It then re-initialises and tells the LIS §2.2.3.
• Loop detection §2.2.4: for each new keyframe, BRIEF descriptors are extracted and matched against earlier ones using DBoW2. The timestamps of the candidate matches are sent to the LIS §2.2.4.

Lidar-inertial system (LIS) §2.3. The LIS adapts LIO-SAM §2.3 fig. 5. It maintains a factor graph with four constraint types: IMU preintegration, VIS odometry, lidar odometry and loop closure §2.3. A new lidar keyframe is added when the robot pose has changed by a threshold. Intermediate scans are discarded. The feature map is a sliding window of keyframes, so the computational cost stays bounded §2.3.

• Initial guess §2.3.1: before the LIS is initialised, the guess comes from integrating raw IMU readings with zero bias and noise, starting from a static, zero-velocity state. The paper reports this works when the initial linear velocity is below 10 m/s and the angular velocity is below 180 °/s §2.3.1. After initialisation, the guess uses the VIS pose when it is available, and IMU readings with corrected bias otherwise §2.3.1.
• Degeneracy check §2.3.2 fig. 6: scan matching is solved as a linear least-squares problem, iterated eq. 1. The LIS reports failure when the smallest eigenvalue of AᵀA, taken at the first iteration, is below a threshold. The lidar-odometry constraint is then not added to the graph §2.3.2.
• Loop closure §2.3: the VIS candidates are checked by scan matching against the keyframe submap before they are added.

Main loop, outline. The flow is shown below. Names are descriptive, not the official API.

# LVI-SAM, one update cycle (outline)  [§2.1] [fig. 1]
# Inputs:  lidar scan, camera image, IMU readings
# Outputs: pose estimate at the IMU rate

def lvi_sam_cycle(scan, image, imu_readings):
    # Visual-inertial sub-system: sliding-window VIO  [§2.2]
    vis_pose, vis_failed = visual_inertial_step(image, imu_readings)

    # Attach lidar depth to visual features, when available  [§2.2.2]
    depths = associate_depth(image_features, stacked_lidar_points)

    # Lidar-inertial sub-system: seed the scan match  [§2.3.1]
    # Use the VIS pose when it is valid, IMU integration otherwise
    if vis_failed:
        initial_guess = integrate_imu_with_bias(imu_readings)
    else:
        initial_guess = vis_pose

    # Scan-to-map registration, with a degeneracy check  [§2.3] [§2.3.2]
    lis_pose, degenerate = scan_match(scan, initial_guess)

    # Add constraints to the factor graph and re-optimise  [§2.1] [§2.3]
    # IMU, VIS odometry, lidar odometry and verified loop closures
    graph_pose = factor_graph_update(lis_pose, vis_pose, loop_candidates)

    # Propagate the optimised biases with IMU readings for the output  [§2.1]
    return propagate_at_imu_rate(graph_pose, imu_readings)

Key state and equation, as stated in the paper:

\mathbf{x} = [\, \mathbf{R}, \; \mathbf{p}, \; \mathbf{v}, \; \mathbf{b} \,] §2.2

\min_{\mathbf{T}} \; \lVert \mathbf{A} \mathbf{T} - \mathbf{b} \rVert^{2} eq. 1

where A and b come from linearisation at T §2.3.2.

Hyperparameters

The paper states only a few values. The official code is listed for values the paper does not state. Repository defaults are not the paper's tuned values unless the paper says so.

Results

Author-reported. Setup §3: three self-gathered datasets, named Urban, Jackal and Handheld. Sensors: a Velodyne VLP-16 lidar, a FLIR camera, a MicroStrain IMU, and a Reach RS+ GPS used as ground truth. Compute: C++ on a laptop with an Intel i7-10710U under Ubuntu. Baselines: VINS-Mono, LOAM, LIO-mapping, LINS and LIO-SAM §3. The paper states each dataset is a loop that starts and ends at the same position, and that loop closure is toggled to compare with and without it §3.2 §3.3.

Table 2 tab. 2: RMSE w.r.t. GPS (m), end-to-end translation (m) and rotation (°). "Fail" is the paper's label. Values are from the paper.

Headline claims, as the paper states them §3.2 §3.3 tab. 2:

• Jackal: LVI-SAM gives the lowest average RMSE against GPS. LINS (w/ loop) has the lowest translation error, 0.09 m, against LVI-SAM's 0.11 m.
• Handheld: the lidar-based baselines fail, and LVI-SAM succeeds with or without loop closure.

Table 1 tab. 1: Urban ablation, end-to-end errors. Setting per §3.1: the operator walks with the sensor suite and the data starts and ends at the same position. Units: translation m, rotation °.

A1: VIS only. A2: LIS only. A3: VIS and LIS, loop closure disabled. A4: all modules, with loop closure §3.1.1–§3.1.4. The paper's text states that A3 depth reduces translation error by 29 %, from 45.42 m to 32.18 m §3.1.3.

Paper vs code

Repository: https://github.com/TixiaoShan/LVI-SAM, BSD-3-Clause, commit 0d822f6dcac3378312f6703b4f45829e049f221a. The paper links it as git.io/lvi-sam, which redirects to this repository, and the owner is the first author §3. Code is cited relative to the repository root. The values in the Hyperparameters table with a [code: ...] citation come from the code, not the paper.

• LIS degeneracy gating. The paper says the lidar-odometry constraint is not added to the factor graph when degeneracy is detected §2.3.2. In the code, the condition that would gate the factor on isDegenerate is commented out, and the lidar-odometry constraint is always added mapOptmization.cpp. Degeneracy only projects the update onto the non-degenerate directions mapOptmization.cpp.
• Degeneracy threshold. The paper says "a threshold" §2.3.2. The code uses 100 for every eigenvalue, hard-coded, and checks from the smallest eigenvalue upward. It only does this on the first iteration mapOptmization.cpp.
• VIS constraints in the LIS graph. The paper lists visual-odometry constraints as one of the four factor types in the LIS graph §2.3. The code has the VIS-to-LIS factor commented out, with the comment saying it was removed because the benefit is not obvious mapOptmization.cpp. The VIS pose is used only as the scan-matching initial guess mapOptmization.cpp.
• VIS failure, feature count. The paper includes a feature-count trigger for VIS failure §2.2.3. The code's feature-count check exists, but its return true is commented out estimator.cpp. The failure conditions that do run are IMU bias, speed and translation jump estimator.cpp.
• Depth validity. The paper gives the 2 m check §2.2.2. The code applies the same 2 m rule to the range of three depth values, and adds two rules the paper does not mention: a rejection when the scale s <= 0.5, and a clamp of the depth into the range of the three points feature_tracker.h.
• Lidar stacking. The paper says frames are stacked to make a dense depth map §2.2.2 without a count. The code uses one of every lidar_skip + 1 scans, with lidar_skip = 3 in the shipped config feature_tracker_node.cpp params_camera.yaml.
• Loop closure timing. The paper describes loop closure as validated by the LIS §2.3. The code also enforces a 5 s minimum interval between accepted loops mapOptmization.cpp and an ICP fitness threshold of 0.3 mapOptmization.cpp.
• Initial guess and IMU-only initialisation. The 10 m/s and 180 °/s validity range §2.3.1 was not traced in the code, and the paper's claim is not checked against it.
• Not traced. The VIS feature-depth association details beyond the lines above, the LIS keyframe-selection rule, and the full lidar_skip behaviour were not compared line by line with the paper. Other values of the paper's tables (Table 1 and Table 2) come from the authors' runs, and the code does not contain the datasets or the baseline configurations.

Pitfalls

• Missing thresholds. The paper omits the degeneracy threshold, the VIS failure thresholds, the keyframe thresholds and the sliding-window size. An implementation has to choose them. The repository values in the table above are a starting point, not the paper's tuned values, and the paper does not say how they were chosen.
• Degeneracy handling. The paper's wording (drop the factor) and the code's behaviour (keep the factor, project out the degenerate directions) differ. Copying the paper alone gives a different system.
• Hard-coded degeneracy threshold. The threshold of 100 applies to the eigenvalues of AᵀA, whose scale depends on the scan-matching problem. A different sensor, point density or unit scale changes which directions count as degenerate. Re-check it when the setup changes.
• Failure can be silent. The code's VIS feature-count check is disabled estimator.cpp, so a VIS that tracks very few features is not reported as failed by that rule, only by the bias, speed and translation rules.
• Depth is a sparse, filtered signal. The paper describes depth as a lookup on a sphere with a nearest-three rule. The lidar must be registered with the VIS odometry, so a poor VIS pose corrupts the depth. Stacking several lidar frames introduces depth ambiguity between objects; the 2 m rule is the paper's only guard §2.2.2 fig. 3.
• Ablation text vs table. The text claims depth reduces error §3.1.1 and then reports rotation error that rises from 6.41° to 7.66° in A3 tab. 1. The rotation claim does not hold for that row.
• Only a few runs. Each result comes from one dataset run per method, with the loop closure toggled by hand §3.2 §3.3. The paper gives no variance, seeds, or repeated runs.
• Hidden costs. The paper says VIS-LIS data exchange is intended to reduce cost §2.1; it gives no timing or memory figures. The code caps the VIS solver at 0.035 s and 10 iterations params_camera.yaml, which is a real-time limit, not a measured figure.
• Unpublished text. The source has commented-out sections, including a KITTI experiment and an alternative description of initialisation. They are not part of the published results and are not used here.

Lineage

• Builds on:
  • VINS-Mono processing pipeline for the VIS, adapted §2.2.
  • LIO-SAM factor-graph lidar-inertial system, adapted §2.3.
  • DBoW2 for loop detection §2.2.4; BRIEF descriptors §2.2.4.
  • iSAM2 for incremental factor-graph solving §2.1.
  • Degeneracy analysis of [zhang2016degeneracy] for LIS failure detection §2.3.2.
  • Corner detection [shi1994good] and KLT tracking [lucas1981iterative] §2.2.
• Compared against: VINS-Mono, LOAM, LIO-mapping, LINS, LIO-SAM §3.
• Superseded by: not stated in this wiki.