CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

In plain words

A LiDAR on a moving vehicle takes many measurements while it turns and bumps, so each sweep of points was captured from slightly different places. Most LiDAR odometry methods assume the sensor moved at a steady speed during one sweep, which breaks when it swerves or tilts quickly.

CT-ICP instead gives each sweep two positions, one at the start and one at the end. Every point is placed using the position at the moment it was measured, and the sweep is stretched to fit the map. Between sweeps there is no forced continuity, which lets the estimate follow sudden jolts.

The authors also add a loop-closure step. It turns recent sweeps into a top-down height image, matches that image against older ones to spot places already visited, and corrects the whole trajectory when a match is found. The point of the method is accurate, fast tracking on driving and high-frequency-motion data, with no inertial sensor needed.

TL;DR

• LiDAR-only odometry with two poses per scan, joined to the previous scan by a soft proximity constraint §3.1 eq. 7 eq. 8.
• Scan-to-map registration against a voxel-based local map; the abstract reports real-time speed on one CPU thread [abstract] §3.2.
• Loop closure matches 2D elevation images and corrects a pose graph; it assumes mostly planar motion and a known vertical axis §4.
• Author-reported gains are largest on KITTI-raw, KITTI-360, KITTI-CARLA and NCLT, where the baseline error is far higher tab. 1.
• Code is MIT-licensed by the first author's repository; loop-closure code is in a separate Kitware repository §4.

Problem

Scan-to-map LiDAR odometry must undo motion during a sweep. Previous methods do this with a constant-velocity model over the scan, which does not capture large changes in direction or speed §1. Trajectory-based methods that do model motion inside a scan either need an IMU or smooth the motion, which the authors say costs accuracy §1 §2.

Loop closure is also an open problem for LiDAR SLAM. Registration-based loop closure only works for small trajectories with low drift, and per-scan place recognition is sensitive to environment change §2.

The loop-closure procedure assumes sensor motion mostly on a plane and an extrinsic calibration that aligns the sensor z axis with the ground normal §4.

Method

Odometry (CT-ICP)

The odometry describes scan n by two poses: a begin pose \Omega_b = (R_b, t_b) and an end pose \Omega_e = (R_e, t_e) §3.1. The pose of a point measured at time \tau_i is interpolated between them. Rotation uses spherical linear interpolation (slerp) and translation is linear, with the interpolation parameter \alpha_i = (\tau_i - \tau_b)/(\tau_e - \tau_b) §3.1 eq. 5 eq. 6.

The joint problem and its terms are:

\underset{X \in SE(3)^2}{\arg\min}\; F_{ICP}(X) + \beta_l\, C_{loc}(X) + \beta_v\, C_{vel}(X) eq. 1

r_i = a_i \, (p_i^W - q_i^W) \cdot n_i eq. 3

R^{\alpha_i} = \mathrm{slerp}(R_b, R_e, \alpha_i), \quad t^{\alpha_i} = (1-\alpha_i)\, t_b + \alpha_i\, t_e eq. 5 eq. 6

C_{loc}(t_b) = \lVert t_b - t_e^{n-1} \rVert^2 eq. 7

C_{vel}(t_b, t_e) = \lVert (t_e - t_b) - (t_e^{n-1} - t_b^{n-1}) \rVert^2 eq. 8

The begin pose of scan n is not tied to the end pose of scan n-1. Instead a proximity constraint pulls them together §3.1. Two terms do this: a location term that keeps the begin translation near the previous end translation eq. 7, and a velocity term that keeps this scan's motion near the previous scan's motion eq. 8. Both have weight 0.001 in all experiments §3.1.

The scan is grid-sampled into keypoints, and the keypoints are registered into the local map §3.1. The solution is the joint minimisation of the ICP term and the two constraints eq. 1. The ICP term averages a robust loss over keypoint residuals eq. 2. Each residual is a planarity-weighted point-to-plane distance to the nearest map point eq. 3.

The planarity weight is a_i = (\sigma_2 - \sigma_3)/\sigma_1, computed from the covariance of the map neighbourhood of the transformed keypoint, not from the current scan. It is recomputed at every ICP iteration §3.1.

The loop stops when the change in the pose parameters falls below a threshold, or after a fixed iteration count chosen for real time (5) §3.1.

After registration the scan points are transformed to the world frame with the solved poses and inserted into the local map §3.2.

