A visual study of ICP variants for Lidar Odometry

TL;DR

This work tackles robust lidar odometry by analyzing how different ICP variants behave under real-world effects such as dynamic objects and non-overlapping regions. The authors introduce a novel visualization method that opens the ICP loop and uses interpolated transformations in $SE(3)$ to plot the objective $\\epsilon$ along a controlled path, enabling qualitative comparison across variants. They integrate LOAM-style features, reciprocal correspondences, an ego blind spot filter, and an Octree Correspondence Filter to mitigate common failure modes, demonstrating improvements on KITTI scenes, including urban and highway scenarios. The approach provides practical insights into which ICP variants are more reliable under specific motions and highlights filtering strategies that enhance robustness, with potential extensions to probabilistic ICP methods. The work has implications for designing more robust lidar odometry pipelines in autonomous driving systems.

Abstract

Odometry with lidar sensors is a state-of-the-art method to estimate the ego pose of a moving vehicle. Many implementations of lidar odometry use variants of the Iterative Closest Point (ICP) algorithm. Real-world effects such as dynamic objects, non-overlapping areas, and sensor noise diminish the accuracy of ICP. We build on a recently proposed method that makes these effects visible by visualizing the multidimensional objective function of ICP in two dimensions. We use this method to study different ICP variants in the context of lidar odometry. In addition, we propose a novel method to filter out dynamic objects and to address the ego blind spot problem.

Figures (10)

• Figure 1: Simplified architecture of ICP with five processing stages. The data filtering stage subsumes functionalities that are done offline, thus before starting the iteration, e.g. downsampling of the point clouds. The data processing stage works online with the point clouds. At least, it applies the current estimate of $\mathbf{T}_e$ to $\mathcal{P}$. The correspondence determination stage, establishes the corresponding point pairs $(p_i \in \mathcal{P}, q_j \in \mathcal{Q})$. Subsequently, the point pairs are evaluated with an objective function $\epsilon$ which will be minimized. The minimizer passes its current estimate $\mathbf{T}_e$ back to the data processing stage. However, our visual method can be seen as an open-loop study of ICP. Instead of passing the current estimated transformation $\mathbf{T}_e$ from the minimizer, we pass interpolated transformations $\mathbf{T}_u$ to control the behavior of the building blocks and visualize the objective function $\epsilon$.
• Figure 2: Illustration of the ego blind spots due to the sensor mounting position on the roof. The figure shows two consecutive positions of the same ego car moving towards the right. The ego blind spot creates non-overlapping areas in the two lidar scans, labeled as no overlap.
• Figure 3: Illustration of the Octree Correspondence Filter after aligning frame $n$ and $n-1$, colored with orange and blue, in a common coordinate frame. The black squares denote voxels that are occupied in both scans. Blue and orange squares denote voxels only occupied by one of the two point clouds. Points within theses voxels will be deleted since they contain dynamic or non-overlapping points.
• Figure 4: The diagram shows two consecutive lidar scans, acquired from a car driving towards a left turn with a tree alongside the road. The most recent scan $n$, marked in orange, needs to be aligned within the reference coordinate system of the previous scan $n-1$, marked in blue. The start and end positions of the trajectory are at $u=0$ and $u=1$. The intervals $[-1, 0)$ and $(1, 2]$ evaluate the objective function for a finite set of extrapolated poses. The graph shows an ideal objective function, as the global minimum is at the ground truth pose at $u=1$.
• Figure 5: KITTI (seq.: 00, frame: 1500): Overview of the performance of the objective functions on an urban scene with mainly straight movement. Except for the point-to-point metric, all objective functions have their global minima nearly at the ground truth transformation $\mathbf{T}_{gt}$.