Visual Odometry (4) - 3D-2D PnP (Perspective-n-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

 

 

 

목차

 

 

 

 

 

 

PnP(Perspective-n-Point)

 

PnP는 feature matching 결과를 통한 카메라 motion 추정 방법 중 3D-2D case이다.

n개의 3차원 점들과 그것들이 투영된 위치가 주어졌을 때, 카메라의 pose를 추정하는 것이다.

 

2D-2D epipolar geometry는 8개 이상의 feature point 쌍이 필요했고, initialization, pure rotation, scale 등에 있어서 문제점이 있었다. (자세한 내용은 이전 포스팅을 참조하자.)

두 이미지에서 feature points 중 하나라도 3차원 위치(좌표)가 주어진다면, 카메라 motion 추정에 최소 3개의 쌍이 필요해진다. (보통은 검증을 위해 한 쌍이 추가로 필요하다.)

Feature point의 3차원 위치는 triangulation이나 RGB-D 카메라의 depth map 등으로 얻게 되는데, binocular 또는 RGB-D visual odometry에서는 PnP를 사용하여 직접적으로 카메라의 motion을 추정할 수 있다.

 

3D-2D 방법은 epipolar constraints가 필요 없고, 더 좋은 결과를 보이므로, 가장 중요한 pose estimation 방법이다.

 

PnP에는 P3P, DLT, EPnP, UPnP, Bundle Adjustment 등 많은 방법이 있는데, 여기서는 DLT, P3P, BA 정도만 다룰 것이다.

 

 

 

 

Direct Linear Transformation (DLT)

 

어떤 점들의 3차원 위치와 그것들의 projection을 알 때, 카메라의 pose를 구하는 방법을 살펴보자.

이 방법으로 map과 image가 주어졌을 때 카메라의 pose를 알아보는 문제, 두 카메라의 상대적인 motion을 구하는 문제 등을 해결할 수 있다.

 

사전에 Lie Algebra 관련 지식을 (개념적으로나마) 알고 있으면 좋다.

간단한 Lie Algebra 내용은 다음 글을 참조하자.

https://jjuke-brain.tistory.com/entry/Lie-Group-and-Lie-Algebra?category=987160 

 

3D point \(P\)의 homogeneous coordinates를 \(\mathbf{P} = (X, Y, Z, 1)^T\)라 하고, image \(I_1\)에 투영된 normalized homogeneous coordinate를 \(\mathbf{x}_1 = (u_1, v_1, 1)^T\)라 할 때, camera의 pose \(\mathbf{R, t}\)가 주어져 있다.

(Normalized homogeneous coordinate를 사용하는 이유는 intrinsic matrix \(\mathbf{K}\)의 영향이 없기 때문이다.)

3 × 4 사이즈의 augmented matrix \( \left[ \mathbf{R} \vert \mathbf{t} \right]\)를 정의하면, 다음과 같은 식을 만들 수 있다.

 

\( s \mathbf{x}_1 = \left[ \mathbf{R} \vert \mathbf{t} \right] \mathbf{P} \)

 

풀어쓰면,

 

\( s \begin{pmatrix} u_1 \\ v_1 \\ 1 \end{pmatrix} = \begin{pmatrix} t_1 & t_2 & t_3 & t_4 \\ t_5 & t_6 & t_7 & t_8 \\ t_9 & t_10 & t_11 & t_12 \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ 1 \end{pmatrix} \)

 

마지막 행으로 \(s\)를 제거하면 다음과 같은 두 constraints를 얻는다.

 

\( u_1 = \cfrac{t_1 X + t_2 Y + t_3 Z + t_4}{t_9 X + t_{10} Y + t_{11} Z + t_{12}}, \quad v_1 = \cfrac{t_5 X + t_6 Y + t_7 Z + t_8}{t_9 X + t_{10} Y + t_{11} Z + t_{12}} \)

 

\(\mathbf{T}\)를 행벡터로 표현하면 아래와 같고, (여기서, \(\mathbf{T}\)가 \(\text{SE}(3)\)의 transformation matrix는 아님에 유의하자.)

 

\( \mathbf{t}_1 = (t_1, t_2, t_3, t_4)^T, \; \mathbf{t}_2 = (t_5, t_6, t_7, t_8)^T, \; \mathbf{t}_3 = (t_9, t_{10}, t_{11}, t_{12})^T \)

 

이를 통해 두 constraints를 간단히 표현하면 다음과 같다.

 

\( \begin{cases} \mathbf{t}_1^T \mathbf{P} - \mathbf{t}_3^T \mathbf{P} u_1 = 0 \\ \mathbf{t}_2^T \mathbf{P} - \mathbf{t}_3^T \mathbf{P} v_1 = 0  \end{cases} \)

 

위와 같이, 각 feature point는 \(\mathbf{t}\)에 대해 두 개의 linear constraints를 제공한다.

총 \(N\)개의 feature points가 있따고 가정하면, 다음과 같은 linear equation system을 구성할 수 있다.

 

\( \begin{pmatrix} \mathbf{P}_1^T & 0 & - u_1 \mathbf{P}_1^T \\ 0 & \mathbf{P}_1^T & - v_1 \mathbf{P}_1^T \\ \vdots & \vdots & \vdots \\ \mathbf{P}_N^T & 0 & - u_N \mathbf{P}_N^T \\ 0 & \mathbf{P}_N^T & - v_N \mathbf{P}_N^T \end{pmatrix} \begin{pmatrix} \mathbf{t}_1 \\ \mathbf{t}_2 \\ \mathbf{t}_3 \end{pmatrix} = \boldsymbol{0} \)

 

\(\mathbf{t}\)는 총 12개의 미지수를 갖고 있으므로(12차원), matrix \(\mathbf{T}\)의 해는 최소 6쌍의 matching point 쌍으로 얻을 수 있다.

 

이와 같은 방법이 바로 Direct Linear Transform (DLT)이다.

DLT에서는 \( \mathbf{T} \) 행렬의 미지수들 사이의 correlation을 고려하지 않고 직접적으로 구한다.

이때 rotation matrix \( \mathbf{R} \)은 \( \text{SO}(3)\)에 속하기 때문에, DLT로 얻은 해는 \(\text{SE}(3)\)의 조건을 만족할 필요가 없다. (일반적인 행렬일 뿐이다.)

 

그리고 DLT에서 추정한 \( \mathbf{R}\)의 왼쪽 3 × 3 matrix를 근사화하여 rotation matrix를 찾는다.

이 때에는 QR decomposition을 사용하거나, 다음 식으로 계산한다.

 

\( \mathbf{R} \leftarrow \left( \mathbf{R} \mathbf{R}^T \right)^{-1/2} \mathbf{R} \)

 

이 식은 matrix space의 결과를 \( \text{SE}(3)\), 즉 3D transformation으로 reproject하고, rotation과 translation 두 부분으로 변환하는 개념이다. (상세한 설명은 생략한다.)

 

한 가지 주목할 점은, SLAM에서는 보통 주어져 있지만, 위에서 normalized plane coordinate (\(\mathbf{x}\))를 사용해서 intrinsic matrix \( \mathbf{K}\)의 영향을 받지 않는다고 하였다.

따라서 intrinsic parameter를 모른다 해도 \(\mathbf{K, R, t}\) 세 개의 matrix를 구해야 할 때 PnP를 사용할 수 있다. (미지수가 증가하므로 결과가 좋지 않기는 하다.)

 

 

 

 

P3P

 

P3P는 PnP를 푸는 또 다른 방법중 하나이다. DLT에서는 6쌍이 필요하다고 했는데, P3P에서는 3쌍만 있으면 카메라의 pose를 구할 수 있다.

 

P3P에서는 주어진 3개 점들 간의 관계를 알아야 한다.

 

P3P Problem

 

Input data는 세 쌍의 3차원 점과 2차원 점이 주어진다. 3차원 점을 \(A, B, C\)(world coordinate), 각각의 projection인 2차원 점을 \(a, b, c\)(camera coordinate)로 두자.

또한, 가능한 solution 중에서 맞는 것을 고르기 위한 검증용 point 쌍 하나가 필요하다. (\(D, d\))

 

카메라 좌표계에서의 3차원 점들의 좌표를 계산하면 그에 상응하는 point를 얻게 되고, PnP problem을 ICP problem(3D-3D problem)으로 바꿀 수 있다.

 

위 그림을 보면, 다음과 같이 닮은 삼각형 3개가 있다.

 

\( \triangle{Oab} - \triangle{OAB}, \quad \triangle{Obc} - \triangle{OBC}, \quad \triangle{Oac} - \triangle{OAC} \)

 

Cosine 법칙에 의해 다음 세 가지 식이 성립한다.

 

\( \begin{cases} \overline{OA}^2 + \overline{OB}^2 - 2 \cdot \overline{OA} \cdot \overline{OB} \cdot \cos{\left\langle a, b \right\rangle} = \overline{AB}^2 \\ \overline{OB}^2 + \overline{OC}^2 - 2 \cdot \overline{OB} \cdot \overline{OC} \cdot \cos{\left\langle b, c \right\rangle} = \overline{BC}^2 \\ \overline{OA}^2 + \overline{OC}^2 - 2 \cdot \overline{OA} \cdot \overline{OC} \cdot \cos{\left\langle a, c \right\rangle} = \overline{AC}^2 \end{cases} \)

 

세 식의 양 변을 \(\overline{OC}^2\)으로 나누고, \(x = \overline{OA} / \overline{OC}, \; y = \overline{OB} / \overline{OC}\)라 하면,

 

\( \begin{cases} x^2 + y^2 - 2xy \cos{\left\langle a, b \right\rangle} = \overline{AB}^2 / \overline{OC}^2 \\ y^2 + 1^2 - 2y \cos{\left\langle b, c \right\rangle} = \overline{BC}^2 / \overline{OC}^2 \\ x^2 + 1^2 - 2x \cos{\left\langle a, c \right\rangle} = \overline{AC}^2 / \overline{OC}^2 \end{cases} \)

 

\(v = \overline{AB}^2 / \overline{OC}^2, \; u = \overline{BC}^2 / \overline{AB}^2, \; w = \overline{AC}^2 / \overline{AB}^2 \)이라 하면,

 

\( \begin{cases} x^2 + y^2 - 2xy \cos{\left\langle a, b \right\rangle} - v = 0 \\ y^2 + 1^2 - 2y \cos{\left\langle b, c \right\rangle} - u v = 0 \\ x^2 + 1^2 - 2x \cos{\left\langle a, c \right\rangle} - w v = 0 \end{cases} \)

 

마지막으로 처음 식의 \(v\)를 우변으로 넘겨 다른 식과 연립하면 다음 식들을 얻는다.

 

\( \begin{cases} (1 - u) y^2 - u x^2 - 2 \cos{\left\langle b, c \right\rangle} y + 2 u x y \cos{\left\langle a, b \right\rangle} + 1 = 0 \\ (1 - w) x^2 - w y^2 - 2 \cos{\left\langle a, c \right\rangle} x + 2 w x y \cos{\left\langle a, b \right\rangle} + 1 = 0 \end{cases} \)

 

2D 점들의 좌표를 통해 3개의 \(\cos\)을 계산할 수 있고, 3차원 점들의 좌표로 \(u, w\)를 계산할 수 있다.

미지수는 \(x, y\)로, 카메라가 움직임에 따라 바뀌게 된다.

직접 식을 풀기는 복잡하기 때문에, Wu's method라는 방법을 사용한다.

 

solution은 epipolar geometry에서 essential matrix \(\mathbf{E}\)를 구할 때와 마찬가지로 4개의 solution이 나오는데, 앞서 문제 정의에서 추가로 준비해둔 검증용 point 쌍 하나를 이용하여 최종 solution을 고름으로써 camera frame에서의 \(A, B, C\)의 3차원 좌표를 구할 수 있다.

이후에 3D-3D point 쌍을 기반으로 camera의 \(\mathbf{R, t}\)를 계산할 수 있는데, 이는 ICP problem이 된다.

 

정리하자면, P3P에서는 삼각형의 닮음 관계를 활용하여 camera frame의 projection points \(a, b, c\)의 3차원 좌표를 구하고, 최종적으로 3D-3D pose estimation problem으로 문제를 변환한다.

 

P3P의 단점은 다음과 같다.

• P3P는 3개 점의 정보만 포함하므로, matching points 쌍이 3개보다 많이 주어진 경우, 그 정보를 다 활용할 수 없다.
• 3차원 점이나 2차원 점들이 noise나 mismatch의 영향을 받는 경우, 정확한 결과를 얻을 수 없다.

 

따라서 SLAM에서는 일반적으로 먼저 P3P나 EPnP를 사용하여 카메라 pose를 추정한 후에, bundle adjustment로 least-square optimization problem을 구성한다.

 

 

 

 

 

Bundle Adjustment (BA)

 

PnP problem은 linear 뿐만 아니라, reprojection error에 대한 nonlinear least-square problem으로 구성할 수 있다. 즉, reprojection error를 최소화하는 방법으로 PnP를 풀 수 있다.

Linear method는 '카메라 pose 추정 후 point의 위치를 추정'한다는 구분 되는 step으로 나뉘지만, nonlinear optimization은 두 과정에서 구하는 것들을 한꺼번에 optimization variables로 두어 최적화(general solution method)한다.

이렇게 camera pose와 3D point 좌표를 한꺼번에 최적화하는 문제를 Bundle Adjustment(BA)라 한다.

 

 

 

Optimizing the Camera Pose with BA

 

카메라가 연속적으로 움직이면 BA를 통해 직접적으로 카메라 pose를 구할 수 있다.

가장 기초적인 형태는 앞서 계속 살펴봤던 two-view form으로, \(n\)개의 3D point \(P\)와 그 점들의 projection인 \(p\)가 주어지고, camera의 pose인 \(\mathbf{R, t}\)를 구하려 한다.

 

\(i\)번째 coordinate를 \(\mathbf{P}_i = [X_i, Y_i, Z_i]^T\), \(i\)번째 투영된 pixel의 coordinate를 \(\mathbf{u}_i = [u_i, v_i]^T\)로 둔다.

2D pixel position과 3D spatial position의 관계(자세한 내용은 링크를 참조하자.)에 따라, 다음이 성립한다.

 

\( s_i \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} = \mathbf{KT} \begin{bmatrix} X_i \\ Y_i \\ Z_i \\ 1 \end{bmatrix} \)

 

