S-PTAM 리뷰 - 이론(1) : Visual Odometry 1

다음과 같은 자료를 참조하였습니다.

1. https://www.ipb.uni-bonn.de/msr2-2020/ (Cyrill Stachniss 교수님의 Mobile Sensing and Robotics 강의자료)
2. http://rpg.ifi.uzh.ch/teaching.html (Davide Scaramuzza 교수님의 Vision Algorithms for Mobile Robotics 강의자료)

물체의 이동 거리와 방향을 연속적으로 추정하는 것, 또는 그 데이터를 odometry라고 부릅니다. 자동차 등 평면을 움직이는 로봇을 예로 들자면, 로봇의 위치와 방향(Pose)는 $ (x, y, \theta) $로 나타낼 수 있습니다. Odometry는 여기에 시간의 개념을 추가합니다. 즉 0초(처음)에서의 위치는 (0, 0, 0), 1초에서의 위치는 (0.1, 0.2, -0.1)... 등으로 나타낸 데이터가 odometry입니다. 정확한 odometry를 추정하는 것은 SLAM 알고리즘의 정확도를 높이기 위한 첫 걸음입니다. Odometry가 정확하다면 지도 작성은 매 순간 측정한 데이터를 이어 붙이기만 하면 되기 때문입니다.

 

그래프 기반 SLAM 방법론에서는 BA와 루프 폐쇄를 통해 각종 오차를 보정하지만, 애초에 최대한 정확한 odometry를 생성하는 것은 당연히 매우 중요합니다. 루프 폐쇄가 언제나 빠짐없이 작동한다는 보장이 있는 것도 아니고, 방문했던 장소를 재방문하지 않아 루프 검출 자체가 이루어지지 않는 상황도 생각할 수 있습니다. 누적되는 오차를 없애는 것은 불가능하지만, 최소화하려는 노력을 투자하는 것이 정확한 SLAM 구현을 위한 첫 발걸음이라고 할 수 있겠습니다.

 

가장 오래되고 단순한 odometry 측정 기법은 wheel odometry입니다. 자동차의 주행 거리계를 odometer라고 부르죠? 주행 거리계가 wheel odometry를 사용합니다.

 

 

Wheel odometry는 바퀴의 회전수를 세어서 이동 거리와 방향을 추정하는 방법론입니다. 둘레 10cm의 바퀴가 10바퀴 굴러갔다면 1m를 이동한 것이고... 그런 식입니다. 왼쪽/오른쪽 바퀴가 따로 움직이는 로봇(differential drive)의 경우에는 두 바퀴의 회전수 차이를 통해 회전 각도를 추정할 수 있고, 자동차형 로봇의 경우에도 측정하는 식이 다 있습니다.

 

하지만 대강 아실 대로 바퀴는 미끄러지기도 하고, 둘레가 항상 일정하지도 않기 때문에 정확도가 그리 좋지 않습니다. 게다가 바퀴가 안 달린 로봇(드론 등)에는 당연히 못 씁니다.

 

스테레오 카메라를 이용한 SLAM에서는 주로 visual odometry를 사용합니다. 카메라가 촬영한 2D 이미지와 실제 3D world 사이에는 기하학적인 관계가 성립하는데, 이를 이용하면 위치를 알고 있는 3차원 point들을 카메라로 촬영한 이미지를 활용해서 카메라의 pose를 추정해낼 수 있습니다.. $ x, y, z $ 좌표뿐만이 아닌 카메라의 roll, pitch, yaw까지도 알아낼 수 있는데, 이를 모두 합쳐 6D pose estimation이라고 부릅니다.

 

※ 이후의 설명을 이해하려면 컴퓨터 비전과 projective geometry에 대한 기본 지식이 필요합니다. 이런 블로그들에서 관련 지식을 얻을 수 있습니다.

 

