Graph-based Online Lidar Odometry with Retrospective Map Refinement

This paper presents a graph-based online Lidar odometry method that enhances trajectory estimation and map accuracy by registering scans against multiple overlapping submaps and performing retrospective refinement of their anchor points, achieving superior performance on automotive datasets while maintaining real-time operation.

The Gist

Imagine you are walking through a dark, foggy forest with a flashlight. Your goal is to figure out exactly where you are and where you've been, just by looking at the trees and rocks around you. This is essentially what Lidar Odometry does for self-driving cars: it uses laser beams (instead of a flashlight) to map the world and track the car's movement.

The paper you shared introduces a new, smarter way to do this. Here is the breakdown using simple analogies.

The Old Way: The "Single Reference Point" Problem

Traditionally, these systems work like a hiker who picks one giant rock as their reference point.

1. They walk a bit, look at the rock, and say, "I'm 10 steps away."
2. They walk another bit, look at the same rock, and say, "Now I'm 20 steps away."
3. The Problem: If they miscounted their first step (a tiny error), that mistake is baked into the rock's position. Every step they take after that is built on that wrong foundation. Over time, the hiker thinks they are in a different country than they actually are. This is called drift.

The New Way: The "Multiple Overlapping Maps" Approach

The authors propose a method called "Graph-based Online Lidar Odometry with Retrospective Map Refinement." That's a mouthful, so let's call it the "Team of Guides" approach.

Instead of relying on just one rock, the car creates multiple overlapping sub-maps (like a team of guides standing at different spots along the path).

1. The "Multitude-of-Maps" Strategy

Imagine you are walking through a city. Instead of just looking at the Eiffel Tower to know where you are, you also look at the Louvre, the Arc de Triomphe, and a local bakery.

• How it works: The car takes its current laser scan and compares it against four or five different maps it has already built, not just one.
• The Benefit: If one map is slightly blurry or the car is confused about one landmark, the other maps act as a safety net. It's like asking four different people for directions; if three say "Left" and one says "Right," you know to go Left. This makes the system much more robust.

2. The "Retrospective Refinement" (The Time-Travel Fix)

This is the coolest part. In the old way, once you made a mistake, it stayed a mistake forever.

• The Analogy: Imagine you are writing a diary. In the old method, if you wrote "I ate pizza at 5 PM" but you actually ate it at 5:15 PM, you couldn't fix it later.
• The New Method: The car keeps a "Pose Graph" (a network of all the places it has been). As it gathers new information, it can look back in time and say, "Wait, based on where I am now, I must have been slightly there at 5 PM, not here."
• It gently nudges the positions of the old maps to make them fit together perfectly. It's like editing a group photo: if someone blinked in the first shot, the software doesn't just delete the photo; it adjusts the whole group's position so everyone looks perfect together.

3. The "Smart Filter" (Gating)

Sometimes, the car might look at a map that is too far away or in a boring place (like a long, empty tunnel) where there aren't enough features to match.

• The system has a "gating mechanism." It's like a bouncer at a club. If a map is too far away or the match is too weak, the bouncer says, "No entry." This prevents bad data from messing up the calculation.

Why is this a big deal?

The paper tested this against other top-tier systems using real driving data (like the famous KITTI dataset).

• Accuracy: It was significantly more accurate (5–15% better) than the competition. It didn't just get the current position right; it kept the entire history of the trip accurate.
• Speed: Even though it's doing complex math and "time-traveling" to fix past errors, it does it fast enough to run in real-time on a car (about 10–19 frames per second).
• Efficiency: Unlike some other complex systems that try to connect every single point to every other point (which is like trying to hold hands with everyone in a stadium at once), this method is smart. It only connects the necessary dots, keeping the computer load light.

The Bottom Line

Think of this new method as a self-correcting GPS.

• Old GPS: "I think I'm here. Oops, I'm actually here. Now I'm here." (Errors pile up).
• New GPS: "I think I'm here. Let me check my notes from 5 minutes ago, 10 minutes ago, and 15 minutes ago. Okay, I see a pattern. Let me adjust my past location slightly so my current location makes perfect sense."

By using multiple maps and constantly fixing the past based on the present, this system allows self-driving cars to navigate with much higher precision, even in tricky environments like tunnels or long straight roads where other systems tend to get lost.

Technical Summary

Here is a detailed technical summary of the paper "Graph-based Online Lidar Odometry with Retrospective Map Refinement" by Kurda, Steuernagel, and Baum.

1. Problem Statement

Lidar-only odometry aims to estimate the trajectory of a mobile platform using a stream of lidar scans. Traditional approaches typically rely on scan-to-map registration, where a new scan is aligned against a single, static, evolving map.

• The Core Issue: In traditional methods, once a scan is integrated into the map, its points are fixed. Any registration error introduced at an early stage propagates forward, accumulating over time and causing trajectory drift.
• The Limitation of Existing Solutions: While fixed-lag smoothing and SLAM (Simultaneous Localization and Mapping) can correct past errors by using future measurements, they are often designed for post-processing or require significant computational resources, making them unsuitable for real-time, online applications where immediate localization is critical.