Local map and robust profile

The local map is a dense point cloud in a sparse voxel grid, so neighbour lookup is constant-time rather than logarithmic as in a kd-tree §3.2. The voxel size is 1.0 m for driving and 0.80 m for high-frequency motion §3.2. Each voxel keeps at most 20 points, and no two kept points are closer than 10 cm §3.2.

Each keypoint's neighbourhood is its k = 20 nearest map points within the 27 voxels around it §3.2. Unlike a sliding window of recent scans, voxels are removed only by their distance to the centre of the last inserted scan §3.2.

The robust profile detects fast orientation changes and registration failures, then reruns the registration with more keypoints and a larger neighbourhood. A scan whose orientation changed by 5 degrees or more is not inserted into the map §3.2.

Loop closure and back end

The loop closure keeps a window of recent odometry scans. When the window holds N_{map} scans, they are merged into one cloud in the frame of the window's centre §4.

The merged cloud is projected onto a 2D elevation grid that keeps the highest point per pixel. Heights are clipped to [z_{min}, z_{max}], giving an elevation image. Rotation-invariant 2D features are extracted from it and stored with the grid §4.

Every N_{map} - N_{overlap} scans, the newest elevation image is matched against stored ones. The 10 stored images closest to the current grid are the candidates §4. A 2D rigid transform is fit with RANSAC, and a threshold on the inlier count validates it. Open3D ICP then refines the transform on the elevation grids' point clouds, giving a 6-DoF loop constraint §4.

The back end is a pose graph optimised with g2o. New odometry poses are added continuously, but the global optimisation runs only when a new loop constraint is found §4.

Paper values: N_{map} = 100 and N_{overlap} = 30; z_{max} = z_{min} + 10 m with z_{min} just below ground; NCLT is rotated so its z axis points up §5.3.

Pseudocode

The paper has no algorithm box, so the two sketches below are our own summary of the text above, with citations to the source of each step.

# CT-ICP registration of one scan  [§3.1] [eq. 1]
# Input:  scan points p_i^L with timestamps tau_i; local map M;
#         previous scan's begin and end translations
# Output: begin pose and end pose of this scan

def register_scan(scan, local_map, prev_t_begin, prev_t_end):
    # Grid-sample the scan into keypoints  [§3.1]
    keypoints = grid_sample(scan)
    # Starting poses; the text does not say how they are set  [§3.1]
    pose_begin, pose_end = initial_poses(prev_t_end)
    for iteration in range(max_icp_iterations):
        # Place keypoints in the world frame, interpolated by time  [§3.1]
        world_points = interpolate_and_transform(keypoints, pose_begin,
                                                 pose_end)
        residuals = []
        for world_point in world_points:
            # Up to k = 20 nearest map points within 27 voxels  [§3.2]
            neighbours = local_map.nearest(world_point, k=20)
            # Normal and planarity (s2 - s3) / s1 of the neighbourhood  [§3.1]
            normal, planarity = plane_fit(neighbours)
            # Planarity-weighted point-to-plane residual  [§3.1] [eq. 3]
            residual = planarity * dot(normal, world_point - neighbours[0])
            # The robust loss is applied to each residual  [§3.1] [eq. 2]
            residuals.append(residual)
        # Proximity constraints to the previous scan  [§3.1] [eq. 7] [eq. 8]
        residuals += proximity_terms(pose_begin, pose_end, prev_t_begin,
                                     prev_t_end)
        # Joint update of both poses  [§3.1] [eq. 1]
        pose_begin, pose_end, step = solve_least_squares(residuals)
        # Stop when the step is small enough  [§3.1]
        if step_is_small(step):
            break
    # Insert the scan unless the robust profile rejects it  [§3.2]
    if not large_rotation(pose_begin, pose_end):
        local_map.insert(transform_scan(scan, pose_begin, pose_end))
    return pose_begin, pose_end

# Loop closure on elevation images  [§4]
# Input:  odometry scans with poses; stored elevation images
# Output: pose-graph loop constraints

