(SLAM) ICP-Iterative Closest Point - 1

ICP 개요

• Iterative Closest Point (ICP) 알고리즘은 두 점군(pointcloud) 집합들이 주어졌을 때 각 점으로부터 최단 거리의 점들(Closest) 을 탐색하여 이를 바탕으로 반복적(Iterative) 으로 정합(Registration) 하는 방법을 말함.
• ICP는 주로 3D Points 데이터 정렬에 사용되며 Point-to-Point, Point-to-Plane 기법 등이 존재한다.
• 두개의 점군이 주어졌을 때, Corresponding point 사이의 거리를 계산하여, 이 거리를 최소화하는 Transformation matrix를 찾는 알고리즘.

---

ICP 데이터 예시

2D Point Cloud Data

• 2D 점군 데이터를 예로 들어 로봇이 2D 스캐너 따위를 통해 다음과 같은 데이터를 $t$, $t+1$ 시점에 대해 획득 하였을 때, 각 포인트 집합은 아래와 같다.

sourcePoints = [[-19, -15], [-18, -10], [-15, -9], [-14, -7], [-11, -6], [-9, -5], [-7, -6], [-4, -8], 
[-1, -11], [0, -14], [1, -17], [5, -20], [9, -24], [10, -25], [13, -24], [14, -25], [17, -25], 
[19, -22], [22, -18], [23, -16]]  
  
targetPoints = [[-12, -8], [-12, -2], [-10, 1], [-10, 4], [-9, 6], [-6, 7], [-3, 8], [-1, 8], 
[3, 6], [6, 5], [10, 3], [14, 1], [17, 1], [19, 0], [22, 1], [24, 2], [27, 4], 
[26, 7], [27, 11], [27, 15]]

3D Point Cloud Data

• 일반적으로 실제 환경에서 주어지는 데이터는 3D 점군 데이터가 주어진다.

sourcePoints = [[-19, -15, 7], [-18, -10, 6], [-15, -9, 5], [-14, -7, 4], [-11, -6, 8], [-9, -5, 5], 
[-7, -6, 7], [-4, -8, 6], [-1, -11, 4], [0, -14, 6], [1, -17, 8], [5, -20, 7], [9, -24, 5], 
[10, -25, 6], [13, -24, 8], [14, -25, 5], [17, -25, 7], [19, -22, 6], [22, -18, 8], [23, -16, 7]]  
  
targetPoints = [ [-12, -8, 9], [-12, -2, 11], [-10, 1, 10], [-10, 4, 12], [-9, 6, 9], [-6, 7, 10], 
[-3, 8, 8], [-1, 8, 12], [3, 6, 11], [6, 5, 9], [10, 3, 8], [14, 1, 12], [17, 1, 11], [19, 0, 10], 
[22, 1, 8], [24, 2, 9], [27, 4, 11], [26, 7, 12], [27, 11, 9], [27, 15, 10]]

---

Point-to-Point ICP

with known data associations

• 가장 쉬운 케이스. 두 점군의 각 포인트들 사이의 관계(correspondence) $r_i\leftrightarrow b_i$ 를 알고 있다면, 해당 정합 문제는 closed form 해가 존재하여 간단하게 문제를 풀 수 있다. (No initial guess, No iterative)
• 빨간색 점군을 ${\mathbf{P}}_{\mathbf{t}}=[P_t^1,P_t^2,\dots ,P_t^n{]}^T$ 파란색 점군을 ${\mathbf{P}}_{t+1}=[P_{t+1}^1,P_{t+1}^2,\dots ,P_{t+1}^n{]}^T$ 라고 하면 두 점군 사이의 관계는 다음과 같이 나타낼 수 있다.

$${\mathbf{p}}_{t+1}\approx \mathbf{R}{\mathbf{p}}_{\mathbf{t}}+\mathbf{t}\text{\hspace{1em}(1)}$$

• 위 식에서 $\mathbf{R}$은 Rotation을 위한 행렬, $\mathbf{t}$는 Translation을 위한 벡터라 할 수 있다.

• 이상적인 환경에서는 모든 포인트에 대해서 위 식을 만족해야 하지만 현실적으로 오차가 포함되기 때문에 전체 오차가 최소화 되는 방향으로 근사화 시키는 최적해를 구하는 방법을 이용해야 한다.