2. Methodology

The authors propose a novel multitude-of-maps approach that combines the real-time capabilities of online odometry with the accuracy of retrospective smoothing. The system operates in a continuous loop with the following key components:

A. Multi-Map Registration (The "Multitude-of-Maps" Approach)

Instead of registering the current scan against a single global map, the system maintains a set of overlapping submaps (keyframes), denoted as $K = \{K_{\alpha_1}, ..., K_{\alpha_N}\}$.

• Process: The current lidar scan is independently registered against all active submaps using a robust Iterative Closest Point (ICP) algorithm.
• Benefit: This generates multiple constraints (edges) connecting the current pose to various past anchor points, leveraging redundant information to improve robustness against outliers and low-overlap scenarios.

B. Pose Graph Optimization with Gating

The constraints derived from the multiple registrations are inserted into a pose graph.

• Optimization Goal: The graph is optimized to simultaneously:
  1. Estimate the optimal current pose ($x_t$).
  2. Retrospectively refine the anchor points of the past submaps.
• Gating Mechanism: To ensure robustness, a gating function $\theta(e)$ excludes constraints where the error exceeds a threshold ($g_{max}$). This prevents erroneous registrations (e.g., in tunnels or featureless areas) from corrupting the global optimization.
• Efficiency: The graph remains small and sparse. Only one new node and a few edges are added per time step, allowing the Gauss-Newton optimizer to converge in fewer than 5 iterations.

C. Keyframe Management and Marginalization

• Insertion: A new keyframe is created if the distance to the last keyframe exceeds a threshold ($k_{min} = 6m$).
• Marginalization: If the current scan is too close to the last keyframe, it is marginalized (removed) to keep the graph size constant. Crucially, the information from the marginalized scan is preserved by combining pairs of constraints into a single new constraint between the remaining nodes, ensuring no information is lost while maintaining graph sparsity.

D. Preprocessing

The system uses a double voxel hash map approach:

1. Deskewing: Raw scans are deskewed based on motion prediction.
2. Subsampling: A dense subset ($H_t$) is created for map updates, while a sparser subset ($I_t$) is used for the ICP registration to reduce computational load.

3. Key Contributions

1. Hybrid Online/Retrospective Framework: The method achieves real-time performance while enabling retrospective map refinement, a feature usually reserved for offline post-processing.
2. Multitude-of-Maps Strategy: By registering against multiple overlapping submaps rather than a single static map, the system reduces error propagation and increases robustness.
3. Lightweight Optimization: Unlike dense graph methods (e.g., FORM) that create high-dimensional residuals, this approach uses a sparse graph with one constraint per registration, keeping computational overhead negligible.
4. Robust Gating: The introduction of an error-gating mechanism specifically addresses the issue of low-overlap alignments in geometrically poor environments.

4. Experimental Results

The method was evaluated on three real-world automotive datasets: KITTI, MulRan, and Odyssey. It was compared against state-of-the-art methods: KISS-ICP, MAD-ICP, and KISS-SLAM.

• Accuracy:
  • The proposed method consistently outperformed competitors.
  • On KITTI, it improved accuracy by ~7% (KITTI metric) and ~2.5% (RPE100) over the second-best method (KISS-SLAM).
  • On MulRan, it achieved a 10% improvement in the KITTI metric and 6% reduction in RPE100.
  • On Odyssey, it outperformed the best reference method (MAD-ICP) by ~15% and KISS-ICP by ~30%.
• Real-Time Performance:
  • The system maintained >10 FPS on all datasets (ranging from 10.73 to 19.73 FPS), meeting the requirement for online autonomous navigation.
  • While slightly slower than KISS-ICP, it offers significantly higher accuracy.
• Ablation Studies:
  • Retrospective Refinement: Removing the ability to refine past poses (GO-LAST) resulted in lower accuracy, proving the value of global optimization.
  • Multiple Registrations: Forcing a single keyframe (SINGLE) degraded performance, confirming that redundant information from multiple submaps is critical.
  • Information Matrices: Using full covariance matrices (INFO-FULL) degraded performance compared to diagonal matrices, likely due to the breakdown of the Hessian approximation in ICP.

5. Significance

This work bridges the gap between online odometry (speed, immediate localization) and SLAM/Smoothing (accuracy, consistency).

• Practical Impact: It demonstrates that high-precision, drift-free localization is achievable in real-time without requiring expensive sensor fusion (e.g., IMU or GPS) or heavy post-processing.
• Scalability: The sparse graph design ensures the method scales well computationally, making it suitable for deployment on resource-constrained mobile robots and autonomous vehicles.
• Future Direction: The authors identify adaptive keyframe selection (based on motion or geometry) as a future improvement to reduce redundancy in static environments and increase it in dynamic ones.

In conclusion, the paper presents a robust, real-time lidar odometry system that significantly advances the state-of-the-art by effectively utilizing redundant map information and retrospective optimization to minimize trajectory drift.