Slam 3-1강 (Point Cloud Registration & ICP algorithm) 요약

Cyrill 교수님의 SLAM 강의를 듣고 정리해보자!

0. Today’s Goal

• Point Cloud Registration이란?
• Direct Optimal Solution with Known Data Association
• Why solution used SVD is good?

1. 강의 및 강의 자료

강의 List는 SLAM KR의 SLAM DUNK seson 2에서 정해준 강의를 따라 정리를 할 것이다.

SLAM DUNK season 2의 Reference는 여기서 확인해 볼 수 있다.

SLAM KR에서 이 강의를 듣고 요약을 세미나 형식으로 준비를 해준다!

이번 포스팅은 다음의 강의들을 참고하여 요약을 해보았다.

Point Cloud Registration & ICP의 경우 2021 강의에서 3번에 걸쳐 나눠 강의를 진행했기 때문에
그 강의를 듣고 요약을 진행했다.

• ICP & Point Cloud Registration - Part 1: Known Data Association & SVD (Cyrill Stachniss, 2021)
• SLAM DUNK Season 2 - Iterative Closest Points

Cyrill Stachniss의 강의 자료는 pdf로 다운 받을 수 있다.

2. Point Cloud Registration이란?

Point Cloud Registration이란 두 Point Cloud를 정렬을 하는 공간 변환을 찾는 과정을 말한다.

이는 Mapping을 할 때 매우 중요한 과정인데 Scan matching을 하거나 Scan registration을 진행할 때
동일한 reference frame에서 보았을 때 각각의 Map point들이 일치해야 정확한 주위 환경을 Mapping 할 수 있기 때문이다.

따라서 Point Cloud Registration을 통해
가장 정렬을 잘하는 Rotation Matrix $R$ 과 Translation Vector $t$를 찾는 것이 핵심이다.

상황에 따라 해결법도 달라지는데,

1. 두 Point Cloud의 대응관계(Correspondences)를 알 때 (Known Data Association)
2. 두 Point Cloud의 대응관계(Correspondences)를 모를 때 (Unknown Data Association)
3. Robust한 Least Squares Approaches를 이용하는 방법

이번 강의에서는 1번 상황에 대한 풀이법으로 대응 관계를 알 때 SVD 알고리즘을 활용해 어떻게 Rotation Matrix $R$ 과 Translation Vector $t$를 찾는지 알아보자.

Known Data Association 일 때, 간단한 예로 Point Cloud Registration을 이해해보자.

위 그림에서 빨간색 Point Cloud를 ${{y_n}}$, 파란색 Point Cloud를 ${{x_n}}$
그리고 각각 Point들의 대응 관계를 $C$라고 하자.

${{y_n}}, {{x_n}}, C $가 모두 주어졌을 때, Rotation Matrix $R$ 과 Translation Vector $t$를 활용하여
두 Point Cloud를 정렬할 수 있다.

이 때, $R$과 $t$에 의해 옮겨진 Point Cloud를 ${\bar{x_n}}$ 이라고 하자.

그럼 ${\bar{x_n}}$을 다음과 같은 식으로 표현할 수 있다.

이 때 유클리디안 거리가 최소화 되도록 하는 $R$과 $t$를 찾는 것이 Point Cloud Registration이다.

3. Direct Optimal Solution with Known Data Association

Point Cloud Registration을 풀기 전, 이 방법을 Absolute Orientation Problem의 특별한 경우라고 이야기했다.

Absolute Orientation Problem이란 간단히 이야기하면 3D points의 집합을 정렬할 때
Transformation을 결정하는 문제이다. 이 또한 Cyrill 교수님의 강의로 설명을 들을 수 있다.
(5분만에 정말 간단히 설명해주신다!)

Absolute Orientation Problem에서는 Scale parameter도 존재하지만,
우리가 이번 강의에서 Point Cloud Registration을 할 때는 Scale parameter를 1로 고정하고 구하게 된다.

따라서 Point Cloud Registration을 Absolute Orientation Problem의 특별한 경우라고 하는 것이다.

다시 Point Cloud Registration을 푸는 방법을 이야기해보자.

우리가 대응관계(Correspondences)를 알고 있을 때는
Initial guess가 없어도, Iterate를 돌지 않아도 완벽한 해(Solution)를 구할 수 있다는 것이다!

이를 구하는 방법은

1. Translation 값을 구하기 위해서 두 Point Cloud의 Center of massess(질량 중심)을 일치시키고,
   이동량을 계산한다.

2. Rotation 값을 구하기 위해서 SVD(Singular Value Decomposition)을 수행한다.

식으로 이를 표현해보자.