• 각 포인트들에 대한 오차는 다음과 같이 정의할 수 있다.

$$\mathbf{E}={\mathbf{p}}_{t+1}-(\mathbf{R}{\mathbf{p}}_t+\mathbf{t})\text{\hspace{1em}(2)}$$

• 따라서, 위 식은 다음과 같이 오차를 최소화하는 $\mathbf{R},\mathbf{t}$를 구하기 위한 최소제곱법(least square)문제로 변환할 수 있다.
• 이 때, ${\mathbf{p}}_t$와 ${\mathbf{p}}_{t+1}$은 벡터이기 때문에 norm을 적용하여 크기값인 스칼라 값으로 바꾸어 오차의 목적함수로 사용한다.

$$\arg \underset{\mathbf{R},\mathbf{t}}{\min }||{\mathbf{p}}_{t+1}-(\mathbf{R}{\mathbf{p}}_t+\mathbf{t})|{|}^2\text{\hspace{1em}(3)}$$

• 위 최소제곱법 문제는 비선형 항 $\mathbf{R}$을 포함하고 있기 때문에 정규방정식(normal equation) 형태로 풀 수 없다. 따라서 이를 해결하기 위해 공분산 행렬을 SVD 하는 방법과 비선형 최소제곱법(Gauss-Newton) 으로 푸는 방법이 존재 한다.

Covariance SVD-based solution

• 두 점군에 대한 공분산 행렬 $\mathbf{C}$ 를 구한 후 이를 SVD 분해하여 회전 행렬의 최적해 ${\mathbf{R}}^{\prime }$ 을 구할 수 있다.
• 우선 이를 위해 두 점군의 무게 중심(centroid) 을 먼저 구한다.

$$\begin{array}{rl}& \\ & {\bar{\mathbf{p}}}_t=\frac{1}{n}\sum {\mathbf{p}}_{t,i}\\ & \\ & {\bar{\mathbf{p}}}_{t+1}=\frac{1}{n}\sum {\mathbf{p}}_{t+1,i}\\ & \end{array}\text{\hspace{1em}(4)}$$

• 다음으로 기존의 모든 점들 $\mathbf{p}$에 대해 무게 중심 $\bar{\mathbf{p}}$를 빼준 ${\mathbf{p}}^{\prime }$ 값을 구한다.
• 중심점을 빼면, 모든 점들이 중심을 원점으로 하는 좌표계에 있게 된다 : 모든 점들은 중심점을 기준으로한 상대적 위치를 나타내게 된다. (모든 데이터들에서 평균을 빼면 그 데이터들의 중심(평균)은 0이 된다)

$$\begin{array}{rl}& \\ & {{\mathbf{p}}^{\prime }}_t={\mathbf{p}}_t-{\bar{\mathbf{p}}}_t\\ & {{\mathbf{p}}^{\prime }}_{t+1}={\mathbf{p}}_{t+1}-{\bar{\mathbf{p}}}_{t+1}\\ & \\ & {\mathbf{p}}_t={{\mathbf{p}}^{\prime }}_t+{\bar{\mathbf{p}}}_t\\ & {\mathbf{p}}_{t+1}={{\mathbf{p}}^{\prime }}_{t+1}+{\bar{\mathbf{p}}}_{t+1}\\ & \\ & \end{array}\text{\hspace{1em}(5)}$$

• 위 결과를 사용하여 목적함수를 다시 표현하면 아래와 같다.

$$\begin{array}{r}\\ \arg \underset{\mathbf{R},\mathbf{t}}{\min }||({{\mathbf{p}}^{\prime }}_{t+1}+{\bar{\mathbf{p}}}_{t+1})-\mathbf{R}({{\mathbf{p}}^{\prime }}_t+{\bar{\mathbf{p}}}_t)-\mathbf{t}|{|}^2\end{array}\text{\hspace{1em}(6)}$$

• 이 때, 두 점군의 이동 벡터는 $\mathbf{t}$ 는 $t+1$ 시점 점군의 무게중심 ${\bar{\mathbf{p}}}_{t+1}$과 $t$시점 회전한 점군의 무게중심 $\mathbf{R}{\bar{\mathbf{p}}}_t$ 의 차이로 설정한다.
• 즉, 두 점군의 상대 회전량을 정확히 보정한다면 두 점군은 정확히 $\mathbf{t}$ 만큼 떨어져 있다고 가정한다.

