M4-T9: Visual-LiDAR-Inertial Odometry

Learning objectives

After this lesson, you should be able to:

• Explain the complementary strengths and failure modes of camera and LiDAR.
• Describe loosely and tightly coupled LiDAR-visual-inertial architectures.
• Show with an information-matrix example how vision constrains a LiDAR-degenerate direction.
• List the calibration and synchronisation requirements.

Conceptual explanation

LiDAR provides accurate metric geometry independent of light but can be degenerate in structureless geometry (lesson M2-T13); a camera provides dense texture and constraints along directions that LiDAR cannot see, but needs light and texture and has no direct scale. A visual-LiDAR-inertial system uses the IMU as the common backbone: prediction, deskewing and a shared state; the exteroceptive sensors provide corrections. Examples: LVI-SAM (factor graph, two subsystems that assist each other), R3LIVE and FAST-LIVO2 (filter-based, direct).

Engineering intuition

Information is a matrix: each sensor adds a positive semi-definite contribution; a direction is well constrained if the sum has a large eigenvalue there. LiDAR may contribute nothing along the corridor; a camera looking at textured walls contributes along it. The fusion is therefore not an average but an addition of information in complementary directions. The practical rules: make sure the camera and LiDAR agree in time and extrinsics, and monitor each sensor’s own health so the fused weights follow reality (lesson M2-T16).

Mathematical formulation

Information (Fisher) form: with measurement Jacobians $J_{L}$ (LiDAR) and $J_{C}$ (camera) and noise covariances $R_{L}$ and $R_{C}$, the information about the state is the sum:

FAST-LIVO2 (Zheng et al.) estimates with an error-state iterated Kalman filter in which the LiDAR point-to-plane update and the image photometric update are applied sequentially within the same iterated framework, with a unified voxel map whose planes also provide the depth for the photometric patches. Photometric methods additionally need exposure (brightness) handling. LVI-SAM runs a visual-inertial and a LiDAR-inertial system, each using the other’s output for initialisation and scale, with a joint factor-graph smoother.

$J_{L}$, $J_{C}$

LiDAR and camera measurement Jacobians with respect to the state.

$R_{L}$, $R_{C}$

noise covariances.

Architecture and data flow

Requirements: synchronised camera, LiDAR and IMU clocks (lesson M7-T8); calibrated camera intrinsics and camera–LiDAR–IMU extrinsics (lesson M7-T16); per-point LiDAR timestamps; exposure handling for photometric methods. Output: odometry with covariance and health for each sensor. Compute: a LiDAR-visual system is the heaviest of the estimators in this track; budget it (lesson M7-T12).

Real-world UAV applications

• GNSS-denied flight across mixed indoor and outdoor environments.
• The perception core of Capstone 4.
• Coloured maps and visual–geometric reconstruction.
• Redundant odometry with diverse failure modes (lesson M2-T16).

Implementation example

This runs as shown: the translational information matrix of a corridor LiDAR (degenerate along the axis) is added to the information from a camera that observes along-axis motion with a given accuracy. The along-axis standard deviation falls from effectively unbounded to the camera’s level, while the other directions stay LiDAR-accurate.

import numpy as np
rng = np.random.default_rng(0)
# LiDAR in a corridor along x: surface normals are only along y and z, so the x direction carries no information
normals = np.vstack([np.tile([0, 1.0, 0], (300, 1)), np.tile([0, 0, 1.0], (300, 1))]) + 0.01 * rng.standard_normal((600, 3)) * np.array([0, 1.0, 1.0])
normals /= np.linalg.norm(normals, axis=1, keepdims=True)
sigma_L = 0.03
I_lidar = normals.T @ normals / sigma_L ** 2

# camera: tracks texture on the walls; observes motion along the corridor with about 5 cm accuracy per frame and little across
I_cam = np.diag([1 / 0.05 ** 2, 1 / 0.5 ** 2, 1 / 0.5 ** 2])



cov_lidar = np.linalg.inv(I_lidar + 1e-9 * np.eye(3)); cov_both = np.linalg.inv(I_lidar + I_cam)
s_l, s_b = np.sqrt(np.diag(cov_lidar)), np.sqrt(np.diag(cov_both))
print("std (x along, y, z) LiDAR only :", np.round(s_l, 3)); print("std (x along, y, z) LiDAR + cam:", np.round(s_b, 4))
assert s_l[0] > 1000 * s_l[1]                                      # LiDAR alone: the along-corridor direction is unconstrained (huge standard deviation)
assert s_b[0] < 0.06 and s_b[1] < s_l[1] and abs(s_b[0] - 0.05) < 0.01   # the camera pins the along direction to about its own accuracy
assert s_b[1] < 0.03 and s_b[2] < 0.03                            # the other directions remain LiDAR-accurate

Common engineering mistakes

Failure modes

Debugging approach

1. Plot the information eigenvalues for each sensor in time.
2. Verify camera–LiDAR extrinsics by projecting the point cloud on the image.
3. Run ablations: LIO only, VIO only, both.
4. Check exposure compensation on a lighting-change sequence.

Practical exercises

1. Compute the information matrix for a camera that looks at a wall ahead and one that looks down.
2. Evaluate an ablation: LIO only versus LIVO on a public dataset with a corridor.
3. List the extra calibration needed beyond LIO.
4. Decide whether to add radar to your design for a dust-prone environment.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• C. Zheng et al., “FAST-LIVO2: fast, direct LiDAR–inertial–visual odometry”, IEEE T-RO, 2025
• T. Shan et al., “LVI-SAM: tightly-coupled lidar-visual-inertial odometry via smoothing and mapping”, ICRA 2021
• FAST-LIVO2 repository

What’s next