먼저 ${{x_n}}$의 Weighted mean을 ${{x_0}}$, ${{y_n}}$의 Weighted mean을 ${{y_0}}$이라고 하자.

아래 식에서 $p_n$은 가중치(Weight)를 의미한다.

위에서 구한 값들을 활용하여 교차 공분산 행렬(Cross Covariance Matrix) $H$를 계산한다.

그리고 $H$에 대해 SVD를 계산한다.

SVD를 이용하여 Rotation Matrix $R$을 구하는 식은 다음과 같다.

그리고 현재까지 구한 값들을 활용하여 Translation Vector $t$를 구하는 식은 다음과 같다.

Rotation Matrix $R$와 Translation Vector $t$를 구하므로써 우리가 원하는 Point Cloud Registration을 풀었다!

위 과정을 한 장의 슬라이드로 정리하면 다음과 같다.

4. Why solution used SVD is good?

그렇다면 왜 이 방법이 최적의 해(solution)일까?

우리는 왜 초기값(Initial guess)이 필요하지 않으며 Iterate를 돌지 않아도 완벽한 해를 구할 수 있는 것일까?

이유를 알기 위해서 우리가 구하는 식을 재정의(Rewrite) 해보자.

우리가 원래 구해야 하는 식은 다음과 같다.

이 식을 원점(origin)이 $y_0$인 Local Coordinate System으로 다시 재정의해보자.

이전에 정의한 것과 같이 $y_0$을 아래와 같이 정의 한 후,

우리가 구해야 하는 식을 아래와 같이 변경하자.

이렇게 되면 구해야하는 Translation Vector가 바뀌였다.

다시 Translation Vector를 재정의 해보자.

$\bar{x_n} = Rx_n + t$ 식을 원점($y_0$)을 활용하여 이동시키면,
$\bar{x_n} - y_0 = Rx_n + t - y_0$ 으로 쓸 수 있다.

Rotation Matrix로 우변을 모두 묶으면 아래와 같은 식이 나온다.

\[\bar{x_n} - y_0 = R(x_n + R^T t - R^T y_0)\]

여기서 $R^T$는 Rotation Matrix의 전치행렬이다.

새로운 변수 $x_0$을 활용하여 다시 식을 정의해보자. 아까 Weighted mean을 구할 때 쓰던 변수가 아니다!

$x_0 = -R^T t + R^T y_0$ 이라고 할 때

우리가 구해야 하는 식을 다음과 같이 쓸 수 있다.

그렇다면 우리가 찾아야 하는 변수는 $R,t$가 아니라 $R, x_0$를 찾는 문제로 바뀐다.

우리가 구해야 하는 식은 2차식인데 이를 행렬로 표현하여 Objective function을 정의한다.

Objective function은 최적화시키려는 함수를 의미한다.

Objective function $\Phi(x_0,R)$이라 정의하고,
이를 최소화시키는 $ x^{*}_{0}, R^{*} $ 을 구하면 우리가 원하는 해를 구할 수 있다.

$\Phi(x_0,R)$는 아래와 같이 정의한다.

우리는 보통 이차식의 최솟값 또는 최댓값을 구할 때 미분을 하고,
미분이 0이 되는 값을 찾아 최솟값 또는 최댓값을 구하곤 했다.

그 방법을 여기서 적용하자.

Objective function을 풀어서 쓰면 아래와 같다.

위 식을 $x_0$에 의해서 미분을 하면 다음과 같다.

미분한 식을 0으로 만드는 식을 쓰면 다음과 같다.

이것을 간단하게 정리하면 다음과 같다.

이때 우변에 있는 식의 의미를 살펴보자.

우리는 $y_0$을 $ y_0 = \frac{\sum y_n p_n}{\sum p_n}$으로 정의했었다!

결국 시그마를 전부 풀어쓰면 우변이 0이 됨을 쉽게 생각할 수 있다.

따라서 우리가 얻을 수 있는 식은 다음과 같다.

이 식을 이용해 $x_0$을 정의해보면 다음과 같은 식을 얻을 수 있다.

\[\sum x_n p_n - \sum x_0 p_n = 0\] \[x_0 = \frac{\sum x_n p_n}{\sum x_n}\]

결국 우리가 임의로 정의했던 $x_0$는 ${{x_n}}$의 Weighted mean을 ${{x_0}}$라고 했을 때,
최적의 해를 구할 수 있음을 알 수 있다!

---

동일한 방법을 $R$에도 적용해보자.

Objective function을 관찰하면 R에 관한 식은 아래에 빨간색 부분 밖에 없는 것을 확인할 수 있다.

