solvePnP 함수 사용법과 Rodrigues 표현법

opencv의 solvePnP 함수 사용법과 3차원 회전변환을 표현하는 한 방법인 Rodrigues' rotation formula에 대한 내용입니다.

1. solvePnP 사용법

solvePnP 함수는 영상을 획득한 카메라의 위치 및 방향 (camera pose)을 알아낼 때 유용하게 사용할 수 있습니다만, 인터넷에 사용법 정보가 잘 없는 것 같아 여기에 적어봅니다.

[solvePnP 사용예제]

// matching pairs

vector<Point3f> objectPoints; // 3d world coordinates

vector<Point2f> imagePoints; // 2d image coordinates

// camera parameters

double m[] = {fx, 0, cx, 0, fy, cy, 0, 0, 1}; // intrinsic parameters

Mat A(3, 3, CV_64FC1, m); // camera matrix

double d[] = {k1, k2, p1, p2}; // k1,k2: radial distortion, p1,p2: tangential distortion

Mat distCoeffs(4, 1, CV_64FC1, d);

// estimate camera pose

Mat rvec, tvec; // rotation & translation vectors

solvePnP(objectPoints, imagePoints, A, distCoeffs, rvec, tvec);

// extract rotation & translation matrix

Mat R;

Rodrigues(rvec, R);

Mat R_inv = R.inv();

Mat P = -R_inv*tvec;

double* p = (double *)P.data;

// camera position

printf("x=%lf, y=%lf, z=%lf", p[0], p[1], p[2]);

위에서 구한 P는 카메라의 위치를 나타내고, 회전변환(월드좌표->카메라좌표) 행렬 R로부터는 카메라의 자세(방향) 정보를 추출할 수 있습니다.

2. Rodrigues' rotation formula (이하 2014.4.22 추가된 내용)

opencv의 solvePnP 함수에서 반환되는 rvec는 Rodrigues를 컴팩트(compact)하게 표현한 벡터입니다. 먼저, Rodrigues가 무엇인지 살펴본 후 opencv에서 사용하는 Rodrigues 표현법에 대해 살펴보겠습니다.

3차원에서 회전변환은 보통 3 × 3 행렬로 표현됩니다. 그런데, Rodrigues를 사용하면 임의의 3차원 회전변환을 4개의 값(회전축 벡터 + 회전각) 만으로 표현할 수 있습니다. 3차원 공간에서 점 p를 회전축 v에 대하여 θ만큼 회전시킨 값은 다음 식에 의해 계산될 수 있는데, 이 식을 Rodrigues' rotation formula라고 부릅니다 (Rodrigues는 이 식을 만든 프랑스 수학자의 이름).

 --- (1)

이 때, v는 단위벡터(unit vector)이어야 하고 회전방향은 오른손 법칙을 따릅니다 (엄지를 펴고 오른손을 쥐었을 때 엄지의 방향이 회전축 방향, 쥔 손가락의 방향이 + 회전방향).

식 (1)을 행렬 형태로 표현하면 다음과 같습니다.

 --- (2)

식 (2)로부터 Rodrigues v = (vx,vy,vz), θ에 대응하는 회전변환 행렬 R이 다음과 같음을 알 수 있습니다.

 --- (3)

식 (3)은 우리가 임의의 회전축에 대한 회전을 회전변환 행렬로 표현할 수 있음을 나타냅니다.

반대의 경우로, 임의의 회전변환 행렬에 대한 Rodrigues 표현은 다음 수식을 이용하여 구할 수 있다고 합니다 (참조: 위키피디아).

 --- (4)

3. opencv에서의 Rodrigues 표현

opencv에서는 회전변환행렬 표현과 Rodrigues 표현 사이의 상호 변환을 위해 Rodrigues()란 함수를 제공합니다. 그런데, opencv에서 사용하는 Rodrigues 표현은 보다 컴팩트(compact)한 형태로서 단 3개의 값만으로 회전변환을 표현합니다.

opencv API 설명문서에 따르면 원래 회전변환은 3 자유도이기 때문에 opencv에서는 회전변환을 rod2 = [a, b, c]의 3차원 벡터로 컴팩트하게 표현하고 이로부터 회전각(θ) 및 회전축 벡터(v)는 다음과 같이 추출합니다.

 --- (5)

 --- (6)

이와같이 solvePnP 함수에서 반환되는 rvec는 Rodrigues에 대한 3차원의 컴팩트한 표현법이므로 회전각 및 회전축을 알기 위해서는 식(5), 식(6)을 이용해야 합니다.

by 다크 프로그래머