(SLAM) ICP-Iterative Closest Point

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 }$ 값을 구한다.
• 중심점을 빼면, 모든 점들이 중심을 원점으로 하는 좌표계에 있게 된다 : 모든 점들은 중심점을 기준으로한 상대적 위치를 나타내게 된다.

$$\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,\cdot ,{\mathbf{x}}_n{]}^T$와 $\mathbf{y}=[{\mathbf{y}}_1,{\mathbf{y}}_2,\cdot ,{\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 라 할 수 있다.

---

참고

• [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