Matrix 형태로 나타내면 다음과 같다.

Homogeneous coordinates에서는 에러의 차원이 3이지만, \(\mathbf{u}\)의 마지막 차원이 1이기 때문에 해당 차원의 에러는 항상 0이고, 따라서 간단히 non-homogeneous coordinates로 나타냈다.

 

\( s_i \mathbf{u}_i = \mathbf{KT} \mathbf{P}_i \)

 

이때 알지 못하는 카메라의 pose와 관측된 점의 noise 때문에, 식은 residual(error)이 존재하게 된다.

이 residual을 합하여 least-square problem을 구성하고, 이를 최소화하여 camera pose를 찾을 것이다.

 

\( \mathbf{T}^* = \underset{\mathbf{T}}{\operatorname{argmin}} \cfrac{1}{2} \sum\limits_{i=1}^n \lVert \mathbf{u}_i - \cfrac{1}{s_i} \mathbf{KT} \mathbf{P}_i \rVert_2^2 \)

 

Reprojection Error

 

위 사진에서와 같이, reprojection error는 residual term(error)으로, 투영된 위치와 관측된 위치 간의 차이이다.

\(p_1, p_2\)는 feature matching에 의한 점 \(P\)의 projection인데, 계산을 통해 얻은 projection인 \(\hat{p}_2\)는 실제 값과 \(e\)만큼의 차이가 있다. 따라서 카메라의 pose를 조정하여 이 차이를 줄인다. (한 점의 에러가 아닌 전체 에러를 줄이는 것이 목표임을 기억하자.)

 

