Visual Odometry (5) - 3D-3D ICP (Iterative Closest Point)

Xiang Gao, Tao Zhang 저자의 'Introduction to Visual SLAM from Theory to Practice' 책을 통해 SLAM에 대해 공부한 내용을 기록하고자 한다.

책의 내용은 주로 'SLAM의 기초 이론', 'system architecture', 'mainstream module'을 다룬다.

 

포스팅에는 기초 이론만 다룰 것이며, 실습을 해보고 싶다면 다음 github를 참조하자.

 

https://github.com/gaoxiang12/slambook2

GitHub - gaoxiang12/slambook2: edition 2 of the slambook

 

Visual Odometry 이전 과정인 feature extraction 및 feature matching 과정에 대한 내용은 다음 글을 참조하자.

 

https://jjuke-brain.tistory.com/entry/Visual-Odometry-1-ORB-Feature-Feature-Matching

Visual Odometry (1) - Feature Method, ORB Feature, Feature Matching

 

카메라의 motion(pose)를 추정할 때의 세 가지 경우 중 2D-2D case인 epipolar geometry를 다루는 내용은 다음을 참조하자.

 

https://jjuke-brain.tistory.com/entry/Visual-Odometry-2-Epipolar-Geometry-Triangulation?category=987160

 

Monocular vision에서 카메라의 motion으로부터 픽셀의 depth를 얻는 triangulation의 개념은 다음을 참조하자.

 

https://jjuke-brain.tistory.com/entry/Visual-Odometry-3-Triangulation

Visual Odometry (3) - Triangulation

 

3D-2D case인 PnP를 다룬 내용은 다음 링크를 참조하자.

 

https://jjuke-brain.tistory.com/entry/Visual-Odometry-4-PnP?category=987160 

 

 

목차

 

 

 

 

 

 

3D-3D Iterative Closest Point (ICP)

 

마지막으로 3D-3D pose estimation problem에 대해 알아보자.

3D-3D pose estimation problem은 LiDAR SLAM, RGB-D SLAM에서 사용된다.

 

LiDAR 3D Pointsets Example

 

다음과 같은 매칭된 3D 점들의 집합이 있을 때,

 