def loop_closure_step(window, memory, n_map, n_overlap):
    # Merge the window's scans into one cloud, in the window centre frame  [§4]
    cloud = aggregate(window.scans, window.centre_frame())
    # Keep the highest point per pixel of a 2D grid  [§4]
    grid = max_elevation_grid(cloud)
    # Clip heights to [z_min, z_max] to form an elevation image  [§4]
    image = clip(grid, z_min, z_max)
    # Extract rotation-invariant 2D features  [§4]
    features = rotation_invariant_features(image)
    # Keep only the newest n_overlap scans in the window  [§4]
    window.keep_last(n_overlap)
    # Compare with the 10 stored images closest to the current grid  [§4]
    for stored in memory.closest(grid, n_candidates=10):
        # RANSAC 2D rigid fit; accept only with enough inliers  [§4]
        transform, inliers = ransac_2d(features, stored.features)
        if inliers_are_enough(inliers):
            # Open3D ICP refinement gives a 6-DoF constraint  [§4]
            constraint = icp_refine(transform, grid, stored.grid)
            pose_graph.add_loop_constraint(constraint)
            # Global optimisation runs only when a loop is added  [§4]
            pose_graph.optimise()
    memory.add(grid, features)

Hyperparameters

Values marked "paper" are stated in the article. Values marked "code" come from the repository at code_commit and are not in the article. "not stated" means neither source gives the value.

Results

Author-reported. Odometry setup: KITTI-corrected (motion-corrected scans, sequences 00 to 10), KITTI-raw, KITTI-360, KITTI-CARLA and ParisLuco use the driving profile; NCD and NCLT use the high-frequency profile. The metric is KITTI Relative Translation Error (RTE, %) over segments of 100 m to 800 m §5.2 tab. 1. Timing hardware is stated only in the abstract: one CPU thread [abstract].

Loop closure, one sequence per dataset, ATE in metres after a rigid fit. LO is odometry alone, LO+LC adds loop closure tab. 2:

Loop-closure cost: about 1.1 s per elevation grid and match, and about 1.2 s per pose-graph optimisation §5.3.

Paper vs code

Code at code_commit is the first author's repository (MIT). The loop-closure implementation is in Kitware/pyLiDAR-SLAM at commit 1baa21a, which the paper links. Its LICENSE is MIT, with copyright held by Kitware (Pierre Dellenbach). The renderer links every [code: ...] citation to the card's code repository, so the pyLiDAR-SLAM lines are written here as plain text.