Lie Algebra를 활용하여 Gauss-Newton method, 혹은 Levenberg-Marquardt method 등으로 optimization 문제를 풀 수 있다. (linear optimization 관련 자세한 내용은 링크를 참조하자.)

이 과정에서 각 error term의 도함수를 구해야 하는데, 이때 다음과 같이 linearization한다.

 

\( \mathbf{e} (\mathbf{x} + \Delta \mathbf{x} ) \approx \mathbf{e} (\mathbf{x}) + \mathbf{J}^T \Delta \mathbf{x} \)

 

이때 \(\mathbf{J}^T\)의 형태에 주목하자.

\(\mathbf{e}\)는 pixel 좌표계에서의 에러(2D)이고, \(\mathbf{x}\)는 카메라의 pose(6D)이다. 따라서 \(\mathbf{J}^T\)의 사이즈는 2 × 6이다.

 

이를 유도하기 위해서는 Lie Algebra(링크 참조), 특히 perturbation model에 대한 개념이 필요하다.

먼저, camera frame의 점의 좌표를 \(\mathbf{P}'\)으로 정의하고, 처음 3개 차원만 고려해보자.

 

\( \mathbf{P}' = (\mathbf{TP})_{1:3} = [X', Y', Z']^T \)

 

이에 대한 projection model은 다음과 같다.

 

