[VSLAM 10] Perspective-n-Points(Localization)

What is the Perspective-n-Points

PnP problem으로 2d data와 3d data 간의 correspondence가 주어 졌을때 World to Camera coordinate system을 추론하는 것을 의미한다. 이때, correspondence의 쌍의 개수를 n이 된다. 이를 통해 현재 camera의 pose를 추정하는 것을 목적으로 한다.

이미지에서 Feature를 추출하고 이를 통해 map에서 현재 카메라의 위치를 추정할 수 있기 때문에 매우 중요한 개념 중 하나이다.(Localization)

이전 Triangulation에서는 하나의 3d point에 관한 descriptor 정보를 두 이미지에서 구한 feature descriptor들 이라고 말할 수 있다. PnP에서는 3d point의 descriptor들과 현재 영상에서 보이는 descripter들을 matching 해서 2d to 3d correspondence를 구한다. 충분한 수의 correspondence가 만들어 진다면, 이를 통해 camera의 정확한 pose를 구할 수 있다.

camera의 pose는 6DoF로 표현되며, 회전 성분인 $R_x, R_y, R_z$와 이동 성분인$T_x, T_y, T_z$로 구성된다. 이를 정확하게 구하기 위해서는 최소 3쌍(6개의 데이터, $\{u_1, p_1\}, \{u_2, p_2\}, \{u_3, p_3\}$)의 데이터가 필요하다.

P3P는 noise를 전혀 고려하지 않기 때문에 RANSAC과 같은 outlier를 제거할 수 있는 framework와 함께 사용된다. OpenCV에는 solvePnP() 함수가 구현되어 있다. 다만, objectPoints를 픽셀 좌표로 받기 때문에 cameraMatrix 정보가 반드시 필요하다.

P3P Grunert method(Minimal solver)

Grunert method는 projection ray의 length를 추론하는 단계와 orientation을 추론하는 단계 총 두 단계에 걸쳐 수행된다.

먼저 위 그림과 같이 3차원 공간상의 세 점에 대한 위치를 알고있을 때, camera의 pose를 추론한다.

먼저, 위 식은 ray에 대한 정보인 방향 vector($^kX^s_i$)와 scale($s_i$)을 3차원 공간의 두 점에 대한 길이를 World to Camera transformation marix인 R을 통해 Camera 영역에 투영한 것과 같은 것으로 정의한다.

방향 vector는 위 식처럼 표현된다. $X_i$는 이미지 좌표계의 한 점을 나타낸다. 여기에 Camera parameter인 K의 역행렬을 곱해주어 Image coordinate(Pixel)를 Normalized image coordinate로 가져온다. N은 방향 벡터가 유닛 벡터가 될 수 있도록 만들어주는 Normalize factor이다. 마지막으로 -sign(c)는 camera의 경우 좌표계의 기준이 카메라 뒤쪽으로 통과하는 형태이기 때문에 부호를 반전시키는 역할을 수행한다.

Projection ray의 길이를 구하는 방법

벡터 간의 dot product를 통해서 두 벡터 사이의 각도를 구할 수 있다. 이렇게 구한 각도는 정규화된 0과 1사이의 비율로 나타나기 때문에 acos을 통해 각도를 얻는다.

World coordinate의 각 점들의 정보를 알고있기 때문에 이들간의 거리를 구할 수 있다. 이렇게 구한 길이와 각도를 제2 코싸인 법칙의 형태로 만들어준다.

v를 중심으로 4차 방정식으로 해를 구하기 때문에 4개의 해가 나온다. 이 중에서 한 해만 실제 물리적인 요소를 나타내는 해이고, 나머지는 기하학적으로만 나타나는 값일 것이다. 이를 해결하기 위해서는 다른 센서 정보를 prior로써 검증하거나 또 다른 2D correspondence 정보를 가져와서 구할 수도 있다.