ICP (Iterative Closest Point) 와 Point Cloud Registration

이번 글에서는 이론적으로 ICP (Iterative Closest Point)에 대한 내용과 Cyrill Stachniss의 ICP 강의 내용을 정리해 보도록 하겠습니다..
강의는 총 1시간 분량의 강의 3개로 구성되어 총 3시간 분량의 강의입니다.
아래 참조 내용은 실제 코드 구현 시 도움 받은 내용입니다.

참조 : https://mr-waguwagu.tistory.com/36
참조 : https://github.com/minsu1206/3D/tree/main

Matched Points의 Point-to-Point ICP 개념

관련 논문 : Least-Squares Fitting of Two 3-D Point Sets

먼저 이번 글에서 다룰 강의 내용에 앞서서 간략하게 다룰 내용은 아래 강의 내용 중 Part 1: Known Data Association & SVD에 해당하는 내용입니다. 뒤의 강의 내용의 이해를 돕기 위하여 아래와 같이 먼저 정리하였습니다.

아래와 같이 2개의 점군 \(P, P'\) 가 있고 각 \(i\) 번째 점인 \(p_{i}, p'_{i}\) 가 서로 대응되는 것을 알고 있는 상황이라고 가정합니다.

\[P = {p_{1}, p_{2}, ... , p_{n}}\]
\[P' = {p'_{1}, p'_{2}, ..., p'_{n}}\]

위 점군에서 \(P\) 는 source에 해당하고 \(P'\) 는 destination에 해당합니다. 즉, \(P \to P'\) 로 변환하기 위한 관계를 알고자 하는 것이 핵심입니다.
따라서 \(P, P'\) 의 각 원소인 \(p_{i}, p'_{i}\) 의 관계를 알기 위해서는 Rotation을 위한 행렬 \(R\) 과 Translation을 위한 벡터 \(t\) 가 필요합니다.