Visual odometry를 위해 흔히 사용하는 방법론은 3D-2D PnP(Perspective n Points)입니다. 3차원 좌표를 알고 있는 점들을 카메라로 관측하여 2차원 이미지상의 좌표를 획득하고, 이 3차원-2차원 좌표 사이의 관계를 통해 카메라의 6D Pose를 추정하는 작업이라고 할 수 있습니다(3차원 좌표점들은 이전 글에서 언급했듯 스테레오 카메라를 통해 얻을 수 있습니다).

 

 

PnP를 시각화해서 설명하면 위의 그림과 같습니다. World coordinate system 기준으로 3D 좌표를 알고 있는 점 $ p_{i} (x_{0},  y_{0}, z_{0})$를 카메라로 촬영했더니 픽셀 $ u_{i} (u_{0},  v_{0}) $에 나타났습니다. $ p_{i} $와 $ u_{i} $사이의 관계는 모든 수치가 완벽하다는 가정 하에 다음 식에 의해 표현될 수 있습니다.

 

$ \lambda \begin{bmatrix}u_{0}\\v_{0}\\1\end{bmatrix} = K[R | T]\cdot \begin{bmatrix}x_{0}\\y_{0}\\z_{0}\\1\end{bmatrix}$

식(1)

 

간략하게 설명하자면 $K$는 캘리브레이션 행렬(Intrinsic calibration matrix)으로 camera coordinate system의 3차원 좌표를 2차원 픽셀 좌표로 변환하는 3x3 행렬이고, $[R|T]$는 world coordinate system의 3차원 좌표를 camera coordinate system의 3차원 좌표로 변환하는 3x4 행렬(Extrinsic calibration matrix)입니다(좌표는 모두 Homogeneous coordinates로 표현). 즉 이 식은 월드 좌표계의 3차원 좌표를 이미지 픽셀 좌표계의 2차원 좌표로 mapping하는 연산입니다.

 

근데 우리가 알고 싶은 것은 $[R|T]$, 즉 카메라의 위치입니다. 캘리브레이션 행렬 $K$는 당연히 알고 있다고 가정하면, 이제 visual odometry문제는 3D-2D point correspondency를 이용해 6D Pose ($[R|T]$)를 구하는 문제가 됩니다. 이론상으로는 미지수가 6개이기 때문에 3개의 correspondency(하나의 correspondency마다 식 2개이므로)가 필요하지만, 실제로는 더 많은 correspondency를 사용해 오차를 최소화해야 합니다. 그러면 이제 미지수의 개수보다 식의 개수가 많아지게 되고, 따라서 비선형 최적화 알고리즘을 사용해 최적해를 찾아야 합니다.

 

비선형 최적화를 이용한 PnP문제 해결의 핵심은 reprojection error의 최소화입니다. Reprojection error란 '어떤 3차원 점이 위의 식을 거쳐 변환된 2차원 픽셀 좌표'와 '실제로 그 점이 관측된 픽셀 좌표'의 차이입니다. 말로는 잘 안 와닿으니 그림을 참조하겠습니다.

 

 

이게 reprojection error입니다. 즉, 3차원 점 $ P $를 (부정확한) $ R, T $를 가지고 위의 식(1)을 이용하여 2차원 픽셀 좌표로 바꿨더니 점 $ p $로 계산됐는데, 보니까 카메라에 실제로 찍힌 점 $ P $는 거기가 아니라 $ \widetilde{p} $인 것입니다!!

 

당연히 이 오차가 발생하는 데는 여러 이유가 있습니다. 애초에 $ R, T $가 부정확하기도 하고, 3D 좌표 자체에도 오류가 있을 것이고, 픽셀 사이의 점이 하나의 픽셀로 결정되는 과정에서 일종의 반올림 등이 발생했을 수도 있고, 하여튼 수많은 원인으로 노이즈가 발생하지만 중요한 것은 이 오차를 error function으로 표기할 수 있고, 입력 데이터가 Gaussian을 따른다는 가정을 두면 이 error function을 최소화하는(비선형 최적화) 과정은 MLE(Maximum Likelihood Estimation)문제가 된다는 것입니다! 쉽게 얘기하면 반복적인 계산(비선형 최적화)을 통해서 가장 '말이 되는'(오차가 작은) $ R, T $를 구할 수 있다는(MLE) 점입니다. 이 개념은 후에 설명할 Bundle Adjustment에서도 거의 동일하게 적용되므로, 열심히 이해해 두면 좋습니다.

 

