Iterative Closest Point (ICP)

SLAM 스터디 ppt

• SLAM 이란?
• 왜 이것을 공부하고자 하였는지?
• Basic Mathematics - Linear Algebra
• Introduction to SLAM
• Self-Driving Lectures 7.1~7.3
• SLAM Roadmap
• 3DVFH* / VINS-Mono

Iterative Closest Point (ICP)

Kabsch algorithm

the rotation of P into Q starts with two sets of paired points, P and Q. $\left[\begin{array}{ccccc}{x}_1& & y_1& & z_1\\ x_2& & y_2& & z_2\\ ...& & ...& & ...\\ x_N& & y_N& & z_N\end{array}\right]$ Three steps: 1. a translation 2. the computation of a covariance matrix 3. the computation of the optimal rotation matrix

Translation

sets of coordinates translated to coincides their centroid with the origin of the coordinate system.

Computation of the covariance matrix

$$H=P^{T}Q$$

or, summation notation

$$H_{ij}=\sum _{k=1}^N{P}_{ki}{Q}_{kj}$$

$H$ is a cross-covariance matrix when $P$ and $Q$ are seen as data matrices

Computation of the optimal rotation matrix

the optimal $R$ based on the matrix formula

$$R=(H^{T}H{)}^{1/2}{H}^{-1}$$

(but if case of $H$ not having an inverse, this solution becomes complicated) If SVD available, the optimal rotation $R$ can be calculated

1. calculate the SVD of the covariance matrix $H$

$$\begin{gathered} H= \\ U\Sigma \\ {V}^T \end{gathered}$$

2. decide whether we need to correct our rotation matrix to ensure a right-handed coordinate system

$$\begin{gathered} d= \\ sign(\det ( \\ V \\ {U}^T)) \end{gathered}$$

3. calculate optimal rotation matrix $R$

$$\begin{gathered} R= \\ V\left[\begin{array}{ccccc}1& & 0& & 0\\ 0& & 1& & 0\\ 0& & 0& & d\end{array}\right] \\ {U}^T \end{gathered}$$

Orthogonal Procrustes problem

= a matrix approximation one is given two matrices $A$ and $B$ and asked to find an orthogonal matrix $\text{ohm}$ which most closely maps $A$ to $B$.

$$\begin{gathered} R=\arg \underset{\text{ohm}}{\min }\Vert \text{ohm} \\ A- \\ B{\Vert }_F\text{subject to}{\text{ohm}}^T\text{ohm}= \\ I \end{gathered}$$

where $\Vert \cdot {\Vert }_F$