\[\forall_{i} \ \ p'_{i} = Rp_{i} + t\]

이상적인 환경에서는 모든 \(i\) 에 대하여 \(p'_{i} = Rp_{i} + t\) 를 만족해야 하지만 현실적으로 오차가 포함되기 때문에 전체의 오차가 최소화 되는 방향으로 근사화 시키는 최적해를 구하는 방법을 이용하여 ICP를 적용합니다.
RGB-D 카메라를 이용하거나 이미지에서 Feature Extraction 및 Matching을 하여 점들끼리 쌍을 매칭한 점 군 \(P, P'\) 를 구한 경우에 지금부터 설명할 방법을 사용할 수 있습니다.

먼저 아래와 같이 \(i\) 점의 오차를 정의 합니다.

\[e_{i} = p'_{i} - (Rp_{i} + t)\]

풀어야 할 문제는 모든 에러 \(e_{i}\) 를 최소화 시키는 목적 함수를 만들고 목적 함수를 최소화 시키는 문제를 푸는 것입니다. 따라서 다음과 같이 오차 제곱 합의 목적 함수를 만듭니다.

\[\min_{R,t} \frac{1}{2} \sum_{i=1}^{n} \Vert (p'_{i} - (Rp_{i} + t)) \Vert^{2}\]

위 식에서 \(p_{i}, p'_{i}\) 는 벡터이기 때문에 norm을 적용하여 크기값인 스칼라 값으로 바꾸어서 목적 함수의 결과로 둡니다.
위 식을 좀 더 단순화하여 전개하기 위해 두 점군의 중심 위치 (centroid)를 정의해 보도록 하겠습니다.

\[p_{c} = \frac{1}{n} \sum_{i=1}^{n}(p_{i})\]
\[p'_{c} = \frac{1}{n} \sum_{i=1}^{n}(p'_{i})\]

앞에서 정의한 목적 함수에 ① \(-p'_{c} + Rp_{c} + p'_{c} -Rp_{c} = 0\) 을 추가한 뒤 ② 제곱식을 전개해 보도록 하겠습니다.

\[\begin{align} \frac{1}{2}\sum_{i=1}^{n} \Vert p'_{i} - (Rp_{i} + t) \Vert^{2} &= \frac{1}{2}\sum_{i=1}^{n} \Vert p'_{i} - Rp_{i} - t - p'_{c} + Rp_{c} + p'_{c} - Rp_{c} \Vert^{2} \\ &= \frac{1}{2}\sum_{i=1}^{n} \Vert (p'_{i} - p'_{c} - R(p_{i} - p_{c})) + (p'_{c} - Rp_{c} - t) \Vert^{2} \\ &= \frac{1}{2}\sum_{i=1}^{n} (\Vert p'_{i} - p'_{c} - R(p_{i} - p_{c}) \Vert^{2} + \Vert p'_{c} - Rp_{c} - t \Vert^{2} + 2(p'_{i} - p'_{c} - R(p_{i} - p_{c}))^{T}(p'_{c} - Rp_{c} - t)) \end{align}\]

위 식에서 다음 부분은 0이 됩니다.

\[\sum_{i=1}^{n} (p'_{i} - p'_{c} - R(p_{i} - p_{c})) = 0\]

왜냐하면 모든 \(p'_{i}\) 의 총합과 \(p'_{c}\) 를 \(n\) 번 더한 것과 값이 같고 모든 \(p_{i}\) 의 총합과 \(p_{c}\) 를 \(n\) 번 더한 것과 값이 같기 때문입니다.
따라서 앞에서 전개한 식에서 \(\sum_{i=1}^{n} (p'_{i} - p'_{c} - R(p_{i} - p_{c}))\) 부분을 소거하면 다음과 같이 정리 가능합니다.

\[\frac{1}{2}\sum_{i=1}^{n} (\Vert p'_{i} - p'_{c} - R(p_{i} - p_{c}) \Vert^{2} + \Vert p'_{c} - Rp_{c} - t \Vert^{2} + 2(p'_{i} - p'_{c} - R(p_{i} - p_{c}))^{T}(p'_{c} - Rp_{c} - t))\]
\[\frac{1}{2}\sum_{i=1}^{n} (\Vert p'_{i} - p'_{c} - R(p_{i} - p_{c}) \Vert^{2} + \Vert p'_{c} - Rp_{c} - t \Vert^{2})\]
\[\therefore \min_{R, t} J = \frac{1}{2}\sum_{i=1}^{n} ( \color{red}{\Vert p'_{i} - p'_{c} - R(p_{i} - p_{c}) \Vert^{2}} + \color{blue}{\Vert p'_{c} - Rp_{c} - t \Vert^{2}})\]

위 식의 빨간색 부분에 해당하는 항은 Rotation만 관련되어 있고 파란색 부분에 해당하는 항은 Rotation과 Translation 모두 관련되어 있지만 추가적으로 \(p_{c}, p'_{c}\) 만 연관되어 있습니다.
따라서 파란색 항은 Rotation만 구할 수 있으면 나머지 \(p_{c}, p'_{c}\) 는 주어진 점들을 통해 계산할 수 있으므로 \(\Vert p'_{c} - Rp_{c} - t \Vert^{2} = 0\) 으로 식을 두면 \(t\) 를 구할 수 있습니다.

빨간색 항 또한 조금 더 간단하게 만들기 위하여 다음과 같이 치환합니다.

\[q_{i} = p_{i} - p_{c}\]
\[q'_{i} = p'_{i} - p'_{c}\]

위 치환식의 정의에 따라서 \(q_{i}, q'_{i}\) 각각은 각 점 \(p_{i}, p'_{i}\) 가 점들의 중앙인 \(p_{c}, p'_{c}\) 로 부터 얼만큼 떨어져 있는 지 나타냅니다.
치환식을 이용하여 다음과 같은 2가지 스텝으로 Rotation과 Translation을 구해보도록 하겠습니다.

① Rotation \(R^{*}\) (예측값)를 다음 최적화 식을 통하여 구해보도록 하겠습니다.

\[R^{*} = \text{argmin}_{R} \frac{1}{2} \sum_{i=1}^{n} \Vert q'_{i} - R q_{i} \Vert^{2}\]

② 앞에서 구한 \(R^{*}\) 을 이용하여 \(t^{*}\) 을 구합니다.

\[t^{*} = p'_{c} - R^{*}p_{c}\]

먼저 ① 에 해당하는 \(R^{*}\) 을 구하는 방법에 대하여 살펴보도록 하겠습니다.

\[\begin{align} \frac{1}{2} \sum_{i=1}^{n} \Vert q'_{i} - R q_{i} \Vert^{2} &= \frac{1}{2} \sum_{i=1}^{n}(q'_{i} - R q_{i} )^{T}(q'_{i} - R q_{i} ) \\ &= \frac{1}{2} \sum_{i=1}^{n}(q{'}_{i}^{t}q{'}_{i} + q_{i}^{t}R^{t}Rq_{i} - q{'}_{i}^{t}Rq_{i} - q_{i}^{t}R^{t}q'_{i}) \\ &=\frac{1}{2} \sum_{i=1}^{n}(q{'}_{i}^{t}q{'}_{i} + q_{i}^{t}q_{i} - q{'}_{i}^{t}Rq_{i} - (q_{i}^{t}R^{t}q'_{i})^{t}) \quad (\because q_{i}^{t}R^{t}q'_{i} \text{ : scalar} ) \\ &= \frac{1}{2} \sum_{i=1}^{n}(q{'}_{i}^{t}q{'}_{i} + q_{i}^{t}q_{i} -q{'}_{i}^{t}Rq_{i} -q{'}_{i}^{t}Rq_{i}) \\ &= \frac{1}{2} \sum_{i=1}^{n}(q{'}_{i}^{t}q{'}_{i} + q_{i}^{t}q_{i} -2q{'}_{i}^{t}Rq_{i}) \end{align}\]

위 식에서 첫번째 항과 두번째 항은 \(R\) 과 관련이 없습니다. 따라서 실제 최적화를 위한 함수는 다음과 같이 변경될 수 있습니다.

\[\frac{1}{2} \sum_{i=1}^{n}(q{'}_{i}^{t}q{'}_{i} + q_{i}^{t}q_{i} -2q{'}_{i}^{t}Rq_{i}) \Rightarrow \frac{1}{2}\sum_{i=1}^{n} -2q{'}_{i}^{t}Rq_{i} = -\sum_{i=1}^{n}q{'}_{i}^{t}Rq_{i}\]

따라서 \(\min_{R, t} J\) 인 목적 함수를 최소화 하기 위해서는 \(\sum_{i=1}^{n}q{'}_{i}^{t}Rq_{i}\) 를 최대화하여 목적함수를 최소화 할 수 있도록 설계해야 합니다. 즉, \(\text{Maximize : } \sum_{i=1}^{n}q{'}_{i}^{t}Rq_{i}\) 를 만드는 것이 실제 풀어야할 최적화 문제가 됩니다.

그러면 \(\sum_{i=1}^{n}q{'}_{i}^{t}Rq_{i}\) 를 최대화 하기 위한 조건을 살펴보도록 하겠습니다.
식을 살펴보면 \(q_{i}, q'_{i}\) 는 벡터이고 \(R\) 은 3 x 3 크기의 행렬이므로 최종적으로 하나의 스칼라 값을 가지게 됩니다.
summation 내부의 결과가 스칼라 값이므로 trace 연산( \(\text{Trace}()\) )의 성질을 이용할 수 있습니다.
trace는 행렬의 대각 성분을 모두 더하는 연산입니다. 만약 최종 결과가 스칼라 값 (1 x 1 행렬)이고 이 값에 trace 연산을 적용하면 그 값 그대로 이기 때문에 값에 영향을 주지 않습니다. 따라서 결과값에 영향을 주지 않으면서 trace 연산의 성질들을 이용할 수 있습니다.
trace 연산의 Cyclic Permutation 성질은 다음을 만족합니다. 아래 기호 \(A, B, C\) 각각은 행렬입니다.

\[\text{Trace}(ABC) = \text{Trace}(CAB) = \text{Trace}(BCA)\]

이 성질을 이용하여 앞에서 전개하였던 \(\sum_{i=1}^{n} q_{i}^{T} R q'_{i}\) 의 식을 변경해 보도록 하겠습니다.

\[\begin{align} \sum_{i=1}^{n}q{'}_{i}^{t}Rq_{i} &= \text{Trace}\left(\sum_{i=1}^{n} q{'}_{i}^{t}Rq_{i} \right) \\ &= \text{Trace}\left(\sum_{i=1}^{n} q_{i}q{'}_{i}^{t}R \right) \\ &= \text{Trace}\left(\sum_{i=1}^{n} Rq_{i}q{'}_{i}^{t}\right) \\ &= \text{Trace}\left(R\sum_{i=1}^{n} q_{i}q{'}_{i}^{t}\right) \\ &= \text{Trace}(RH) \text{, where } \left(H = \sum_{i=1}^{n} q_{i}q{'}_{i}^{t}\right) \end{align}\]

위 식에서 \(q_{i} : (3 \times 1) \text{column vector}\) 이고 \(q{'}_{i}^{t} : (1 \times 3) \text{row vector}\) 이므로 \(\sum_{i=1}^{n} q_{i}q{'}_{i}^{t}\) 는 \(3 \times 3\) 행렬입니다. 따라서 SVD (Singular Value Decomposition)을 이용하여 행렬 분해를 할 수 있습니다. 특이값 분해 관련 내용은 아래 링크에 자세하게 설명되어 있습니다.
- 특이값 분해 : https://gaussian37.github.io/math-la-svd/

따라서 특이값 분해를 하면 다음과 같이 분해할 수 있습니다.

\[H = \sum_{i=1}^{n} q'_{i} q_{i}^{T} = U \Sigma V^{T}\]

여기서 \(U, V\) 는 orthonormal matrix이고 \(\Sigma\) 는 diagonal matrix이며 대각 성분은 특이값을 가집니다. 특이값과 고유값은 다음의 관계를 가집니다.

\[\text{singular value} = \sqrt{\text{eigen value}} > 0\]

SVD를 이용하여 분해한 값과 앞의 식을 이용하여 식을 좀 더 전개해 보도록 하겠습니다.

\[\text{Trace}(RH) = \text{Trace}(R \sum_{i=1}^{n} q'_{i} q_{i}^{T}) = \text{Trace}(R U \Sigma V^{t})\]

위 식에서 최종적으로 구해야 하는 값은 \(R\) 이며 \(\text{Trace}(RH)\) 가 최대화 되도록 \(R\) 을 잘 설정하면 문제를 해결할 수 있습니다.
결론적으로 \(R = VU^{T}\)이 최적해의 솔루션이 됩니다. 최적해를 찾는 과정을 이해하기 위하여 다음 소정리(Lemma)를 이용하도록 하겠습니다.
소정리를 이해하기 위하여 Positive Difinite Matrix의 이해가 필요합니다. 관련 정의는 아래 링크에서 참조하시기 바랍니다.
- 양의 정부호 행렬 (Positive Definite Matrix)

먼저 Positive Definite Matrix를 만족하는 \(AA^{t}\) 행렬과 orthonormal matrix \(B\) 가 있다고 가정하겠습니다. 이 때, 다음 식을 만족하는 것이 소정리 입니다.

\[\text{Trace}(AA^{t}) \ge \text{Trace}(BAA^{t})\]

행렬 \(A\) 의 \(i\) 번째 열벡터를 \(a_{i}\) 로 가정하겠습니다. 이 때, Trace의 Cycle Permutation 성질과 Trace의 정의를 이용하면 다음과 같이 식을 전개할 수 있습니다.

\[\begin{align} \text{Trace}(BAA^{t}) &= \text{Trace}(A^{t}BA) \\ &= \sum_{i}a_{i}^{t}(Ba_{i}) \end{align}\]

(Cauchy)Schwarz inequality를 이용하면 다음과 같이 식을 전개할 수 있습니다. (Cauchy)Schwarz inequality 정의는 다음과 같습니다.

\[\vert \langle u, v \rangle \vert \le \sqrt{\langle u, u \rangle} \cdot \sqrt{\langle v, v \rangle}\]
\[u, v \text{ : vector}\]
\[\langle u, v \rangle \text{ : inner product}\]

(Cauchy)Schwarz inequality 의 정의에 따라 아래 식을 전개해 보도록 하겠습니다.

\[a_{i}^{t}(Ba_{i}) \le \sqrt{(a_{i}^{t}a_{i})(a_{i}^{t}B^{t}Ba_{i})} = a_{i}^{t}a_{i} \quad (\because B \text{ : orthonormal matrix})\]
\[\therefore \ \text{Trace}(BAA^{t}) \le \sum_{i} a_{i}^{t}a_{i} = \text{Trace}({AA^{t}})\]

위 성질을 이용하여 앞에서 증명하던 내용을 이어서 증명해 보도록 하겠습니다.

\[H = U \Sigma V^{t}\]
\[U, V \text{ : orthonormal matrix}\]
\[\Sigma \text{ : diagonal matrix with singular value}\]
\[\text{Let } X = VU^{t}\]

위 식에서 \(X\) 는 orthonormal matrix 입니다. 왜냐하면 \(U, V\) 가 모두 orthonormal matrix이기 때문에 그 곱또한 orthonormal matrix를 만족하기 때문입니다.

\[X = VU^{t}\]
\[XH = VU^{t}U \Sigma V^{t} = V \Sigma V^{t}\]

Singular Value는 모두 0보다 크기 때문에 eigenvalue는 Singular Value의 제곱이므로 모든 eigenvalue는 양수임을 알 수 있습니다. eigenvlaue가 모두 양수이면 Positive Definite Matrix 조건을 만족하기 때문에 (양의 정부호 행렬 (Positive Definite Matrix) 참조) \(XH\) 는 Positive Definite Matrix 입니다.
따라서 앞에서 정리한 소정리를 이용할 수 있습니다.

\[\text{Trace}(XH) \ge \text{Trace}(BXH) \quad (\forall \text{ orthonormal matrix } B )\]

이 내용을 앞에서 정리한 식과 연결해서 설명해 보도록 하겠습니다.

\[\text{Trace}(RH) = \text{Trace}(VU^{T}H) \ge \text{Trace}(BRH)\]

즉, \(\text{Trace}(RH)\) 에서 \(R = VU^{T}\) 일 때, 모든 경우의 수에서 상한값을 가질 수 있으므로 최대화를 만족하기 위한 \(R^{*}\) 은 \(R^{*} = VU^{T}\) 을 통해서 구할 수 있습니다.
이 때, 구한 \(R\) 의 \(\text{det}()\) (determinant) 값은 +1이어야 합니다. 왜냐하면 \(R\) 은 orthonormal matrix이기 때문에 determinant가 +1을 만족하기 때문입니다. 하지만 -1이 도출되는 경우가 발생할 수 있습니다. 이 경우는 reflection이 발생한 경우이고 예외 케이스 입니다. 이 케이스를 처리하는 방법은 바로 뒤에서 살펴보도록 하겠습니다.

다음으로 ② 과정의 \(t^{*} = p'_{c} - R^{*}p_{c}\) 를 통해 간단하게 \(t^{*}\) 또한 구할 수 있습니다.

지금까지 살펴본 과정을 통해서 일반적인 상황에서의 Matched Points 쌍에 대한 Rotation과 Translation을 구할 수 있었습니다. 그리고 도출 과정을 보면 Rotation이 정상적으로 계산되면 Translation은 부가적으로 얻을 수 있기 때문에 Rotation을 잘 구하는 것이 핵심이 되는 것을 확인하였습니다.
그러면 Rotation을 정상적으로 구할 수 있는 경우와 그렇지 않은 경우를 구분하여 어떤 케이스가 존재하는 지 살펴보도록 하겠습니다.

① \(q_{i}\) 가 coplanar가 아닌 경우 : 먼저 위 그림과 같이 점들이 하나의 평면에 있지 않는 경우가 가장 일반적인 상황입니다. 하나의 평면에 점들이 존재하는 상황을 coplanar라고 하며 위 그림과 같은 상황은 coplanar한 상황이 아닌 것으로 이해할 수 있습니다. 이 상황에서는 Rotation의 솔루션이 유일하게 존재하므로 SVD를 이용하여 구한 해를 정상적으로 사용할 수 있습니다.

② \(q_{i}\) 가 colinear인 경우 : 두번째 케이스는 위 그림의 왼쪽과 같이 점들이 coplanar이면서 colinear인 경우 입니다. 이 경우에는 무한히 많은 rotation의 경우의 수를 만들어 낼 수 있습니다. 따라서 이와 같은 경우에는 유일한 해를 구할 수 없습니다. 따라서 사전에 모든 점들이 colinear인 지 확인해 보는 것이 중요합니다. 하지만 현실 데이터에서는 발생할 가능성이 매우 희박합니다.

③ \(q_{i}\) 가 coplanar이지만 colinear가 아닌 경우 : 마지막 케이스는 위 그림에서 오른쪽에 해당합니다. 점들이 coplanar이지만 colinear가 아닌 경우입니다. 이 경우에도 ①과 마찬가지 방법으로 해를 구할 수 있습니다.

최종적으로 구한 Rotation이 정말 Rotation으로써 의미를 가지려면 orthonormal matrix이고 determinant 또한 +1을 가져야 합니다.
하지만 경우에 따라서 determinant가 -1인 경우의 해를 얻을 수도 있습니다. 이와 같은 경우는 Rotation이 아니라 Reflection이 구해진 경우입니다.
determinant가 -1인 경우 2가지로 해석할 수 있습니다.
- ① coplanar인 상태에서 Reflection이 발생한 경우 \(V = [v_{1}, v_{2}, v_{3}] \to V = [v_{1}, v_{2}, -v_{3}]\) 로 변경하여 Rotation을 구할 수 있습니다. 이 때, \(\lambda_{3} = 0\) 을 만족해야 합니다.
- ② determinant가 -1임에도 불구하고 \(\lambda_{3} \ne 0\) 인 경우에는 해를 구할 수 없습니다. 이 경우에는 coplanar도 아니면서 Rotation을 구하는 최적화를 하는 데 실패한 경우에 해당합니다. 이와 같은 상황은 Noise가 많아서 Rotation을 구하기 어려우므로 Noise를 다루는 RANSAC과 같은 방법을 사용하는 작업이 필요합니다.

그러면 바로 위의 ①에 해당하는 상황을 살펴보도록 하겠습니다. coplanar에서는 모든 점들이 같은 평면에 있기 때문에, 차원이 하나 줄어든 것처럼 생각할 수 있습니다. 이 때, Rotation이 아닌 Reflection이 발생하더라도 만족할 수 있습니다.
모든 점들이 coplanar이면 앞에서 구한 \(H\) 의 Singular Value 하나가 0이 됩니다. 차원이 하나 소멸하기 때문에 발생한 문제이고 이와 같은 현상을 degeneracy라고 합니다.

\[H = \lambda_{1}u_{1}v'_{1} + \lambda_{2}u_{2}v'_{2} + 0 \cdot u_{3}v'_{3}\]
\[u_{i}, v_{i} \text{ : column vector of } U, V\]
\[\lambda_{1} > \lambda_{2} > \lambda_{3} = 0\]

따라서 위 케이스와 같이 행렬 분해가 된 경우 \(u_{3}\) 또는 \(v_{3}\) 의 부호를 바꾸더라도 \(H\) 에 영향을 주지 않습니다. 따라서 다음과 같이 \(V\) 를 변경합니다.

\[V = [v_{1}, v_{2}, v_{3}] \to V' = [v_{1}, v_{2}, -v_{3}]\]

최종적으로 \(R = V'U^{T}\) 를 이용하여 \(R\) 을 구함으로써 Rotation 을 정상적으로 구할 수 있습니다.

위 과정을 정리하면 다음과 같습니다.
① 행렬 \(H\) 를 구합니다.
② 행렬 \(H\) 의 SVD 적용을 한 후 \(H = U \Sigma V^{T}\) 로 분해합니다.
③ \(R = VU^{T}\) 를 이용하여 구합니다.
④ 만약 \(\text{det}(R) = 1\) 이면 정상적으로 Rotation이 구해진 상황입니다.
⑤ 만약 \(\text{det}(R) = -1\) 이면 다음 2가지로 해석할 수 있습니다.
- ⑤-1 \(\lambda_{3} = 0\) 이면 coplanar이므로 \(R = V'U^{T}\) 로 구할 수 있습니다.
- ⑤-2 \(\lambda_{3} \ne 0\) 이면 노이즈가 많아서 해를 구할 수 없는 상황입니다.

Matched Points의 Point-to-Point ICP Python Code

지금부터 살펴볼 내용은 임의의 Rotation과 Translation 그리고 점군 \(P\) 를 생성한 다음 생성한 Rotation과 Translation을 이용하여 \(P' = R*P + t\) 를 통해 \(P'\) 를 만들어 보겠습니다.
그 다음 \(P, P'\) 를 이용하여 ICP를 하였을 때, 생성한 Rotation과 Translation을 그대로 구할 수 있는 지 확인해 보도록 하겠습니다.

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")

# 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

print("t : \n", t)
print("t_est : \n", t_est)
print("t and t_est are same : ", np.allclose(t, t_est))
print("\n")

# t : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t_est : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t and t_est are same :  True

위 코드의 결과와 같이 정상적으로 \(R, t\) 를 구할 수 있음을 확인하였습니다.

지금까지 살펴본 방법은 매칭이 주어질 때, \(R, t\) 를 추정하는 문제에 해당합니다.
매칭을 알고있는 경우에는 최소 제곱 문제를 해결하기 위한 analytic solution이 존재하기 때문에 numerical solution을 이용한 최적화가 반드시 필요하진 않습니다.
하지만 점들의 매칭에 오류가 있거나 점들의 \(X, Y, Z\) 값이 부정확한 outlier가 포함되면 ICP를 진행하는 데 방해가 될 수 있습니다. 따라서 별도의 outlier를 제거해야 좋은 \(R, t\) 값을 구할 수 있으므로 outlier 제거 알고리즘인 RANSAC을 적용하여 정상적인 \(R, t\) 를 구하는 방법에 대하여 알아보도록 하겠습니다.
RANSAC과 관련된 내용은 아래 링크를 참조하시기 바랍니다.
- RANSAC : https://gaussian37.github.io/vision-concept-ransac/

RANSAC을 이용할 때에는 추출할 샘플 갯수, 반복 시험 횟수, inlier threshold를 파라미터로 필요로 합니다. 그 부분은 추가적인 실험이나 위에서 공유한 RANSAC 개념 링크의 글을 통해 어떻게 파라미터를 셋팅하는 지 참조할 수 있습니다.
아래 코드는 앞선 예제 코드에서 outlier 데이터를 추가한 뒤 RANSAC 과정을 거쳐서 좀 더 강건하게 Rotation과 Translation을 구하는 예제입니다.

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

def icp_svd_ransac(points_source, points_destination, 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(points_source.shape[1], n, replace=False)
        points_src_sample = points_source[:, indices]        
        points_dst_sample = points_destination[:, 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
        points_src_transformed = R @ points_source + t
        errors = np.linalg.norm(points_destination - points_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

# 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

# Add outliers to P_prime to create P_prime2
num_outliers = 10
P_prime2 = P_prime.copy()
P_prime2[:, -num_outliers:] = np.random.rand(3, num_outliers) * 100

################################## Calculate R and t using ICP with SVD, plus RANSAC
R_est, t_est, _ = icp_svd(P, P_prime2)

print("ICP without RANSAC : \n")
print("R : \n", R)
print("R_est : \n", R_est)
print("R and R_est are same : ", np.allclose(R,R_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.33635851  0.94169333 -0.00875314]
#  [-0.70013826 -0.25627327 -0.66643111]
#  [-0.62981693 -0.21803137  0.74551523]]
# R and R_est are same :  False

print("t : \n", t)
print("t_est : \n", t_est)
print("t and t_est are same : ", np.allclose(t, t_est))
print("\n")

# t : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t_est : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t and t_est are same :  True

print("diff R and R_est : \n", np.linalg.norm(np.abs(R - R_est)))
print("\n")

# diff R and R_est : 
#  0.5379242095232378

R_est, t_est, inliers = icp_svd_ransac(P, P_prime2)
print("ICP with RANSAC : \n")
print("R : \n", R)
print("R_est : \n", R_est)
print("R and R_est are same : ", np.allclose(R,R_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

print("t : \n", t)
print("t_est : \n", t_est)
print("t and t_est are same : ", np.allclose(t, t_est))
print("\n")

# t : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t_est : 
#  [[7.19317157]
#  [5.15828552]
#  [2.92487954]]
# t and t_est are same :  True

print("diff R and R_est : \n", np.linalg.norm(np.abs(R - R_est)))
print("\n")

# diff R and R_est : 
#  1.7603605962323948e-15

print("num inliers : ", inliers)
# num inliers :  20

icp_svd_ransac을 통하여 outlier의 비율이 꽤 큰 경우에도 정상적인 Rotation, Translation을 추정할 수 있음을 확인하였습니다.
지금까지 살펴본 내용은 두 점군의 쌍을 매칭할 수 있을 때, analytic solution을 이용하여 최적해를 구하는 방법에 대하여 알아보았습니다.

Part 1: Known Data Association & SVD

위 내용은 앞에서 다룬 Matched Points의 Point-to-Point ICP 개념과 일치합니다.

ICP (Iterative Closest Point) 와 Point Cloud Registration

목차

Matched Points의 Point-to-Point ICP 개념

Matched Points의 Point-to-Point ICP Python Code

Part 1: Known Data Association & SVD

Part 2: Unknown Data Association

Part 3: Non-linear Least Squares

Matched Points의 Point-to-Point ICP 개념

Matched Points의 Point-to-Point ICP Python Code

Part 1: Known Data Association & SVD

Part 2: Unknown Data Association

Part 3: Non-linear Least Squares