$R$의 관점에서 빨간색 부분만 고려를 하면 된다는 의미이다.

빨간색 부분의 부호가 (-)이므로 이를 최대화하면 Objective function은 최솟값을 가질 수 있다.

이때, $R$은 Rotation Matrix이므로 $RR^T = I$라는 성질을 만족한다는 것을 기억하자.

좀 더 식을 간편하게 보기 위해

\[a_n = (x_n - x_0), b_n = (y_n - y_0)\]

이라고 정의하자. 여기서 $x_0, y_0$은 우리가 이전에 정의했던 Weighted mean값이다.

그럼 식은 다음과 같이 정리된다.

이를 trace의 정의를 이용해서 다음과 같이 나타낼 수 있다.

\[R^{*} = argmax_{R} tr(R^T H)\]

(위 식은 강의자료에 있는 식과 다른데, Cyrill 교수님의 댓글에서 강의 자료의 오타가 있음을 확인했다.)

위 식에서 나온 $H$는 Cross covariance matrix로 다음과 같이 정의된다.

이제 $R$을 최대화하는 $tr(R^TH)$를 찾으면 된다. 이는 SVD를 이용할 것이다.

$H$를 SVD를 이용하여 분해하면 다음과 같이 나타낼 수 있다.

\[svd(H) = UDV^T\] \[U^T U = I, V^T V = I, D = diag(d_i)\]

$diag$는 대각행렬을 의미한다. 이 때 $R$을 $ U V^T$라고 정의하자.

그렇다면 다음과 같은 식을 얻을 수 있다.

\[tr(R^T H) = tr(V U^T * U D V^T) = tr(VIDV^T) = tr(VDV^T)\]

이 때, $D$는 모든 원소가 양수인 대각행렬이므로 다음과 같이 쓸 수 있다.

\[tr(VDV^T) = tr(V D^{\frac{1}{2}} D^{\frac{1}{2}} V^T)\]

대각행렬의 전치행렬은 자기자신이므로 다음과 같이 쓸 수 있다.

\[tr(V D^{\frac{1}{2}} D^{\frac{1}{2}} V^T) = tr(V D^{\frac{1}{2}} (D^{\frac{1}{2}} V)^T)\]

여기서 $V D^{\frac{1}{2}} = A$라고 정의하자.

그럼 다음과 같은 식이 성립한다.

\[tr(R^TH) = tr(A A^T)\]

SVD에 만들어진 원소에 만들어진 $A$는 다음과 같은 부등식을 만족한다.

이 부등식은 코시-슈바르츠 부등식이다.

여기서 $ R’$은 임의의 어떤 Rotation Matrix이다.

이것은 아래의 식을 의미하는데,

\[tr(R^T H) = tr(A A^T) \ge tr( R' A A^T) = tr( R' R^T H)\]

여기서 $ R’ R^T$ 또 다른 Rotation Matrix를 의미한다.

이를 통해 얻을 수 있는 결론은 $R = U V^T$일 때 $tr(R^T H)$가 최대의 값을 가질 수 있고,
결국 Objective function을 최소화하는 $R$은 SVD의 결과로 만들어진 $V,D$를 활용하여
만든 Matrix라는 의미이다.

그렇다면 SVD를 활용해 만든 Rotation Matrix가 Unique한 Solution인가라는 궁금증을 가질 수 있다.

결론적으로, $H$가 Full rank matrix일 경우 $svd(H) = UDV^T$이고 $D = Diag(d_1,d_2,d_3)$으로 쓸 수 있다. 세 원소가 0보다 크면 이를 Unique한 Solution으로 볼 수 있다.

우리는 Objective function을 최소화하는 parameter $R$과 $x_0$를 알고 있으므로,
Objective function을 만들 때 정의한 식 $x_0 = - R^T t + R^T y_0$ 을 이용하여
Translation Vector를 $t = y_0 - R x_0$와 같이 나타낼 수 있다.

위 과정을 통해 초기값이 없어도 최적의 해를 구할 수 있는 방법을 증명했다!

여기서 행렬의 순서때문에 헷갈리는 상황이 발생할 수 있는데
아래의 슬라이드를 통해 결국 다 똑같다는 것을 알려줬다.

---

지금까지 Known Data Association 일 때 Point Cloud Registeration하는 방법을 알아봤다.
이는 추후 ICP Algorithm의 기초가 되고 수많은 SLAM 또는 Odometery 알고리즘의 기초가 되므로
잘 알아두도록 하자!

이 방법에 대해 조금 더 자세하게 알고 싶을 경우,
Least-Squares Fitting of Two 3-D Point Sets 논문을 참고해보자.