Visual-inertial odometry

Overview

Visual-inertial odometry (VIO) tracks a sensor rig's pose by combining an inertial measurement unit (IMU), which reports acceleration and rotation at a high rate, with a camera, which tracks image features at a lower rate. The IMU predicts the motion between camera frames, and the camera's feature measurements constrain that prediction 2101.06894 §3.1 2102.12400 §IV-D, §IV-H 2104.10831 §2.2, §2.3.

Four papers with cards cover this. Kimera's VIO module needs only a camera and an IMU, with stereo or RGB-D images 2101.06894 §3. The other three put the camera in a lidar-inertial-visual system, where the visual-inertial part works next to a lidar-based part. R2LIVE adds the camera to a lidar-inertial filter 2102.12400 §I. LVI-SAM runs a visual-inertial sub-system and a lidar-inertial sub-system side by side, and each gives the other a starting guess 2104.10831 §2.1. R3LIVE runs a visual-inertial step against a map that its lidar-inertial part builds 2109.07982 §II. Cards: Kimera, R2LIVE, LVI-SAM and R3LIVE.

Variants

Five variants. The first comes from Kimera and runs on a camera and IMU alone. The next two come from R2LIVE and differ in where the camera measurements enter. The fourth comes from LVI-SAM, where the visual-inertial part gets its depth from the lidar. The fifth comes from R3LIVE, which updates the state against a lidar-built map using photometric residuals.

Keyframe fixed-lag smoother, camera and IMU only

Kimera-VIO estimates pose in a keyframe-based maximum a posteriori problem with IMU preintegration 2101.06894 §3.1. Shi-Tomasi corners are tracked with Lucas-Kanade at every frame. The tracker is seeded with rotation predicted by the IMU, because the robot moves through crowds 2101.06894 §3.1. Feature detection, stereo matching and geometric verification run only at keyframes 2101.06894 §3.1. The back end is a factor graph solved with iSAM2 in GTSAM. Each feature's 3-D point is triangulated and then eliminated from the state, so the graph has no landmark variables 2101.06894 §3.1. The smoothing horizon is fixed-lag, with 6 s as the default in the TX2 configurations 2101.06894 §4.9. Its verification defaults are IMU-aided RANSAC, with 2-point for stereo and 1-point for mono 2101.06894 §3.1.

Camera update inside an error-state filter

Camera measurements are fused at each camera time, in the same iterated filter that fuses LiDAR and IMU 2102.12400 §IV-E, §IV-F. Each tracked corner gives one reprojection residual, the difference between the observed pixel and the projection of a 3D landmark through the IMU and camera extrinsic 2102.12400 §IV-H, eq. 16. Corners are found with FAST and tracked with KLT optical flow 2102.12400 §IV-H. The measurement noise enters as the pixel noise covariance, which the paper does not give a value for 2102.12400 §IV-H, eq. 20.

Sliding-window visual factor graph

A back end optimizes camera poses and landmark positions over a sliding window of image keyframes 2102.12400 §V. It adds IMU preintegration factors between keyframes and estimates the camera-LiDAR time offset. The authors say the graph is similar in structure to VINS-Mono's 2102.12400 §V. It runs in a separate thread at a lower rate than the filter 2102.12400 §VI-F.

Visual-inertial sub-system with lidar depth

LVI-SAM's visual-inertial sub-system adapts the VINS-Mono pipeline. It tracks corners with KLT and optimizes a sliding window per frame. The state holds rotation, position, velocity and IMU biases 2104.10831 §2.2. Lidar depth is attached to each feature when it is available. The depth comes from lidar frames registered to the camera with the visual odometry, and a feature keeps it only if the three nearest depth points lie within 2 m of one another 2104.10831 §2.2.2. In the Urban ablation, the visual-inertial run without lidar depth has a translation error of 239.19 m, against 142.12 m with depth 2104.10831 tab. 1. Loop candidates are found with DBoW2 in this part and checked by the lidar-inertial part 2104.10831 §2.2.4, §2.3. The visual-inertial pose is passed to the lidar-inertial sub-system as the starting guess for scan matching, and IMU readings with corrected bias fill in when it is not available 2104.10831 §2.3.1.

Photometric update that colours a lidar map

R3LIVE's visual-inertial step runs after its lidar-inertial part has built a global point map. Each image updates the state twice, both times with the same iterated error-state filter 2109.07982 §V-A2, §V-B2. The first update uses frame-to-frame reprojection residuals on points tracked with Lucas-Kanade 2109.07982 §V-A1. The second uses frame-to-map photometric residuals: the colour stored in a map point, minus the colour seen at the point's predicted pixel 2109.07982 §V-B1. The observed colours are then fused into each map point by a precision-weighted average 2109.07982 §V-C. The state also holds the camera intrinsics and the camera-IMU time offset, estimated online 2109.07982 §III-A. The paper states that the method needs no extracted visual features, and that camera-based methods struggle on texture-less surfaces 2109.07982 §I.

When to use which

