Point-to-Point ICP
Given two scans, the target \(P\) and the source \(Q\), repeat until convergence:
-
Find pairs of closest points \((p_i, q_i)\). Use a kD-tree to speed up the search, e.g., a k-nearest neighbor lib.
-
Find rotation \(R\) and translation \(t\) which minimize \(\text{min}_{R,t} \sum_i || p_i - (Rq_i + t) ||^2\).
This algorithm aligns the source point cloud with the target point cloud.
Computation of rotation \(R\) and translation \(t\)
-
Compute the center of mass of the target and source point clouds
\[
\mu_p = \frac{1}{N_x} \sum p_i \quad \text{and}\quad \mu_q = \frac{1}{N_q} \sum q_i
\]
-
Introduce the vectors
\[
v_i = p_i - \mu_p \quad \text{and} \quad \hat{v}_j = q_j - \mu_q
\]
-
Compute the cross-covariance matrix
\[
W = \sum v_i \cdot \hat{v}_i^T
\]
-
Solve the singular value decomposition (SVD) for \(W\)
\[
W = U \cdot \chi \cdot V^T
\]
The rotation matrix and the translation vector are
\[
\begin{align}
R &= U\cdot V^T\\
t &= \mu_p - R\mu_q\\
\end{align}
\]
Alignment of the point clouds
Transform source point cloud
\[
q_i \leftarrow R q_i + t
\]
Note:
In order to visualize the IPC alignment we slow down the animation to 5 fps
The JavaScript Code