\( \mathbf{P} = \{ \mathbf{p}_1, \dots , \mathbf{p}_n \}, \quad \mathbf{P}' = \{ \mathbf{p}_1', \dots, \mathbf{p}_n' \} \)

 

다음을 만족하는 Euclidean transformation \( \mathbf{R, t}\) 를 찾으려 한다.

 

\( \forall i, \; \mathbf{p}_i = \mathbf{R} \mathbf{p}_i' + \mathbf{t} \)

 

이 문제를 푸는 과정을 iterative closest point(ICP)라 한다. 3D-3D pose estimation에서는 camera model이 필요하지 않다. 즉, 3차원 점들의 두 집합 간의 transformation만 쓰인다.

 

PnP와 비슷하게, ICP의 풀이 과정은 다음과 같이 두 방법으로 나뉜다.

•  Using linear algebra (특히, 주로 SVD가 사용됨)
• Using nonlinear optimization (bundle adjustment와 비슷함)

 

 

 

 

 

 

Using Linear Algebra (SVD)

 

먼저, 위에서 언급한 ICP problem에 따라, i번째 point에 대한 error term을 다음과 같이 정의한다.

 

\( \mathbf{e}_i = \mathbf{p}_i - \left( \mathbf{R} \mathbf{p}_i' + \mathbf{t} \right) \)

 

다음으로, 오차 제곱의 합을 최소화하는 least-square problem을 구성하여 \( \mathbf{R, t}\)를 찾는다.

 

\( \underset{\mathbf{R, t}}{\min} \cfrac{1}{2} \sum\limits_{i=1}^n \lVert ( \mathbf{p}_i - ( \mathbf{R} \mathbf{p}_i' + \mathbf{t} )) \rVert_2^2 \)

 

이를 풀기 위해서는 먼저 두 pointset의 centroid를 각각 구해주어야 한다. (subscript가 없는 것을 centroid로 표현하기로 하자.)

 

\( \mathbf{p} = \cfrac{1}{n} \sum\limits_{i=1}^n ( \mathbf{p}_i), \quad \mathbf{p}' = \cfrac{1}{n} \sum\limits_{i=1}^n ( \mathbf{p}_i' ) \)

 

이를 이용하여 error function을 변형하면,

 

\( \begin{align*} & \cfrac{1}{2} \sum\limits_{i=1}^n \lVert ( \mathbf{p}_i - ( \mathbf{R} \mathbf{p}_i' + \mathbf{t} )) \rVert^2 \\ &= \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{p}_i - \mathbf{R} \mathbf{p}_i' - \mathbf{t} - \mathbf{p} + \mathbf{R} \mathbf{p}' + \mathbf{p} - \mathbf{R} \mathbf{p}' \rVert^2 \\ &= \cfrac{1}{2} \sum\limits_{i=1}^n \lVert ( \mathbf{p}_i - \mathbf{p} - \mathbf{R}( \mathbf{p}_i' - \mathbf{p}' ) ) + ( \mathbf{p} - \mathbf{R} \mathbf{p}' - \mathbf{t} ) \rVert^2 \\ &= \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{p}_i - \mathbf{p} - \mathbf{R} ( \mathbf{p}_i' - \mathbf{p}' ) \rVert^2 + \lVert \mathbf{p} - \mathbf{R} \mathbf{p}' - \mathbf{t} \rVert^2 \\ & \quad \quad \quad + 2 ( \mathbf{p}_i - \mathbf{p} - \mathbf{R} ( \mathbf{p}_i' - \mathbf{p}' ) )^T (\mathbf{p} - \mathbf{R} \mathbf{p}' - \mathbf{t})) \end{align*} \)

 

\( ( \mathbf{p}_i - \mathbf{p} - \mathbf{R} ( \mathbf{p}_i' - \mathbf{p}' ) ) \) term은 summation 결과 0이 되기 때문에, optimization objective function은 다음과 같이 간단히 나타낼 수 있다.

 

\( \underset{\mathbf{R, t}}{\min} J = \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{p}_i - \mathbf{p} - \mathbf{R} ( \mathbf{p}_i' - \mathbf{p}' ) \rVert^2 + \lVert \mathbf{p} - \mathbf{R} \mathbf{p}' - \mathbf{t} \rVert^2 \)

 

함수를 잘 관찰해보면, 첫 번째 term은 rotation matrix \(\mathbf{R}\)에 관련된 term이고, 두 번째 term은 \(\mathbf{R, t}\) 둘 다에 관련되어 있지만 centroid에만 관련이 있다.

따라서 \(\mathbf{R}\)만 구한다면 두 번째 term을 0으로 두어서 \( \mathbf{t}\)를 얻을 수 있다.

 

정리하자면 ICP의 과정을 다음과 같이 요약할 수 있다.

1. 두 pointset의 centroid \(\mathbf{p}, \mathbf{p}'\)을 계산하고, 각 point의 de-centroid coordinates (centroid를 원점으로 하는 좌표계)를 구한다.
   • \( \mathbf{q}_i = \mathbf{p}_i - \mathbf{p}, \quad \mathbf{q}_i' = \mathbf{p}_i' - \mathbf{p}' \)
2. 다음의 optimization problem에 따른 rotation matrix를 계산한다.
   • \( \mathbf{R}^* = \underset{\mathbf{R}}{\operatorname{argmin}} \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{q}_i - \mathbf{R} \mathbf{q}_i' \rVert^2 \)
3. \(\mathbf{R}\)에 따른 \(\mathbf{t}\)를 구한다.
   • \( \mathbf{t}^* = \mathbf{p} - \mathbf{R} \mathbf{p}' \)

 

\(\mathbf{t}\)는 \( \mathbf{R}\)을 알고 있을 때 쉽게 구할 수 있음을 확인했으므로, \(\mathbf{R}\)의 계산 과정을 살펴보자.

Error term을 \( \mathbf{R}\)에 대해 전개하면,

 

\( \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{q}_i - \mathbf{R} \mathbf{q}_i' \rVert^2 = \cfrac{1}{2} \sum\limits_{i=1}^n \mathbf{q}_i^T \mathbf{q}_i + \mathbf{q}_i'^T \mathbf{R}^T \mathbf{R} \mathbf{q}_i' - 2 \mathbf{q}_i^T \mathbf{R} \mathbf{q}_i' \)

 

식에서 첫 번째 term은 \(\mathbf{R}\)과 상관이 없고, 두 번째 term은 \(\mathbf{R}^T \mathbf{R} = I\), 즉 rotation matrix가 orthogonal matrix이므로 마찬가지로 상관이 없다.

따라서, 실제로 optimization objective function은 다음과 같아진다.

 

\( \sum\limits_{i=1}^n - \mathbf{q}_i^T \mathbf{R} \mathbf{q}_i' = \sum\limits_{i=1}^n - \operatorname{tr}(\mathbf{R} \mathbf{q}_i' \mathbf{q}_i^T) = - \operatorname{tr} \left( \mathbf{R} \sum\limits_{i=1}^n \mathbf{q}_i' \mathbf{q}_i^T \right) \)

 

다음으로, SVD를 통해 최적의 \( \mathbf{R}\)을 구해보자.

먼저 다음과 같이 행렬을 정의한다.

 

\( \mathbf{W} = \sum\limits_{i=1}^n \mathbf{q}_i \mathbf{q}_i'^T\)

 

3차원 벡터에 대한 곱이므로 \(\mathbf{W}\)는 3×3 matrix이다. SVD를 적용하면,

 

\( \mathbf{W} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^T \)

 

\(\boldsymbol{\Sigma} \)는 singular value를 대각 요소로 갖는 diagonal matrix이고, \(\mathbf{U, V}\)는 diagonal matrix이다.

\(\mathbf{W}\)가 full rank인 경우, \(\mathbf{R}\)은 다음을 만족한다.

 

\( \mathbf{R} = \mathbf{U} \mathbf{V}^T \)

 

이렇게 rotation matrix \(\mathbf{R}\)을 구할 수 있고, step 3에서의 식을 통해 \(\mathbf{t}\)도 쉽게 얻을 수 있다.

만약 \(\mathbf{R}\)의 행렬식이 음수라면 optimal value로 \(- \mathbf{R}\)을 취할 것이다.

 

 

 

 

 

Using Nonlinear Optimization

 

이 방식은 PnP에서와 유사하다. (PnP에서의 nonlinear optimization은 링크를 참조하자.)

먼저, pose를 Lie algebra로 나타낼 때, objective function은 다음과 같다.

 

\( \underset{\boldsymbol{\xi}}{\min} = \cfrac{1}{2} \sum\limits_{i=1}^n \lVert (\mathbf{p}_i - \text{exp}(\boldsymbol{\xi}^\wedge) \mathbf{p}_i') \rVert_2^2 \)

 

pose에 대한 하나의 error term의 도함수(gradient 역할)는 Lie algebra의 perturbation model을 사용하여 다음과 같이 얻는다.

 

\( \cfrac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}} = - \left( \text{exp} \left( \boldsymbol{\xi}^\wedge \right) \mathbf{p}_i' \right)^\odot \)

 

이를 사용하여 통해 반복적으로 minimum value를 찾아간다.

 

ICP problem의 해는 유일하거나 무한히 많다고 증명되었다.

해가 유일한 경우, minimum value를 찾기만 한다면 global minimum이 된다. 이로 인해 point가 서로 매칭만 된 상태라면 초기값을 임의로 선택해도 상관 없다는 장점이 있다.

 

 

사실 matching된 점을 아는 경우, 이 least-square problem은 analytical solution이 존재한다. 하지만 RGB-D SLAM 등에서 pixel의 depth를 얻을 수 없는 경우가 있기 때문에, PnP와 ICP optimization을 함께 사용할 수 있기 때문에 nonlinear optimization 방식을 알아둘 필요가 있다.

즉, feature points의 depth를 아는 경우에는 3D-3D error(ICP)를 모델링하고, depth를 모르는 경우 3D-2D reprojection error(PnP)를 모델링하여 모든 error를 한 문제에서 풀 수 있는 것이다.