Accuracy numbers are author-reported. On a 300 m sequence against D-GPS, the median translation error is 3.84% for VINS-Mono, 0.34% for FAST-LIO and 0.21% for R2LIVE 2102.12400 fig. 11. LVI-SAM reports RMSE against GPS on its own Jackal and Handheld datasets 2104.10831 tab. 2. Kimera-VIO's ATE on EuRoC ranges from 0.05 m to 0.21 m 2101.06894 tab. 1. The paper aligns trajectories with SE(3), not Sim(3), and says Sim(3) would give a smaller error for Kimera 2101.06894 §4.2. R3LIVE reports relative pose error against RTK ground truth in a seaport, for example 0.21 deg and 0.17% at 300 m for R3LIVE-HiRes 2109.07982 tab. III. Each paper's test uses its own dataset and metric, so do not rank these methods against each other from these numbers. LVI-SAM's timing is not measured in the paper. Its shipped configuration caps the visual solver at 0.035 s and 10 iterations params_camera.yaml, which are limits, not measured figures.

Implementation sketch

The sketch covers the camera side, which is specific to VIO, and the handoff from the visual-inertial part to the lidar-inertial part in LVI-SAM. The first two blocks follow R2LIVE, and the third follows R3LIVE's photometric update. The IMU propagation and the iterated update are described in the error-state Kalman filter concept page.

# Track corners and measure their pixels at each camera time  [§IV-H]
def camera_measurements(prev_frame, frame, tracked_corners):
    # Carry corners forward with KLT optical flow, detect new FAST corners
    # [§IV-H]
    corners = klt_track(prev_frame, frame, tracked_corners)
    corners += fast_detect(frame, exclude=corners)
    return corners

# One reprojection residual per corner, for the iterated update  [§IV-H] [eq. 16]
def camera_residuals(state, corners, landmarks):
    residuals = []
    for pixel, landmark in zip(corners, landmarks):
        # Landmark in camera frame through IMU and camera extrinsic  [eq. 16]
        point_cam = world_to_camera(state, landmark)
        # Observed pixel minus the projection of the landmark  [§IV-H]
        residuals.append(pixel - pinhole(point_cam))
    return residuals

# Photometric residual of one lidar map point in the current image  [§V-B1]
def photometric_residual(state, point, image):
    # Predicted pixel, with the estimated time-offset correction  [§V-A1]
    pixel = project_with_time_offset(point, state)
    # Colour at that pixel, interpolated from neighbouring pixels  [§V-B1]
    observed = bilinear_colour(image, pixel)
    # Stored map colour minus observed colour  [§V-B1]
    return point.colour - observed

# Visual-inertial pose seeds the lidar scan match  [§2.3.1]
def lidar_initial_guess(vis_pose, vis_valid, imu_readings, bias):
    # Use the visual-inertial pose while it is available  [§2.3.1]
    if vis_valid:
        return vis_pose
    # Otherwise integrate IMU readings with the corrected bias  [§2.3.1]
    return integrate_imu(imu_readings, bias)

Gating of camera updates is not described in the paper. The card's Paper vs code section notes that the authors' code applies acceptance gates, which an implementation must decide for itself.

Contradictions

Within R2LIVE, the paper and its code differ on the camera covariance update and on gating. See the card's Paper vs code section.

Within LVI-SAM, the paper and its code differ in four places. The paper says the lidar-odometry factor is dropped when degeneracy is detected, but the code always adds it and only projects out the degenerate directions 2104.10831 §2.3.2 mapOptmization.cpp. The paper lists visual-odometry factors in the lidar-inertial graph, but the code comments that factor out 2104.10831 §2.3. The paper includes a feature-count failure trigger for the visual part, but its check is disabled in the code 2104.10831 §2.2.3. See the card's Paper vs code section for the citations.

The LVI-SAM text and its ablation table disagree on rotation. The text says depth reduces error, while the rotation error for the depth-enabled A3 run rises from 6.41° to 7.66° 2104.10831 §3.1.3, tab. 1.

Within R3LIVE, the colour-fusion equation printed in the paper, read literally, takes the inverse of the weighted sum before multiplying by the covariance. The code, and the card, use the precision-weighted form instead [arxiv:2109.07982 §V-C]. The paper and its code also differ in the VIO. The paper's cost is a plain squared Mahalanobis term, while the code applies a Huber scale to every residual in both updates 2109.07982 §V-A2, §V-B2. The paper describes removing points with large residuals 2109.07982 §V-D, but the code's residual rejection is skipped for the argument it passes. The code also runs a fundamental-matrix RANSAC that the paper does not mention. The paper gives no iteration count for the photometric update, and the code runs a fixed two iterations 2109.07982 §V-B2. See the card's Paper vs code section for the code paths. The paper does not say whether the VIO can run without the lidar map, so a camera-only port needs its own depth source 2109.07982 §II, §V.

Papers

• Kimera: a camera-and-IMU VIO module with a keyframe fixed-lag smoother, which the paper reports as competitive on EuRoC; the wider system builds a 3D scene graph.
• R2LIVE: a LiDAR-inertial-visual filter with a visual factor graph in the back end, reported as robust to a covered camera and to fast rotation.
• LVI-SAM: a lidar-visual-inertial factor graph with a visual-inertial sub-system that uses lidar depth, reported with the lowest RMSE against GPS on its Jackal and Handheld datasets, with loop closure.
• R3LIVE: a LiDAR-inertial-visual system whose visual part colours a lidar map by photometric residuals, reported with translation drift of 0.09 to 0.16 m over 1.2 to 1.5 km campus runs.