Scan Matching

Iterative Closest Point (ICP)

Goal: Estimate the transformation to move one cloud so that it is aligned with the other.

Resources

• https://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf

Not sure where I got this from, maybe ChatGPT, or F1TENTH, or

Math

I still really don’t understand how ICP work. I need to actually look at the math for this.

Process

1. Take two scans of 2D/3D point cloud
2. Compute the center of mass and shift the point clouds on top of each other

Use point-to-point metric

$putequationhere$

Two steps that you repeat over and over until convergence

1. Data Association Step
   • Minimize the distance between point pairs.
   • Compute correspondences from $P$ to $Q$. For every $p_i$ we search the closest $q_i$ to it.
2. Transformation Step
   1. Compute the rotation and translation
   2. Compute rotation using SVD to get $t$

Use the previous scan and current scan to align them.

Use Singular Value Decomposition to do this computation.

1. Make some initial guess of R
2. For each point in new scan (t=k+1), find closest point in previous set (t=k) (Correspondence Search)
3. Make another better guess
4. Set next guess to current guess, repeat steps 2-4 until convergence

the naive metric is the sum of squared distances. This is sensitive to noise. A better metric point-to-point metric.

Iteratively

ICP can be very slow, you need better estimates.

Absolute Orientation

Point-to-Line ICP (PL-ICP)

Two improvements

1. Change from point-to-point to point-to-line metric
2. Clever tricks to allow Fast Correspondence Search

From Visual SLAM book

ICP is just a non-linear optimization problem.

The solution can be divided into 2 ways:

1. Using Linear Algebra (SVD)
2. Nonlinear optimization (similar to Bundle Adjustment)