$$\mathbf{t}={\bar{\mathbf{p}}}_{t+1}-\mathbf{R}{\bar{\mathbf{p}}}_t\text{\hspace{1em}(7)}$$

• 위 식을 목적함수에 다시 대입하면 다음과 같이 $\mathbf{t}$ 항이 소거되어 $\mathbf{R}$을 찾는 문제로 단순화 된다.

$$\begin{array}{rl}& \\ & \arg \underset{\mathbf{R},\mathbf{t}}{\min }||({{\mathbf{p}}^{\prime }}_{t+1}+{\bar{\mathbf{p}}}_{t+1})-\mathbf{R}({{\mathbf{p}}^{\prime }}_t+{\bar{\mathbf{p}}}_t)-({\bar{\mathbf{p}}}_{t+1}-\mathbf{R}{\bar{\mathbf{p}}}_t)|{|}^2\\ & \\ & =\arg \underset{\mathbf{R},\mathbf{t}}{\min }||{{\mathbf{p}}^{\prime }}_{t+1}+{\bar{\mathbf{p}}}_{t+1}-\mathbf{R}{{\mathbf{p}}^{\prime }}_t-\mathbf{R}{\bar{\mathbf{p}}}_t-{\bar{\mathbf{p}}}_{t+1}+\mathbf{R}{\bar{\mathbf{p}}}_t|{|}^2\\ & \\ & =\arg \underset{\mathbf{R}}{\min }||{{\mathbf{p}}^{\prime }}_{t+1}-\mathbf{R}{{\mathbf{p}}^{\prime }}_t|{|}^2\\ & \end{array}\text{\hspace{1em}(8)}$$

• 위 식을 전개하면 $\mathbf{R}$이 소거되어 중간의 항만 $\mathbf{R}$과 관련있는 항이 된다.

