Iterative Closest Point
Posted on
This note is for Besl, P. J., & McKay, N. D. (1992). Method for registration of 3-D shapes. Sensor Fusion IV: Control Paradigms and Data Structures, 1611, 586–606..
describe a general method for registration of 3D shapes including free-form curves and surfaces
The method handles the full six-degrees of freedom and is based on the Iterative Closest Point (ICP) algorithm, which requires only a procedure to find the closest point on a geometric entity to a given point.
The ICP algorithm always converges monotonically to the nearest local minimum of a mean-square distance metric, and experience shows that the rate of convergence is rapid during the first few iterations. Therefore, given an adequate set of initial rotations and translations for a particular class of objects with a certain level of “shape complexity”, one can globally minimize the mean-square distance metric over all six degrees of freedom by testing each initial registration.
The proposed shape registration algorithm can be used with the following representations of geometric data:
- point sets
- line segment sets (polylines)
- implicit curves: $g(x, y, z) = 0$
- parametric curves: $(x(u), y(u), z(u))$
- triangle sets (faceted surfaces)
- implicit surfaces: $g(x, y, z) = 0$
- parametric surfaces: $(x(u, v), y(u, v), z(u, v))$
Mathematical Preliminaries
Let $A$ be a point set with $N_a$ points denoted $\vec{a}_i$: $A=\{\vec{a}_i\}_{i=1}^{N_a}$. The distance between the point $\vec{p}$ and the point set $A$ is
\[d(\vec{p}, A) = \min_{i\in\{1,\ldots,N_a\}}d(\vec p, \vec a_i)\,.\]Let $l$ be the line segment connecting the two points $\vec r_1$ and $\vec r_2$. The distance between the point $\vec p$ and the line segment $l$ is
\[d(\vec p, l) = \min_{u+v=1}\Vert u\vec r_1 + v\vec r_2 - \vec p\Vert\]where $u, v\in[0, 1]$. Let $L$ be the set of $N_l$ line segments denoted $l_i$: $L=\{l_i\}_{i=1}^{N_l}$. The distance between the point $\vec p$ and the line segment set $L$ is
\[d(\vec p, L) = \min_{i\in\{1,\ldots,N_l\}} d(\vec p, l_i)\,.\]Let $t$ be the triangle defined by the three points $\vec r_1, \vec r_2, \vec r_3$, the distance between the point $\vec p$ and the triangle $t$ is
\[d(\vec p, t) = \min_{u+v+w}\Vert u\vec r_1+v\vec r_2 + w\vec r_3-\vec p\Vert\]where $u\in [0, 1], v\in[0, 1]$ and $w\in[0, 1]$. Let $T=\{t_i\}_{i=1}^{N_t}$, then
\[d(\vec p, T) = \min_{i\in\{1,\ldots,N_t\}}d(\vec p, t_i)\,.\]Point to Parametric Entity Distance
Let $\vec r(\vec u)$ be a parametric curve when $\vec u=u\in \IR$, and be a parametric surface when $\vec u=(u, v)\in \IR^2$.
Then for a parametric entity $E$,
\[d(\vec p, E) = \min_{\vec r(\vec u)\in E}d(\vec p, \vec r(\vec u))\,.\]For the set of parametric entities $F={E_i}_{i=1}^{N_e}$,
\[d(\vec p, F) = \min_{i\in\{1,\ldots,N_e\}} d(\vec p, \vec E_i)\,.\]The first step towards computing $d(\vec p, E)$ is creating a simplex-based approximation (line segments or triangles). For a parametric space curve $C={\vec r(u)}$, one can compute a polyline $L(C,\delta)$ such that the piecewise-linear approximation never deviates from the space curve by more than a prespecified distance $\delta$. Then the $u_a$ argument value of the closest point from the line segment set can be obtained.
Similarly for a parametric surface $S={\vec r(u, v)}$, compute a triangle set $T(S, \delta)$ such that the piecewise-triangular approximation never deviates from the surface by more than a pre-specified distance $\delta$. Then obtain $(u_a, v_a)$ from the closest point in the triangle set.
Treat $\vec u_a$ as initial point, and employ a pure Newton’s approach to minimize
\[f(\vec u) = \Vert \vec r(\vec u) - \vec p\Vert^2\,.\]Point to Implicit Entity Distance
An implicit geometric entity is defined as the zero set of a possibly vector-valued multivariate function $\vec g(\vec r)=0$. The distance from a given point $\vec p$ to an implicit entity $I$ is
\[d(\vec p, I)=\min_{\vec g(\vec r)=0}d(\vec p, \vec r)=\min_{\vec g(\vec r)=0}\Vert \vec r-\vec p\Vert\,.\]For $J = {I_k}_{k=1}^{N_I}$,
\[d(\vec p, J) = \min_{k\in \{1,\ldots,N_I\}}d(\vec p, \vec I_k)\,.\]The first step to compute $d(\vec p, I)$ is also creating a simplex-based approximation (line segments or triangles). (TODO: but how?) It yields an approximate closest point $\vec r_a$.
Then solve the optimization problem with non-linear constraints
\[\min f(\vec r) = \Vert \vec r - \vec p\Vert^2 \;\text{subject to }\; \vec g(\vec r) = 0\,.\]Corresponding Point Set Registration
The unit quaternion (see wiki: https://en.wikipedia.org/wiki/Quaternion) is a 4-vector: $\vec q_R=\begin{bmatrix}q_0 & q_1 & q_2 & q_3\end{bmatrix}’$ where $q_0 \ge 0$ and $q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1$. The 3x3 rotation matrix generated by a unit rotation quaternion is $R$. (TODO)
Let $\vec q_T = \begin{bmatrix}q_4 & q_5 & q_6\end{bmatrix}$ be a translation vector. The complete registration state vector $\vec q$ is denoted $\vec q=[\vec q_R \mid \vec q_T]’$.
Let $P={\vec p_i}$ be a measured data point set to be aligned with a model point set $X={\vec x_i}$ where $N_x = N_p$ and where each point $\vec p_i$ corresponds to the point $\vec x_i$ with the same index.
The objective function is
\[f(\vec q) = \frac{1}{N_p}\sum_{i=1}^{N_p}\Vert \vec x_i - R(\vec q_R)\vec p_i - \vec q_T\Vert^2\,.\]The Iterative Closest Point Algorithm
A “data” shape $P$ is moved (registered, positioned) so as to be in best alignment with a “model” shape $X$.
Let
\[d(\vec p, X) = \min_{\vec x\in X}\Vert \vec x -\vec p\Vert\]
The Set of Initial Registrations
To get the global minimum, the only way to be sure is to find the minimum of all the local minima. The problem with reaching the desired global minimum with certainty is that it is difficult to precisely characterize in general the partitioning of the registration state space into local minima wells (regions of attraction) because this partitioning is potentially different for every possible different shape encountered.
Initial States for Global Matching
If the point data set $P$ covers a significant portion of the model shape $X$ such that the condition
\[\alpha_1\sqrt{tr(\Sigma_x)} \le \sqrt{tr(\Sigma_p)}\le \sqrt{tr(\Sigma_x)}\]holds for a sufficiently large factor, then it is generally not necessary to use multiple initial translation states as long as enough rotation states are used.
The general rule of thumb is the more complicated the object, the more initial states required. In general, every application may want to use a customized set of initial quaternions that will maximize the probability of choosing a good starting point early for the shapes of interest.
Local Shape Matching
The proposed registration is not useful if only a subset of the data point shape $P$ corresponds to the model shape $X$ or a subset of the model shape $X$, but it is still useful for local matching problems where the entire set of data points $P$ matches a subset of the model shape $X$. The drawback is that more than one initial translation must be used which increases the cost of computing the correct registration.
Experimental Results
Point Set Matching
The algorithm does not pay attention to the extra points or to the ordering of the points because it always pairs a given point on the first set to the closest point on the second set.
Curve Matching
define a 3D parametric space curve spline as a linear combination of cubic B-splines and control points, then rotate and translate its copy, also add noise to be the sample points.
it seems that in this example, it resembles point-to-curve, since discarding the order of the query points.
Surface Matching
- Bezier surface patch
- NRCC African Mask
- Terrain data
Conclusions
- ICP does not require pre-processing of 3D point data, such as smoothing, as long as the number of statistical outliers in near zero.