• Stop criterion units. Paper: 0.1 cm translation and 0.01 deg rotation §3.1. Driving config: orientation 0.1 and translation 0.01 driving_config.yaml. The NCLT config comments these as degrees and metres nclt_config.yaml. Defaults in odometry.cpp set orientation to 0.1, then overwrite it with 0.01 odometry.cpp. The stop test is at ct_icp.cpp.
• Planarity weight. Paper: residual weight is a_i eq. 3. Ceres path: weight is 0.9 \cdot a^{p} + 0.1 \cdot \exp(-d / (0.5 \cdot k)), with p = power_planarity = 2 ct_icp.cpp driving_config.yaml. The 0.5 in the exponent is max_dist_to_plane_ct_icp, which the YAML does not set, so the default in odometry.cpp applies only if the profile defaults are loaded. The planarity weights are normalised from 0.9 and 0.1 ct_icp.cpp.
• Robust Cauchy loss. Paper: \rho unspecified §3.1. Code: Cauchy, \sigma = 0.1 ct_icp.cpp driving_config.yaml.
• Map voxel size. Paper: 1.0 m driving and 0.80 m high-frequency §3.2. Driving YAML: 0.8 m, one level, 30 points per voxel, search radius 0.75 m driving_config.yaml. NCLT YAML: size_voxel_map 1.0 with three resolutions (0.5, 1, 2 m) nclt_config.yaml. The code path that reads size_voxel_map is odometry.cpp.
• Keypoints and iterations for NCLT. Paper gives no NCLT setting §3.1. The NCLT YAML uses 20 ICP iterations, adaptive sampling at 0.8 m, and up to 1500 keypoints nclt_config.yaml nclt_config.yaml nclt_config.yaml.
• Threads. Paper says CT-ICP "runs on a single thread" by default §3.1. Both YAMLs set ls_num_threads: 6 driving_config.yaml nclt_config.yaml.
• Large-rotation rule. Paper: a scan with orientation change of 5 deg or more is not inserted §3.2. Code: skip insertion only when the change is strictly greater than the threshold odometry.cpp. It also forces insertion after more than 5 consecutive failures, which the paper does not mention odometry.cpp.
• Loop window size. Paper: N_{map} = 100, N_{overlap} = 30 §5.3. The pyLiDAR-SLAM defaults are 50 and 20 [pyLiDAR-SLAM slam/loop_closure.py#L84-L85]. Its repository config leaves loop_closure unset [pyLiDAR-SLAM config/slam.yaml#L21], so the paper's values are not in the repository.
• Candidate filter. Paper: the 10 candidates closest to the current grid §4. Code also excludes stored maps that are too recent (min_id_distance 200 frames divided by the window step) and keeps only candidates within 100 m of the query pose [pyLiDAR-SLAM slam/loop_closure.py#L88-L89] [pyLiDAR-SLAM slam/loop_closure.py#L301-L312].
• Height range. Paper: z_{max} = z_{min} + 10 m §5.3. Code default: z_{min} = -3, z_{max} = 5 [pyLiDAR-SLAM slam/loop_closure.py#L98-L99].
• Planarity definition. Code computes planarity from the singular values as (\sigma_2 - \sigma_3)/\sigma_1 neighborhood.h, which matches the paper's a_i definition §3.1. The power applied to it is the difference noted above.
• KITTI leaderboard. The 0.59% test score is in the paper text only §5.2. No leaderboard record was found in the repository's file listing.
• Agreements checked: the residual uses one nearest map neighbour (num_closest_neighbors: 1) driving_config.yaml ct_icp.cpp; the constraint weights are 0.001 in both YAMLs driving_config.yaml; the minimum spacing of 10 cm is in both YAMLs. The code's time-interpolation function was not separately checked against eq. 5 and eq. 6.

Pitfalls

• Units in the stop test. The paper's cm and degree values and the YAML's metre and degree values disagree. Use the code's units and check one run.
• Weight is not the paper's weight. A distance-dependent exponential term is added and planarity is squared. Implement the code's form if you want to reproduce results.
• Unstated values that change results. Loss scale, Ceres step limit, planarity power, voxel size, robust attempts and keypoint sampling are not in the paper's text. The values in the hyperparameter table that come from the repository are the code's, and the paper's own experiments may have used others.
• Solver is unnamed. The paper text never names the solver. The repository has Ceres and Gauss-Newton paths, and the configs select Ceres driving_config.yaml.
• Minimum residuals. The ICP fails if too few residuals are found ct_icp.cpp driving_config.yaml. The paper does not describe this failure mode.
• Timestamps are estimated for some datasets. For KITTI-raw and KITTI-360, per-point times are estimated linearly from azimuth §5.1. The paper does not analyse how errors in this estimate affect the results.
• Intrinsic angle correction. A 0.205 deg correction was applied to KITTI-corrected, KITTI-raw and KITTI-360 §5.1. Where it is applied in the repository was not checked.
• Ground truth limits. ParisLuco ground truth has translations only, so its RTE covers translation drift only §5.1. KITTI-raw sequence 03 is absent from the uncorrected set §5.1 tab. 1.
• Loop closure assumes planar motion. The elevation-image method needs mostly planar motion and a known vertical axis §4. NCLT data must be rotated so z points up §5.3.
• Loop closure is slow per event. It takes about 1.1 s per grid and match, plus about 1.2 s per pose-graph optimisation §5.3. The paper does not give the hardware for these times.
• Loop closure can raise ATE. ATE rose on KITTI-CARLA (0.21 to 0.26) and NCD (0.22 to 0.36) with loop closure on tab. 2. The text says KITTI-CARLA's ATE is almost unchanged by loop closure, and reports 21 cm of trajectory precision there before it §5.3.
• Leaderboard claim is time-dependent. "First among public-code methods" is the paper's own claim for its version dated 2022-02-24 (raw metadata). It is not a current standing §5.2.

Lineage

• Builds on: IMLS-SLAM for the dense local map and the planarity weights §2 §3.1 §3.2; point-to-plane ICP §2; Elastic LiDAR Fusion and MARS LiDAR odometry for continuous-time trajectories, which CT-ICP departs from by allowing discontinuity between scans §2.
• Compared against: IMLS-SLAM, MULLS and pyLiDAR F2M on odometry tab. 1; pyLiDAR F2M on NCD and NCLT tab. 1.
• Loop closure builds on the elevation-image approach of BVMatch, but runs on local maps instead of single scans §2.
• Superseded: not known from this wiki.