$$\begin{array}{rl}& \\ ||{{\mathbf{p}}^{\prime }}_{t+1}-\mathbf{R}{{\mathbf{p}}^{\prime }}_t|{|}^2& =({{\mathbf{p}}^{\prime }}_{t+1}-\mathbf{R}{{\mathbf{p}}^{\prime }}_t{)}^T({{\mathbf{p}}^{\prime }}_{t+1}-\mathbf{R}{{\mathbf{p}}^{\prime }}_t)\\ & \\ & ={{\mathbf{p}}^{\prime }}_{t+1}^T{{\mathbf{p}}^{\prime }}_{t+1}-2{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t+{\mathbf{R}}^T{{\mathbf{p}}^{\prime }}_t^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t\\ & \\ & ={{\mathbf{p}}^{\prime }}_{t+1}^T{{\mathbf{p}}^{\prime }}_{t+1}-2{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t+\mathbf{I}{{\mathbf{p}}^{\prime }}_t^T{{\mathbf{p}}^{\prime }}_t\\ & \\ & =-2{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t+C\\ & \end{array}\text{\hspace{1em}(9)}$$

• 위 전개한 식을 보면, 회전행렬 $\mathbf{R}$의 특성에 의해 ${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$를 만족한다.

회전행렬의 성질

• 회전행렬은 3D공간 혹은 2D 공간 상의 벡터를 회전시키는 변환을 나타내는 행렬.
• 회전행렬은 다음 두 가지 조건을 만족해야함:
  • 길이 보존(벡터의 크기가 회전 전후로 변하지 않음)
    • $||\mathbf{Rv}||=||\mathbf{v}||$
  • 각도 보존(벡터들 간의 내적, 즉 각도 관계가 유지됨)
    • $(\mathbf{R}{\mathbf{v}}_1)\cdot (\mathbf{R}{\mathbf{v}}_2)={\mathbf{v}}_1\cdot {\mathbf{v}}_2$
• 위 성질로 인해, 아래 전개를 만족함.

$$\begin{array}{rl}& ||\mathbf{Rv}|{|}^2=(\mathbf{Rv}{)}^T(\mathbf{Rv})={\mathbf{v}}^T{\mathbf{R}}^T\mathbf{Rv}\\ & \\ & {\mathbf{v}}^T{\mathbf{R}}^T\mathbf{Rv}={\mathbf{v}}^T\mathbf{v}\text{(길이보존)}\\ & \\ & \therefore {\mathbf{R}}^T\mathbf{R}=\mathbf{I}\end{array}$$

• 즉, 회전행렬은 항상 직교행렬(otrhogonal)이다.

• 따라서 최적화 수식은 다음과 같이 다시 쓸 수 있다. 마이너스 부호(-)가 사라지면서 argmin 문제가 argmax 문제로 변환된다.

$$\begin{array}{rl}& \\ {\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\min }\left[-2{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t\right]\\ & \end{array}\text{\hspace{1em}(10)}$$

• 이 때 내부의 결과가 스칼라 값이므로 trace 연산의 성질을 이용할 수 있다.
• trace 는 행렬의 대각 성분을 모두 더하는 연산으로, 아래 cyclic permutation 성질은 다음을 만족한다.
  • $\mathrm{Tr}(\mathbf{ABC})=\mathrm{Tr}(\mathbf{CAB})=\mathrm{Tr}(\mathbf{BCA})$
• trace 연산을 아래와 같이 적용할 수 있다.

$$\begin{array}{rl}& \\ {\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\max }\left[{{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({{\mathbf{p}}^{\prime }}_{t+1}^T\mathbf{R}{{\mathbf{p}}^{\prime }}_t)\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}(\mathbf{R}{{\mathbf{p}}^{\prime }}_t{{\mathbf{p}}^{\prime }}_{t+1}^T)\right]\leftarrow \mathrm{Tr}(\mathbf{AB})=\mathrm{Tr}(\mathbf{BA})\\ & \\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}(\mathbf{R}\mathbf{C})\right]\\ & \end{array}\text{\hspace{1em}(11)}$$

• 위 식에서 ${{\mathbf{p}}^{\prime }}_t{{\mathbf{p}}^{\prime }}_{t+1}^T$는 두 점군의 공분산 행렬(covariance matrix)의 정의와 동일하다. 따라서 이를 $\mathbf{C}$로 치환해준다.

Covariance matrix of x, y

• 두데이터 집합 $\mathbf{x}=[{\mathbf{x}}_1,{\mathbf{x}}_2,\cdots ,{\mathbf{x}}_n{]}^T$와 $\mathbf{y}=[{\mathbf{y}}_1,{\mathbf{y}}_2,\cdots ,{\mathbf{y}}_n{]}^T$이 주어졌을 때 두 데이터의 공분산 행렬은 다음과 같이 구할 수 있다.
• 두 데이터의 평균을 구한다.

$\bar{\mathbf{x}}=\frac{1}{n}\sum {\mathbf{x}}_i$ $\bar{\mathbf{y}}=\frac{1}{n}\sum {\mathbf{y}}_i$

• 원본 데이터에서 각각 평균을 빼준다. (편차)

${\mathbf{x}}^{\prime }=\mathbf{x}-\bar{\mathbf{x}}$ ${\mathbf{y}}^{\prime }=\mathbf{y}-\bar{\mathbf{y}}$

• 두 데이터의 공분산 행렬 ${\mathbf{C}}_{\mathbf{xy}}$는 다음과 같이 구할 수 있다.

${\mathbf{C}}_{\mathbf{xy}}={\mathbf{x}}^{\prime }{\mathbf{y}}^{\prime T}$

• 따라서, 최적의 ${\mathbf{R}}^{\ast }$ 을 구하기 위해서는 다음 식을 풀어야 한다.

$$\begin{array}{rl}& \\ {\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}(\mathbf{RC})\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({\mathbf{RUDV}}^T)\right]\\ & \end{array}\text{\hspace{1em}(12)}$$

• 공분산 행렬 $\mathbf{C}$는 값이 마이너스(-)가 나올 수 없으므로 항상 positive (semi-)definite 행렬이다. 따라서 위 식의 두 번째 줄에서 $\mathbf{C}$를 SVD 분해하면 모든 특이값(singular value)는 항상 0보다 크거나 같다.

Method 1 for ${\mathbf{R}}^{\ast }$

Lemma

• 임의의 positive definite 행렬 ${\mathbf{AA}}^T$와 정규직교행렬(orhonormal) $\mathbf{B}$에 대하여 다음과 같은 코시-슈바르츠 부등식(Cauchy-Schwarz inequality)이 성립한다.

$$\mathrm{Tr}({\mathbf{AA}}^T)\ge \mathrm{Tr}({\mathbf{BAA}}^T)\text{\hspace{1em}(13)}$$

Proof

• 임의의 벡터 $\mathbf{u},\mathbf{v}$에 대한 코시-슈바르츠 부등식은 다음과 같다. (임의의 두 벡터의 내적의 절댓값은 각 벡터의 크기(norm)의 곱보다 크지 않다)​

$$|⟨\mathbf{u},\mathbf{v}⟩|\le \sqrt{⟨\mathbf{u},\mathbf{u}⟩}\cdot \sqrt{⟨\mathbf{v},\mathbf{v}⟩}$$ $$⟨\mathbf{u},\mathbf{v}⟩:\text{inner product}$$

• $\mathrm{Tr}({\mathbf{BAA}}^T)$는 다음과 같이 벡터 형태로 표현이 가능하다. (trace 의 cyclic permutation 성질을 이용)

$$\begin{array}{rl}& \\ \mathrm{Tr}({\mathbf{BAA}}^T)& =\mathrm{Tr}({\mathbf{A}}^T\mathbf{BA})\\ & =\sum _i{\mathbf{a}}_i^T\mathbf{B}{\mathbf{a}}_i\end{array}$$

• 위 식을 코시-슈바르츠 부등식에 대입하면 다음과 같은 식을 얻는다.

$$\begin{array}{rl}& \\ {\mathbf{a}}_i^T\mathbf{B}{\mathbf{a}}_i\le & \sqrt{({\mathbf{a}}_i^T{\mathbf{a}}_i)({\mathbf{a}}_i^T{\mathbf{B}}^T\mathbf{B}{\mathbf{a}}_i)}\\ & \sqrt{({\mathbf{a}}_i^T{\mathbf{a}}_i)({\mathbf{a}}_i^T\mathbf{I}{\mathbf{a}}_i)}\\ & \sqrt{({\mathbf{a}}_i^T{\mathbf{a}}_i)({\mathbf{a}}_i^T{\mathbf{a}}_i)}\\ & {\mathbf{a}}_i^T{\mathbf{a}}_i\\ & \\ {\mathbf{a}}_i^T\mathbf{B}{\mathbf{a}}_i\le & {\mathbf{a}}_i^T{\mathbf{a}}_i\end{array}$$

• 따라서 다음과 같은 Lemma를 얻는다.

$$\therefore \mathrm{Tr}({\mathbf{AA}}^T)\ge \mathrm{Tr}({\mathbf{BAA}}^T)$$

정리

• 만약 $\mathbf{R}={\mathbf{VU}}^T$로 정의하면 식 $(12)$에서 정규직교행렬 ${\mathbf{U}}^T\mathbf{U}$가 서로 소거되어 ${\mathbf{VDV}}^T$만 남는다.
• 이를 통해 아래와 같이 ${\mathbf{AA}}^T$ 형태로 만들 수 있다. (${\mathbf{R}}^{\prime }$은 임의의 정규직교행렬(orthonormal))

$$\begin{array}{rl}& \\ \mathrm{Tr}({\mathbf{RUDV}}^T)& =\mathrm{Tr}({\mathbf{VU}}^T{\mathbf{UDV}}^T)\\ & =\mathrm{Tr}(\mathbf{V}{\mathbf{DV}}^T)\\ & =\mathrm{Tr}(\mathbf{V}{\mathbf{D}}^{\frac{1}{2}}{\mathbf{D}}^{\frac{1}{2}}{\mathbf{V}}^T)\\ & =\mathrm{Tr}(\mathbf{V}{\mathbf{D}}^{\frac{1}{2}})({\mathbf{D}}^{\frac{1}{2}}{\mathbf{V}}^T)\leftarrow {\mathbf{AA}}^T\text{form}\\ & =\mathrm{Tr}({\mathbf{AA}}^T)\\ & \ge \mathrm{Tr}({\mathbf{R}}^{\prime }\mathbf{A}{\mathbf{A}}^T)\\ & \end{array}\text{\hspace{1em}(14)}$$

• 이를 다시 정리하면 다음과 같다.

$$\mathrm{Tr}(\mathbf{RC})=\mathrm{Tr}({\mathbf{VU}}^T\mathbf{C})\ge \mathrm{Tr}({\mathbf{R}}^{\prime }\mathbf{A}{\mathbf{A}}^T)=\mathrm{Tr}({\mathbf{R}}^{\prime }\mathbf{RC})\text{\hspace{1em}(15)}$$

• 위 식의 의미는 $\mathbf{R}$은 $\mathbf{R}={\mathbf{VU}}^T$인 경우에 모든 다른 임의의 회전행렬 ${\mathbf{R}}^{\prime }\mathbf{R}$보다 큰 값을 가진다는 의미이다.
• 따라서 argmax의 해가 된다.

$$\begin{array}{rl}& \\ & \mathbf{R}={\mathbf{VU}}^T\\ & \mathbf{t}=\mathbf{R}{\bar{\mathbf{p}}}_t-{\bar{\mathbf{p}}}_{t+1}\\ & \end{array}\text{\hspace{1em}(16)}$$

Method 2 for ${\mathbf{R}}^{\ast }$

• 식 $(12)$를 다시 보면 trace 내부의 값을 최대화해야 한다. 이 때, 공분산 행렬 $\mathbf{C}$는 특이값이 항상 0보다 크거나 같고, 따라서 대각행렬 $\mathbf{D}$의 모든 값은 항상 0보다 크거나 같다.
• $\mathbf{R},\mathbf{U},\mathbf{V}$는 정규직교행렬(orthonormal) 행렬이므로 대각행렬과 곱해지면 항상 trace 값은 $\mathbf{D}$ 자체보다 작아지게 된다.

$$\mathrm{Tr}(\mathbf{D})\ge \mathrm{Tr}({\mathbf{RUDV}}^T)$$

• trace 성질에 의해 순서를 바꿔주게 되면 다음과 같다.

$$\begin{array}{rl}& \\ & \\ {\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({\mathbf{RU}\cdot \mathbf{DV}}^T)\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({\mathbf{DV}}^T\cdot \mathbf{RU})\right]\\ & \\ & \end{array}$$

• 만약 $\mathbf{R}={\mathbf{VU}}^T$ 인 경우 정규직교행렬들이 소거되어 최대 trace 값을 가진다.

$$\begin{array}{rl}& \\ & \\ {\mathbf{R}}^{\ast }& =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({\mathbf{DV}}^T\mathbf{RU})\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}({\mathbf{DV}}^T{\mathbf{VU}}^T\mathbf{U})\right]\\ & =\arg \underset{\mathbf{R}}{\max }\left[\mathrm{Tr}(\mathbf{D})\right]\\ & \end{array}\text{\hspace{1em}(17)}$$

• 따라서 앞서 method 1에서 유도한 식 $(16)$과 동일한 $\mathbf{R}$ 값을 구할 수 있다.

$$\begin{array}{rl}& \\ & \mathbf{R}={\mathbf{VU}}^T\\ & \mathbf{t}=\mathbf{R}{\bar{\mathbf{p}}}_t-{\bar{\mathbf{p}}}_{t+1}\\ & \end{array}\text{\hspace{1em}(18)}$$

• 회전행렬의 최적해를 구할 때 $\mathbf{R}$이 so(3)군의 조건을 만족하려면 반드시 행렬식이 +1 이어야 한다 ($\det (\mathbf{R})=1$).
• 하지만 실제 ICP 해를 구하다보면 행렬식이 -1이 되는 경우가 발생하는데, 이는 reflection(특정 축 기준으로 상하좌우 반전)이 된 경우이다.
• 이러한 degenerate 케이스를 방지하기 위해 일반적으로 다음과 같이 회전 행렬의 최적해를 구한다.

$$\mathbf{R}=\mathbf{V}\left[\begin{array}{ccc}1& & \\ & 1& \\ & & \det ({\mathbf{VU}}^T)\end{array}\right]$$

---

with unknown data associations

• 이전 섹션에서 최적해 ${\mathbf{R}}^{\ast },{\mathbf{t}}^{\ast }$는 두 점군 사이의 대응 관계(correspondences)를 모두 알고 있는 경우에 사용할 수 있는 방법이다.
• 하지만 일반적으로 센서에서 얻어지는 두 점군 데이터는 서로의 대응관계를 알기 어렵다. > 대응 관계를 모르는 경우, 바로 구할 수 있는 closed form 해는 존재하지 않는다.
• 따라서 하나의 점으로부터 가장 가까운 점(closest point)을 대응점 쌍으로 설정하여 반복적(interative)으로 최적해를 구하는 알고리즘이 ICP 라 할 수 있다.

ICP 과정

• ICP의 전체적인 과정은 아래와 같다.

1. source 점군과 target 점군의 평균(또는 centroid) ${\bar{\mathbf{p}}}_t,{\bar{\mathbf{p}}}_{t+1}$ 를 구한다.
2. 각 점군에 두 평균을 빼줌으로써 평균 0으로 정규화 한다. ${\mathbf{p}}^{\prime }=\mathbf{p}-\bar{\mathbf{p}}$
3. 각 source 점들마다 최단 거리의 target 점들을 대응점으로 설정한다. (nearest neighborhood 알고리즘 등)
4. 공분산 행렬을 SVD 분해하여 회전행렬 $\mathbf{R}$을 구하고 평균 간 차이를 통해 이동 벡터 $\mathbf{t}$를 구한다. (${\mathbf{R}}_j={\mathbf{VU}}^T,{\mathbf{t}}_j={\bar{\mathbf{p}}}_{t+1}-{\mathbf{R}}_j{\bar{\mathbf{p}}}_t$ )
5. source 점군을 최적해를 통해 회전 및 이동 시킨다. (${\mathbf{p}}_{t,j+1}={\mathbf{R}}_j{\mathbf{p}}_{t,j}+{\mathbf{t}}_j$)
6. 두 점군의 거리가 충분히 가까워질 때까지 위 과정을 반복한다.

• 위 ICP 과정을 그림으로 나타내면 다음과 같다.

• 지금까지 설명한 알고리즘을 가장 기본적인 ICP 알고리즘이라는 뜻에서 일반적으로 Vanila ICP라고 부른다.
• Vanila ICP는 구현하기 비교적 쉽고 초기 추정값(initial guess)가 정확하면 잘 동작한다는 장점이 있으나 일반적으로 수렴하는데 많은 반복 횟수가 필요하며 잘못된 correspondence에 영향을 많이 받아 결과가 안 좋아질 수 있는 한계점이 존재한다. 
• 이러한 Vanila ICP의 한계점을 극복하기 위해 모든 점들이 아닌 점군의 부분 집합(e.g., 특징점)만 추출하여 ICP를 수행한다거나 point-to-plane과 같이 다른 대응 관계를 이용한다거나 correspondence에 가중치를 두어 outlier의 영향력을 축소한다거나 잠재적인 point outlier를 아예 제거하여 보다 강건한 ICP를 수행하는 등 많은 변형 ICP 방법들이 존재한다.

• 비선형 최소제곱법(Gauss-Newton)을 사용하여 ICP 문제를 푸는 Least squares ICP 방법 : (SLAM) ICP, Iterative Closest Point - 2

Code

• 매칭된 점군을 이용한 Point-to-Point ICP Python 코드
• 점군 $\mathbf{P}$를 $\mathbf{R},\mathbf{t}$를 이용해 회전한 ${\mathbf{P}}^{\prime }$에 대해서 $\mathbf{P}$와 ${\mathbf{P}}^{\prime }$을 입력으로, ICP를 통해 다시 $\mathbf{R},\mathbf{t}$를 구하는 예제.
• ICP (Iterative Closest Point) 와 Point Cloud Registration - gaussian37

import numpy as np
from scipy.stats import special_ortho_group
 
def icp_svd(p_src, p_dst):
    """
    Calculate the optimal rotation (R) and translation (t) that aligns
    two sets of matched 3D points P and P_prime using Singular Value Decomposition (SVD).
 
    Parameters:
    - p_src: np.array of shape (3, n) -- the first set of points.
    - p_dst: np.array of shape (3, n) -- the second set of points.
 
    Returns:
    - R: Rotation matrix
    - t: Translation vector
    """
    # Step 1: Calculate the centroids of P and P_prime
    centroid_p_src = np.mean(p_src, axis=1, keepdims=True)  # Centroid of P    
    centroid_p_dst = np.mean(p_dst, axis=1, keepdims=True)  # Centroid of P'   
 
    # Step 2: Subtract centroids
    q_src = p_src - centroid_p_src    
    q_dst = p_dst - centroid_p_dst
 
    # Step 3: Construct the cross-covariance matrix H
    H = q_src @ q_dst.T
 
    # Step 4: Perform Singular Value Decomposition
    U, Sigma, Vt = np.linalg.svd(H)
    V = Vt.T
 
    # Step 5: Calculate rotation matrix R    
    R_est = V @ U.T
 
    result = True
    # Step 6: Ensure R is a proper rotation matrix
    det_R_est = np.linalg.det(R_est)
    if np.abs(det_R_est - (-1)) < 0.0001: # check detminant
        if np.abs(Sigma[-1]) < 0.00001 == 0: # check reflection
            # Reflection in coplanar cse
            V[:,-1] *= -1  # Flip the sign of the last column of V
            R_est = V @ U.T
        else:
            # can't get rotation matrix
            result = False
 
    # Step 7: Calculate translation vector t        
    t_est = centroid_p_src - R_est @ centroid_p_dst
    t_est = t.reshape(3, 1)
 
    return R_est, t_est, result
 
# Example usage with dummy data
# Define the set of points P
P = np.random.rand(3, 30) * 100
 
# Set a random Rotation matrix R (ensuring it's a valid rotation matrix)
R = special_ortho_group.rvs(3)
 
# Set a random Translation vector t
t = np.random.rand(3, 1) * 10
 
# Apply the rotation and translation to P to create P_prime
P_prime = R @ P + t
 
################################### Calculate R and t using ICP with SVD
R_est, t_est, _ = icp_svd(P, P_prime)
 
print("R : \n", R)
print("R_est : \n", R_est)
print("R and R_est are same : ", np.allclose(R,R_est))
print("\n")
 
print("t : \n", t)
print("t_est : \n", t_est)
print("t and t_est are same : ", np.allclose(t, t_est))
print("\n")
 
# R : 
#  [[-0.65800821  0.75067865 -0.05921784]
#  [-0.56577368 -0.54475838 -0.61898179]
#  [-0.49691583 -0.3737912   0.78316971]]
# R_est : 
#  [[-0.65800821  0.75067865 -0.05921784]
#  [-0.56577368 -0.54475838 -0.61898179]
#  [-0.49691583 -0.3737912   0.78316971]]
# R and R_est are same :  True
 
# t : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t_est : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t and t_est are same :  True
 

• RANSAC 을 활용한 ICP Python 함수

def icp_svd_ransac(p_src, p_dst, n=3, num_iterations=20, inlier_threshold=0.01):
    # n = 3  # Number of points to estimate the model, for affine 3D at least 4 points
    # num_iterations = 20  # Number of iterations
    # inlier_threshold = 0.1  # Inlier threshold, this might be a count or a percentage based on your needs
    best_inliers = -1
    best_R = None
    best_t = None
 
    for _ in range(num_iterations):
        # Step 1: Randomly select a subset of matching points
        indices = np.random.choice(p_src.shape[1], n, replace=False)
        points_src_sample = p_src[:, indices]        
        points_dst_sample = p_dst[:, indices]
 
        # Step 2: Estimate rotation and translation using SVD based ICP
        R, t, result = icp_svd(points_src_sample, points_dst_sample)
        
        # Step 3 and 4: Calculate error and inliers
        p_src_transformed = R @ p_src + t
        errors = np.linalg.norm(p_dst - p_src_transformed, axis=0)
        inliers = np.sum(errors < inlier_threshold)
 
        # Step 5: Check if current iteration has the best model
        if inliers > best_inliers:            
            best_inliers = inliers
            best_R = R
            best_t = t
 
        # Step 6: Check terminating condition
        if best_inliers > inlier_threshold or _ == num_iterations - 1:
            break
 
    return best_R, best_t, best_inliers

---

참고

• [SLAM] Iterative Closest Point(ICP) 개념 정리 (+ point-to-plane, GICP)
• ICP (Iterative Closest Point) 와 Point Cloud Registration - gaussian37
• ICP & Point Cloud Registration - Part 2: Unknown Data Association (Cyrill Stachniss, 2021) - YouTube
• Jupyter Notebook Viewer