※ 비선형 최적화를 이용한 방법 외에도 $ R, T $를 구하는 방법은 존재합니다. 예를 들어, 점이 4개인 경우에는 P3P알고리즘을 사용하면 analytic하게 유일해를 계산할 수 있습니다. 이제 모든 점들에 대해 RANSAC을 사용해서 가장 feasible한 $ R, T $를 구할 수 있습니다. S-PTAM에서는 비선형 최적화를 사용하였기에 이 방법으로 설명을 진행합니다.

 

일단 위의 그림처럼 점이 1개인 경우의 error function을 써 보면 다음과 같습니다.

 

$ e(x) = \left \| \widetilde{p} - p \right \|^{2} = \left \| \widetilde{p} - \pi(P, K, R, T) \right \|^{2} $

식(2)

 

점이 $ N $개인 경우의 error function은 이렇게 되겠죠??

$ e(x) = \sum_{i=1}^{N} \left \| \widetilde{p_{i}} - \pi(P_{i}, K, R, T) \right \|^{2} $
식(3)

 

$ e(x) $는 error function이고, $ \widetilde{p} $는 실제로 찍힌 점 $ P $의 (2D 픽셀)좌표, $ p(=\pi(P, K, R, T)) $는 식(1)을 이용해 계산한 점 $ P $의 2D projected 좌표입니다. 여기서 나머지는 모두 알고 있고, 모르는 것(구하려는 것)은 $ R, T $입니다. 비선형 최적화에 관한 지식이 있다면 아시겠지만, 최적화라는 게 기본적으로 (정답에 대강 가깝지만)부정확한 점에서 시작해서 iterative하게 정답에 가까워져 가는 작업이기 때문에 $ R, T $에 대한 initial estimate(=시작점)가 필요합니다. 이 initial estimate를 설정하는 방법은 여러 가지가 있습니다. 그냥 이전 프레임의 $ R, T $를 그대로 사용할 수도 있고(프레임 사이 간격이 길지 않다면 현재의 $ R, T $와 크게 차이나지는 않겠죠??), wheel odometry를 사용할 수도 있습니다. S-PTAM에서는 decaying velocity model을 사용하여 이전 프레임의 위치/속도 데이터를 기반으로 현재의 $ R, T $를 대략적으로 추정하고 이것을 initial estimate으로 사용합니다. Decaying velocity model은 그렇게 복잡한 것은 아니고, 

 

(이전의 위치) +  $ \alpha $ $ \times $ (이전의 속도 $ \times $ 현재 프레임과 이전 프레임 사이의 시간)

 

이라는 간단한 식을 사용해서 현재 위치를 예측합니다. 저 $ \alpha $가 1보다 작기 때문에 decaying이라는 단어가 붙은 것으로, 이게 없어도 큰 상관은 없습니다. 어쨌든 이러저러한 방법을 사용해서 현재 프레임에서의 대강의 $ R, T $를 얻게 되었다면 이것을 시작점으로 비선형 최적화 알고리즘을 돌려서 식(3)으로 계산되는 error function을 최소화하면 statistically optimal한 $ R, T $(이 경우에는 오차가 최소화되는 $ R, T$)를 얻게 됩니다. 이걸 매 프레임마다 반복하는 것이 모든 SLAM 방법론의 기초이고, 이것을 visual odometry라고 합니다.

 

다음 포스트에서 실제로 비선형 최적화를 수행해서 $ R, T $를 얻는 방법을 설명하도록 하겠습니다.