\( s \mathbf{u} = \mathbf{K} \mathbf{P}' \)

 

전개하면,

 

\( \begin{bmatrix} s u \\ s v \\ s \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}  \begin{bmatrix} X' \\ Y' \\ Z' \end{bmatrix} \)

 

세 번째 행으로 \(s\), 즉 \(\mathbf{P}'\)의 distance를 제거하면 다음을 얻는다.

 

\( u = f_x \cfrac{X'}{Z'} + c_x, \quad v = f_y \cfrac{Y'}{Z'} + c_y \)

 

이 식의 모양은 camera model을 배울 때 봤던 식 \( X' = f \cfrac{X}{Z}, \; Y' = f \cfrac{Y}{Z} \)과 비슷하다.

Error를 찾기 위해서는 \(u, v\)와 관측 값을 비교해 보아야 한다.

\(\mathbf{T}\)에 disturbance quantity \(\delta \boldsymbol{\xi}\)를 left multiply한 후에 \(\mathbf{e}\)의 도함수를 구한다.

Chain rule에 의해 다음을 만족하게 된다.

 

\( \cfrac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}} = \lim_\limits{\delta \boldsymbol{\xi} \rightarrow 0} \cfrac{\mathbf{e} (\delta \boldsymbol{\xi} \oplus \boldsymbol{\xi}) - \mathbf{e} (\boldsymbol{\xi})}{\delta \boldsymbol{\xi}} = \cfrac{\partial \mathbf{e}}{\partial \mathbf{P}'} \cfrac{\partial \mathbf{P}'}{\partial \delta \boldsymbol{\xi}} \)

 

