Introduction
지난 포스트에서는 Epipolar Geometry에서 Feature Matching을 통해 얻은 두 매칭쌍에서 항상 성립하는 Epipolar Constraint를 유도했습니다.
$$\mathbf{x_2^T[t]_x R x_1=0}$$
위 식에서 $\mathbf{x_1, x_2}$의 관계식(변환식) $\mathbf{[t]_x R}$을 Essential Matrix, 이 관계식을 Pixel Coordinate로 가져온 식을 Fundamental Matrix라고 합니다. 이렇게 나름 힘들게 구한 식을 가지고 우리는 무엇을 할 수 있을까요? Feature Matching 된 매칭쌍의 관계식을 어떠한 방법으로 구할 수 있다면, 우리는 역으로 $\mathbf{R, t}$를 구할 수 있습니다. 즉, Camera Pose Estimation을 할 수 있습니다. 요약하면, 앞으로 아래의 2-stage 과정을 통해 Camera Pose를 구하는 방법에 대해 이번 포스트를 통해 다루고자 합니다.
- Feature Matching으로 획득한 매칭쌍들을 통해 Essential Matrix or Fundamental Matrix 구하기
- Essential Matrix or Fundamental Matrix를 이용해 $R, t$ 구하기
추가로, 보통 $\mathbf{K}$(Camera Intrinsic Parameter)가 정의되어 있는 SLAM 문제에서는 보다 간단한 Essential Matrix를 통해 계산을 합니다. 이번 포스트에서도 Fundamental Matrix에 대한 설명보다는, Essential Matrix를 활용한 예제와 함께 설명을 이어나갈 생각입니다.
Essential Matrix
Essential Matrix는 $\mathbf{[t]_x R}$의 결과물이기에 $3 \times 3$ Matrix임을 알 수 있습니다. 이를 풀기 위해서는 9개의 매칭쌍을 가지고 해를 찾아야 하는데요. 보다 쉬운 해법을 위해 아래와 같은 추가적인 정보들을 이용합니다.
- Homogeneous를 가정해 획득한 Epipolar Constraint이기에, Essential Matrix 역시 Homogeneous를 만족합니다. 이는 Scale에 대해서 $\mathbf{E \cdot s}$ 역시 Epipolar Constraint를 만족함을 의미합니다.
- Essential Matrix는 $\mathbf{R}$(Rotation Matrix)와 $\mathbf{t}$(Translation Matrix, skew-symmetrix matrix)의 Matrix product 결과입니다. 여기서 $\mathbf{R}$은 계산 결과의 Singular Value 패턴을 변화시키지 않습니다. 따라서, $\mathbf{t}$의 skew-symmetrix 특성이 유지되어 Essential Mastrix의 Singular Value 역시 $\mathbf{[\sigma,\sigma,0]^T}$을 유지합니다.
- Rotation Matrix는 3 DOF(degree of freedom), Translation Matrix 역시 3 DOF를 갖고 있습니다. 그렇기에 Essential Matrix는 도합 6 DOF를 가져야 하나, 1번 항목에서 알 수 있듯이 Equivalence of Scale이 있기에 최종적으로 5 DOF를 갖게 됩니다. 즉, 최소 5개의 식(매칭쌍)을 필요로 합니다.
이렇게 세 개의 부가적인 정보를 활용해 Essential Matrix를 구하게 됩니다. 이중 특히 2번 항목의 경우 Internal constraints of the essential matrix라고 부르고 있습니다. 이제 Essential Matrix를 구해야 하는데요. 5개의 매칭쌍만을 이용해서도 구할 수 있지만, 그렇게 풀 경우 좀 과정이 복잡해져 8개의 매칭쌍을 이용한 방법을 소개하고자 합니다. 사실 매칭쌍은 너무 많아서 문제이지, 부족한 것이 문제이지는 않습니다.
Eight Point Algorithm
Essential Matrix를 풀기 위해 사용하는 매칭쌍의 개수($N$)에 따라 $N$-point Algorithm이라고 부릅니다. 여기서는 8개의 매칭쌍을 사용할 예정이니, 8-point Algorithm이겠죠? 5개만 있으면 풀 수 있는데 왜 8개를 사용하는걸까요? 간략하게 말하면, 좀 더 쉽게 풀기 위해서입니다. 5-point Algorithm은 Non-linear equation을 풀어야 합니다. 반면에, 8-point Algorithm은 Linear equation을 풀기 때문에 훨씬 난이도가 쉽습니다.
Normalized plane의 매칭쌍 하나를 가져와 봅시다. $\mathbf{x_1}=[u_1,v_1,1]^T$, $\mathbf{x_2}=[u_2,v_2,1]^T$가 있을 때 Epipolar Constraint에 따라 아래와 같이 식을 쓸 수 있습니다.
$$\begin{pmatrix}e_1 &e_2 &e_3 \\
e_4 &e_5 &e_6 \\
e_7 &e_8 &e_9 \\
\end{pmatrix} \begin{pmatrix}
u_1 \\
v_1 \\1
\end{pmatrix}=0$$
이 식을 조금 더 다듬어 봅시다.
$$(u_2u_1,u_2v_1,u_2,v_2u_1,v_2v_1,v_2,u_1,v_1,1) \cdot \begin{pmatrix}
e_1 \\
e_2 \\
e_3 \\
e_4 \\
e_5 \\
e_6 \\
e_7 \\
e_8 \\e_9
\end{pmatrix} =0$$
이제 Essential Matrix에 대한 Linear equation 하나를 완성했습니다. 이제 매칭쌍 8개를 stack 해 $8 \times 9$ Matrix로 만들어 줍시다. 그러면 $\mathbf{e}$가 8개의 매칭쌍에 대한 Null space에 위치함을 알 수 있습니다. 이에 대한 풀이는 하지 않도록 하겠습니다. 하나 알고 갈 부분은 Full rank인 경우 line으로서 해가 주어지게 됩니다. 이때 Scale Equivalence를 생각한다면, 모두가 같은 해가 되며 Unique 한 Essential Matrix를 구한 것과 같습니다. 반면에, Full rank가 아닌 경우에는 noise가 매칭쌍에 들어가 있음을 의미합니다. 이 경우 조금 번거롭지만 Total least squares problem으로 넘어가 $\mathbf{||e^TY||}$을 구해야 합니다. 하지만 Real-world에서는 noise가 섞여 있을 확률이 높기 때문에 알아두시면 좋을 것 같네요.
Camera Pose Estimation
이제 Essential Matrix를 구했습니다. 여기서 Camera Pose는 어떻게 구할 수 있을까요? 답은 SVD (singular value decomposition)을 통해 구할 수 있습니다. Essential Matrix를 2개의 Orthogonal matrix($\mathbf{U,V}$)와 1개의 Diagonal matrix($\mathbf{\Sigma}$)로 표현할 수 있습니다.
$$\mathbf{E=U\Sigma V^T}$$
그리고 우리는 이미 $\mathbf{E}$의 singular value의 형태를 알고 있습니다. 그렇기에 $\mathbf{\Sigma}=diag(\sigma,\sigma,0)$이 성립함을 알 수 있습니다. 이를 통해 구한 답 중에 두 카메라에서 모두 Positive depth를 가지고 있는 해를 구하면 그것이 우리가 원하는 $\mathbf{R, t}$가 됩니다. 이에 대한 자세한 설명은 Essential Matrix Wikipedia와 SVD에 대한 참고할만한 링크를 남겼으니 읽어보시기 바랍니다.
