Closing an 8.7 km LiDAR SLAM loop

During my NavINST internship, I wanted to go past LiDAR odometry and close a full urban loop. The vehicle drove an 8.7 km route and returned to its starting point. The stock pipeline had other plans: its estimated trajectory ended 93.1 m away.

I got that gap down to 3.15 m by fixing a timestamp bug and improving how difficult loop candidates were checked. Horizontal absolute position error also fell from 22.5 m to 10.38 m. The plot finally looked like the route that had actually been driven.

That should have been the victory lap. Then full 3D APE came back at 38.93 m because the LiDAR-only trajectory was wandering vertically. This post covers both sides: what I fixed, what the system still got wrong, and how separating the metrics turned a confusing result into a useful one.

For comparison, this is the actual Urban01 route overlaid on the city map:

The setup

I ran KISS-SLAM, built on KISS-ICP, on the NavINST Urban01 daytime route. The estimator only saw Velodyne VLP-16 scans. This was LiDAR-only by design. GNSS/INS stayed out of the estimator and was used afterward as the reference trajectory.

The high-level flow was:

1. deskew each LiDAR scan
2. estimate the vehicle's motion by aligning consecutive scans with ICP
3. combine roughly 100 m of scans into local submaps
4. search older submaps for places the vehicle may have revisited
5. verify those matches using their 3D geometry
6. add verified loop-closure constraints and optimize the full trajectory

A loop closure tells the optimizer that two separately estimated positions represent the same physical place. That is what lets the system correct drift accumulated between the first and second visit.

Stock KISS-SLAM already helped. With loops enabled it beat a no-loops baseline on several metrics. But it still missed the obvious one: the car clearly returned to the start, and the trajectory did not.

That was the clue. The loop-closure system was finding some revisits, but it missed the one between the end of the route and the starting area.

Bug 1: timestamps were paired with the wrong points

A spinning LiDAR captures points at different times. To deskew a scan you need each point's XYZ and its timestamp to stay matched.

The reader filtered invalid XYZ rows but kept the original timestamp array. Point i could therefore receive timestamp j. On the first scan alone, about 99.95% of retained points had the wrong time. A point cloud with a shuffled clock is not a great place to start.

The fix was small but strict: apply one mask to both arrays.

valid = ~np.any(np.isnan(points), axis=1)
valid_points = points[valid]
valid_times = timestamps[valid]

That cleaned up local motion error, but it did not recover the final loop. One bug down, one very visible 93 m gap still on the screen.

Bottleneck 2: the right loop never got a fair ICP shot

When I looked at the start and end scans side by side, the geometry was clearly similar. So why no closure?

Because finding a candidate and proving it are different steps.

The retrieval stage ranked old submaps using a compact description of each scene. ICP then checked whether the 3D point clouds actually aligned. Stock KISS-SLAM mostly passed only its highest-ranked candidate directly to fine ICP. Fine ICP works well when its starting guess is already close, but it can reject a real revisit when that guess is weak.

The first and final submaps, numbered 0 and 81, really did overlap. With a better initial alignment, they passed the geometric-overlap test. The match was valid. The original verification process just never gave it a fair trial.

What I built

I built a conservative top-k verification path:

• examine the 5 highest-ranked candidates instead of only the first
• reject candidates captured too close together in time or implausibly far apart in the current trajectory
• use coarse-to-fine ICP to give weaker initial alignments a chance to converge
• add no more than one verified loop-closure constraint for each search

This process identified eight valid loop closures during the run. Most importantly, it added a constraint between the first submap, 0, and the final submap, 81, near the route start. I checked every constraint against saved submap geometry and the held-out INS trajectory, so I was not grading my own homework with a prettier plot.

What improved

I compared my verifier against stock KISS-SLAM after applying the same timestamp fix to both. APE measures absolute position error against the aligned reference trajectory. RPE measures error in the estimated motion over each 100 m interval.

The table is satisfying. The route closes, horizontal APE drops by 53.9%, and heading improves. Overall 3D RPE barely changes because both versions use the same scan-to-scan LiDAR odometry. My changes improve loop closure, not the underlying local-motion estimate.

At first glance, that looked like a clean win. Then the full 3D metrics arrived and made the result look much worse. Treating everything as one number would have hidden what was actually happening.

Separating horizontal and vertical error

For the improved run:

The horizontal and vertical metrics tell very different stories. Horizontal RPE is 0.785 m per 100 m, which is consistent with the 10.38 m horizontal APE over the full route. Vertical RPE is 2.659 m per 100 m, about 3.4 times larger. Globally, the aligned estimate spans 159.86 m in elevation while the reference only spans 21.18 m. In other words, the estimator invented a rather large hill.

This distinction also prevents a misleading calculation. Dividing 10.38 m of horizontal APE by the 8.7 km route length does not produce a drift rate. APE measures absolute position error after alignment. RPE measures relative motion over a fixed interval, so the 100 m horizontal RPE is the appropriate local drift measure here.

Diagnosing the vertical failure

I did not want to label it "pitch drift" just because the z-axis looked bad. I wrote a diagnostic that compared reference and estimated height over time, fit a linear trend, and checked whether the error shape repeated across different runs.

The vertical error ranged from -66.81 m to +83.51 m. It did not steadily increase or decrease with time. A linear trend explained little of the shape (R² = 0.122), so a single accumulating pitch bias was not a good description. The error wandered with the route.

The stronger clue was repeatability. The vertical-error shape correlated between 0.90 and 0.98 across all evaluated runs. I changed which loop closures were added, but the same deformation kept returning. That points to the shared scan-to-scan LiDAR odometry rather than the loop-closure optimizer as the main source.

I also tried to disprove the diagnosis. Before measuring error, I aligned each trajectory to the reference using one rigid 3D rotation and translation, with no rescaling. This can correct a constant offset or tilt, but it cannot flatten a shape that deforms differently along the route. Applying the known LiDAR-to-INS coordinate transform changed aligned 3D APE by only 3.6 mm, so a forgotten sensor offset was not secretly ruining the score.

That wording matters. The evidence localizes and characterizes the failure, but it does not prove one exact internal cause. Adding an IMU gravity constraint, GNSS, or map priors would be the natural next experiment. It would also turn this into a sensor-fusion project. Chasing a sub-meter headline by tuning more loop thresholds would not address the limitation I had actually measured.

Evaluation had one earlier trap too. My first metrics pass reported impossible values such as 134 m of drift per 100 m for every run. That was a body-frame conversion mistake, not a SLAM result. Numbers that absurd are annoying, but useful: they tell you to debug the evaluator before blaming the algorithm.

What I took away

• Small data-loading bugs can look like algorithm failures. Timestamp parity is part of the sensor model.
• Retrieval and verification are a team. A stricter ICP step cannot save a candidate that retrieval never surfaces.
• Combined 3D metrics can hide a healthy horizontal estimate behind one failing axis. Separating APE and RPE by axis localized the problem.
• One strong headline metric is not the whole story. The 96.6% smaller endpoint gap did not make this a finished 3D navigation stack.
• Evaluation is part of the engineering. Frame conventions, alignment, and metric definitions need the same care as the estimator.

The endpoint reduction is the clean headline, but the part I learned the most from came afterward. The system succeeded horizontally, failed vertically, and forced me to prove why those two statements could both be true.

References

• NavINST dataset · paper
• KISS-ICP · KISS-SLAM

Built on open-source KISS-SLAM / KISS-ICP. This writeup covers my integration, debugging, and evaluation work on NavINST.