여기서 \(\oplus\)는 Lie Algebra에서의 disturbance의 left multiplication을 뜻한다.

첫 번째 항은 투영된 점에 대한 error의 도함수로, 위에서 \(s\)를 제거하여 얻은 식에서 쉽게 얻을 수 있다.

 

\( \cfrac{\partial \mathbf{e}}{\partial \mathbf{P}'} = - \begin{bmatrix} \cfrac{\partial u}{\partial X'} & \cfrac{\partial u}{\partial Y'} & \cfrac{\partial u}{\partial Z'} \\ \cfrac{\partial v}{\partial X'} & \cfrac{\partial v}{\partial Y'} & \cfrac{\partial v}{\partial Z'} \end{bmatrix} = - \begin{bmatrix} \cfrac{f_x}{Z'} & 0 & - \cfrac{f_x X'}{Z'^2} \\ 0 & \cfrac{f_y}{Z'} & - \cfrac{f_y Y'}{Z'^2} \end{bmatrix} \)

 

두 번째 항은 Lie Algebra에 대한 변환된 점의 도함수이다.

 

\( \cfrac{\partial (\mathbf{TP})}{\partial \delta \boldsymbol{\xi}} = (\mathbf{TP})^\odot = \begin{bmatrix} \mathbf{I} & - \mathbf{P}'^\wedge \\ \boldsymbol{0}^T & \boldsymbol{0}^T \end{bmatrix} \)

 

처음 문제 정의에서 \(\mathbf{P}'\)를 처음 3개 차원만 정의한다고 하였으므로, 다음과 같이 간략하게 표현할 수 있다.

 

\( \cfrac{\partial \mathbf{P}'}{\partial \delta \boldsymbol{\xi}} = \left[ \mathbf{I}, \; - \mathbf{P}'^\wedge \right] \)

 

이렇게 구한 두 term들을 곱하면 다음과 같은 2 × 6 사이즈의 Jacobian matrix를 얻는다.

 

\( \cfrac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}} = - \begin{bmatrix} \cfrac{f_x}{Z'} & 0 & - \cfrac{f_x X'}{Z'^2} & - \cfrac{f_x X' Y'}{Z'^2} & f_x + \cfrac{f_x X'^2}{Z'^2} & - \cfrac{f_x Y'}{Z'} \\ 0 & \cfrac{f_y}{Z'} & - \cfrac{f_y Y'}{Z'^2} & - f_y - \cfrac{f_y Y'^2}{Z'^2} & \cfrac{f_y X' Y'}{Z'^2} & \cfrac{f_y X'}{Z'} \end{bmatrix} \)

 

 

이 Jacobian matrix는 left perturbation model에 대한 reprojection error의 1차 도함수를 나타낸다.

error는 '관측값 - 예측값'이므로, - 부호를 유지한다. (예측값 - 관측값 형태로 정의할 경우에는 - 부호를 뺌)

\(\mathfrak{se}(3)\)의 정의가 rotation 후 translation이라면, 처음 3개 column과 뒤쪽 3개 column 순서를 바꾸면 된다.

 

Pose를 최적화하는 것 외에도 feature point들의 3차원 좌표를 최적화 하고싶다.

따라서 \(\mathbf{e}\)를 3D 점의 좌표 \(\mathbf{P}\)에 대한 도함수도 필요하다.

마찬가지로 chain rule을 적용하면,

 

\( \cfrac{\partial \mathbf{e}}{\partial \mathbf{P}} = \cfrac{\partial \mathbf{e}}{\partial \mathbf{P}'} \cfrac{\partial \mathbf{P}'}{\partial \mathbf{P}} \)

 

첫 번째 항은 이전에 유도했고, 두 번째 항은 \(\mathbf{P}'\)의 정의에 의해 다음과 같이 계산할 수 있다.

 

\( \mathbf{P}' = (\mathbf{TP})_{1:3} = \mathbf{RP + t} \)

 

\( \therefore \cfrac{\partial \mathbf{e}}{\partial \mathbf{P}} = - \begin{bmatrix} \cfrac{f_x}{Z'} & 0 & - \cfrac{f_x X'}{Z'^2} \\ 0 & \cfrac{f_y}{Z'} & - \cfrac{f_y Y'}{Z'^2} \end{bmatrix} \mathbf{R} \)

 

 

이렇게 camera pose와 feature point 좌표 에 대한 두 개의 Jacobian matrix를 유도해 보았다.

이는 optimization 과정에서 gradient를 제공하므로 아주 중요하다.