LiDAR odometry and loop closure for a car-mounted Velodyne, written in NumPy and SciPy: the KISS-ICP odometry pipeline, Scan Context place recognition and an SE(3) pose graph. Evaluated on KITTI with the official devkit metric, next to the KISS-ICP paper and the authors' own package run through the same evaluation code.
flowchart LR
A[Velodyne scan] --> B[vertical-angle correction<br/>range crop, 2 voxel grids]
B --> C[constant-velocity<br/>prediction]
C --> D[point-to-point ICP<br/>to local voxel map]
D --> E[adaptive threshold<br/>sigma from past corrections]
D --> F[map update]
D --> G[every 5th scan:<br/>Scan Context]
G --> H[candidates: ring key k-d tree,<br/>drift budget, column shift]
H --> I[ICP verification<br/>from the found yaw]
I --> J[pose graph<br/>chi-square loop check]
- KISS-ICP from the paper. Point-to-point ICP against a local voxel map (at most 20
points per voxel), a Geman-McClure kernel and a correspondence threshold that adapts to how
far ICP had to correct the motion prediction in the past. All seven parameters are the
paper's. The authors' package (
kiss-icp) runs through the same loader and metric as a reference row, so differences are in the method and not in the evaluation. - KITTI's vertical-angle error. The HDL-64E in KITTI has vertical beam angles that are off by about 0.2 degrees, which bends the flat ground into a shallow cone. Without the correction used by IMLS-SLAM and KISS-ICP, the error on sequence 04 more than doubles (1.06 % instead of 0.45 %, see the ablations).
- Loop closure that refuses look-alikes. Scan Context finds revisited places, a drift budget (3 % of the distance driven since) removes candidates the odometry cannot have drifted to, and coarse-to-fine ICP from the yaw found by Scan Context verifies the match. ICP must align 70 % of the points within 0.5 m, which rejects look-alike stretches of the same straight street, and a chi-square test on the solved graph drops loops that disagree with everything else.
- Pose graph with a sparse Jacobian. 4541 poses, 4540 odometry edges and 143 loops are solved by SciPy's trust-region least squares in under two minutes: every residual touches two poses, so the Jacobian sparsity pattern is given explicitly.
- Only the data it needs. The Velodyne archive is 85 GB. The downloader reads the ZIP's central directory and the members of the requested sequences with HTTP range requests.
- Tested without the dataset. ICP must recover a known motion of a synthetic scene, the odometry must follow a simulated drive, Scan Context must find a rotated scan's yaw, the pose graph must remove drift with one correct loop and ignore a wrong one.
KITTI odometry, all scans of each sequence. Errors as in the KITTI devkit: average over all 100-800 m sub-trajectories (translation % / rotation deg per 100 m), plus ATE RMSE after a rigid alignment. Every sequence with ground truth (00-10, 22 km) runs on the same settings.
Odometry
| Sequence | This repo: t % / r deg/100 m / ATE m | kiss-icp package, same evaluation |
KISS-ICP paper: t % |
|---|---|---|---|
| 00 (3.7 km, urban) | 0.59 / 0.21 / 2.3 | 0.53 / 0.19 / 3.5 | 0.51 |
| 01 (2.5 km, highway) | 1.11 / 0.17 / 6.4 | 0.79 / 0.10 / 3.1 | - |
| 02 (5.1 km, urban/suburban) | 0.58 / 0.15 / 2.2 | 0.54 / 0.15 / 6.5 | - |
| 03 (0.6 km, country road) | 0.70 / 0.14 / 0.4 | 0.68 / 0.16 / 0.5 | - |
| 04 (0.4 km, straight road) | 0.45 / 0.13 / 0.3 | 0.39 / 0.13 / 0.2 | 0.36 |
| 05 (2.2 km, suburban) | 0.40 / 0.15 / 1.3 | 0.34 / 0.13 / 1.5 | - |
| 06 (1.2 km, suburban loop) | 0.29 / 0.08 / 0.3 | 0.28 / 0.08 / 0.4 | - |
| 07 (0.7 km, urban) | 0.40 / 0.23 / 0.3 | 0.38 / 0.16 / 0.3 | - |
| 08 (3.2 km, suburban) | 0.86 / 0.19 / 2.3 | 0.82 / 0.19 / 2.3 | - |
| 09 (1.7 km, suburban) | 0.57 / 0.16 / 1.1 | 0.53 / 0.13 / 1.3 | - |
| 10 (0.9 km, country road) | 0.58 / 0.20 / 0.8 | 0.51 / 0.19 / 0.7 | - |
| all, segment-weighted | 0.63 / 0.17 | 0.56 / 0.16 | 0.50 |
| seconds per scan (one CPU node) | 0.12 (NumPy + SciPy) | 0.04 (C++, TBB) | - |
Paper: Vizzo et al., RA-L 2023, Table VI (per-sequence, adaptive threshold); its average over 00-10 is 0.50 %. The authors' package scores within 0.02-0.06 percentage points of the paper with this evaluation code, so the metric matches. The reimplementation is 0.01-0.07 points behind the package on ten sequences, with the same input scans and parameters. The exception is 01, the highway and the hardest sequence for both (fast, with few structures beside the road), where the gap is 0.32 points (1.11 % against 0.79 %). The remaining difference is in implementation details: the package's parallel nearest-neighbour search over neighbouring voxels against a k-d tree over the map here. The reimplementation's ATE is lower than the package's on six of the eleven sequences, most on 00 and 02.
Loop closure (Scan Context + ICP verification + pose graph), sequences with accepted loops:
| Sequence | Loops | Rejected by the graph check | Odometry: t % / ATE m | With loop closure: t % / ATE m |
|---|---|---|---|---|
| 00 | 143 | 0 | 0.59 / 2.31 | 0.61 / 0.99 |
| 02 | 43 | 6 | 0.58 / 2.22 | 0.62 / 1.97 |
| 05 | 70 | 0 | 0.40 / 1.27 | 0.35 / 0.41 |
| 06 | 46 | 0 | 0.29 / 0.26 | 0.31 / 0.25 |
| 07 | 3 | 0 | 0.40 / 0.29 | 0.41 / 0.30 |
| 08 | 40 | 0 | 0.86 / 2.33 | 0.90 / 2.36 |
| 09 | 4 | 0 | 0.57 / 1.12 | 0.60 / 1.04 |
Loop closure cuts the absolute error where the drive revisits places after drift has built up: 57 % on 00 and 68 % on 05. It does not lower the relative error the KITTI metric measures: the pose graph spreads each correction over the scans between two loops, which fixes the global shape and leaves the local drift. On 06-09 the accepted loops change ATE by less than 0.1 m.
Every loop is verified twice before it reaches the graph. ICP must align 70 % of the points within 0.5 m, which rejects look-alike places such as two stretches of the same straight street, and a chi-square test on the solved graph removes any loop that disagrees with the rest (on 02 it removed 6). A unit test feeds the graph a deliberately wrong loop.
Ablations (odometry only, same code, one setting changed)
| Variant | 04: t % / r deg/100 m | 07: t % / r deg/100 m |
|---|---|---|
| full (kernel scale sigma / 3, vertical-angle correction) | 0.45 / 0.13 | 0.40 / 0.23 |
| without the vertical-angle correction | 1.06 / 0.61 | 0.51 / 0.37 |
kernel scale sigma (the kiss-icp 1.x code default) |
0.49 / 0.13 | 0.44 / 0.28 |
The vertical-angle correction matters most on 04, a straight road with few vertical structures: the coned ground is most of what ICP sees there. The paper's kernel width (sigma / 3) is slightly better than the package's current default with this implementation. Over all 11 sequences, the vertical-angle correction halves the drift: 0.63 % with it, 1.19 % without.
pip install -e ".[dev]"
bash scripts/download_kitti.sh 04 07 # 2.7 GB of the 85 GB archive, via range requests
slam-run --data data/kitti --sequences 04 07 --out runs/odometry
slam-run --data data/kitti --sequences 07 --loops --out runs/slam
slam-summary --runs runs/odometry runs/slamslam-run writes NN.txt per sequence in the KITTI pose format (left-camera frame, readable
by the official devkit), NN_metrics.json and NN_trajectory.png; with --loops also
NN_odometry.txt, the trajectory before the pose graph. Settings can be changed from the
command line, e.g. --set odometry.kernel_scale=1.0 or --set correct_vertical_angle=false.
pip install -e ".[reference]" adds the authors' package for slam-reference.
Everything runs on CPU, one array task per sequence:
bash scripts/setup_cluster.sh && bash scripts/download_kitti.sh # login node, ~45 GB
python3 slurm/generate.py odometry --submit # 11 tasks
python3 slurm/generate.py slam --submit
python3 slurm/generate.py reference --submit # the kiss-icp package
python3 slurm/generate.py no-correction --submit # ablation
python3 slurm/generate.py summary --submitsrc/lidar_slam/
datasets/kitti.py Velodyne reader, vertical-angle correction, calibration, range-request download
voxel.py voxel keys, downsampling, the local map (capped voxels + k-d tree)
icp.py point-to-point ICP with Geman-McClure IRLS, adaptive threshold
odometry.py the KISS-ICP pipeline: preprocessing, prediction, registration, map update
scan_context.py descriptor, ring key, column-shift distance
loop_closure.py candidate search, ICP verification, chi-square loop rejection
pose_graph.py SE(3) residuals, sparse least squares
metrics.py KITTI relative errors (devkit procedure), ATE
reference.py the kiss-icp package through the same loader and metric
run.py, summary.py CLI, tables; plots.py for the figures
tests/ synthetic scenes for ICP, odometry, Scan Context and the pose graph
- Why point-to-point ICP? The KISS-ICP paper argues that with a good motion prediction, an adaptive threshold and a robust kernel, plain point-to-point ICP is as accurate as point-to-plane or feature-based methods and has almost nothing to tune. The same seven parameters are used on every sequence, nothing is tuned per drive.
- Why keyframes for loop closure? Scan Context and ICP verification run on every 5th scan only (0.5 s of driving), and a loop must be at least 300 scans (30 s) old. Consecutive keyframes on a revisited street then give a run of loops, which is what pulls a long stretch of the trajectory into place rather than one pose.
- Loop weights. Odometry edges get 2 cm and 0.002 rad standard deviations, loops 20 cm and 0.005 rad. Loops come from registering two single scans, which is less precise than registering a scan to a dense local map, and a looser loop edge limits the damage of one that slips past the checks.
- Frames. KITTI's reference poses describe the left camera. LiDAR poses are converted with
the extrinsic calibration (
T_cam = Tr T_velo Tr^-1) before evaluation. - Not included: deskewing (the KITTI odometry scans are already motion-compensated), IMU input and a global map. A long-term map for road inspection would need those.
- I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, C. Stachniss. KISS-ICP: In Defense of Point-to-Point ICP - Simple, Accurate, and Robust Registration If Done the Right Way. IEEE RA-L, 2023.
- G. Kim, A. Kim. Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map. IROS 2018.
- J.-E. Deschaud. IMLS-SLAM: Scan-to-Model Matching Based on 3D Data. ICRA 2018 (vertical-angle correction for KITTI's HDL-64E).
- A. Geiger, P. Lenz, R. Urtasun. Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite. CVPR 2012.