Extrinsic Camera Calibration - 카메라의 위치 및 자세 파악

그동안 카메라 영상 기하학에 관련된 글을 많이 올렸었는데, 이번 글은 그동안 올렸던 글들을 정리함과 동시에 마무리 단계로서 카메라의 외부 파라미터(extrinsic parameter)인 3차원 위치 및 자세(팬, 틸트 또는 pitch, roll, yaw)를 파악하는 방법을 정리해 보고자 합니다.

이 글에서 다룰 extrinsic camera calibration은 아래 그림과 같이 크기 및 형태를 미리 알고 있는 외부의 물체(마크)에 대한 영상을 분석하여 이 영상을 획득할 당시의 카메라의 3차원 위치(지면으로부터 높이 등) 및 3D 자세 정보(팬, 틸트 등)를 추출하는 것이 목적입니다.

<그림 1> 마크그림 출처: 3M Neo-CAPS

1. 관련 글

먼저, 그동안 올렸던 영상 기하학과 관련된 글들을 간략히 정리해 보면 다음과 같습니다.

카메라 캘리브레이션 (Camera Calibration)

-> 카메라 캘리브레이션에 대한 전반적인 내용 소개 (초점거리, 주점, 왜곡계수 등 내부 파라미터 설명 및 캘리브레이션 툴 소개)

카메라 왜곡보정 - 이론 및 실제

-> 렌즈계의 영상왜곡 모델(수식) 및 보정 방법 소개

[영상 Geometry #1] 좌표계

[영상 Geometry #2] Homogeneous Coordinates

[영상 Geometry #3] 2D 변환 (Transformations)

[영상 Geometry #4] Homography 보완

[영상 Geometry #5] 3D 변환

[영상 Geometry #6] 이미지 투영(Imaging Geometry)

[영상 Geometry #7] Epipolar Geometry

->카메라 영상 기하학에 대한 전반적인 내용을 소개. #1 글에서는 영상기하학에서 사용하는 좌표계(월드 좌표계, 카메라 좌표계, 정규 이미지 좌표계, 픽셀 좌표계)의 개념 및 관계를 설명. #2: homogeneous 좌표 표현 소개. #3: 2D 이미지 상에서 물체의 변화를 모델링하기 위한 다양한 2D 변환 모델들을 소개. #4: 2D homography의 유효성을 판단하기 위한 팁 소개. #5: 3D 공간에서 물체의 변화를 모델링하기 위한 다양한 3D 변환 모델들을 소개. #6. 물체의 3D 공간좌표(월드좌표)가 어떤 과정을 거쳐서 2D 이미지 픽셀 좌표로 매핑되는지 설명. #7. essential matrix, fundamental matrix등 3차원 물체에 대한 카메라 영상과 영상 사이의 매핑 모델 소개

3D 좌표계 변환 방법 (예: 월드좌표계 - 카메라 좌표계)

-> 카메라의 3차원 위치 및 자세정보(팬,틸트)를 알고 있을 때, 3차원 월드좌표를 카메라 좌표로 변환시키는 변환 행렬을 구하는 방법 소개

solvePnP 함수 사용법과 Rodrigues 표현법

-> 3D월드좌표-2D이미지픽셀좌표 쌍들로부터 카메라의 3차원 위치 및 자세를 추출하는데 사용되는 함수인 opencv의 solvePnP 함수 사용법 예제 샘플코드

타겟 추적을 위한 카메라의 팬,틸트 제어

-> 추적하고자 하는 대상이 항상 카메라 영상 중심에 오도록 카메라의 팬, 틸트를 제어하기 위한 방법 및 수식 설명

2. 3D 공간좌표와 2D 영상좌표

위와 같이 그동안 영상 기하학(geometry)에 대한 여러 글들을 올렸지만 그 핵심은 결국 2D 영상을 3차원으로 해석하는데 있습니다.

그런데 주변에 보면 이미지를 3차원으로 해석하는 문제는 아예 딴세상의 영역으로 치부하고 막연한 두려움 또는 어려움을 느끼는 경우가 많은데 사실 알고보면 그렇게 어려운 수학이 필요한 것은 아닙니다. 카메라 좌표계 입장에서만 본다면 우리가 중학교때 배우는 도형의 닮음비 정도만 이용해도 충분히 수식 도출이 가능합니다.

월드좌표계는 무시하고 카메라를 중심으로 3차원 좌표계(카메라의 광학축 방향이 Z축, 오른쪽이 X축, 아래쪽이 Y축)를 설정했을 때, 외부의 한점 P(X,Y,Z)에 대한 영상좌표를 구하는 문제를 생각해 보겠습니다.

<그림 2>

그러면 P(X,Y,Z)는 카메라로부터 (수직방향으로) Z만큼 떨어진 거리에 있는 점이므로 카메라로부터의 거리가 1인 가상의 정규 이미지 평면에서의 좌표는 삼각형의 닯음비를 이용하면 P'(X/Z,Y/Z,1)이 됨을 쉽게 알수 있습니다. 그런데 실제 카메라 영상은 초점거리 f만큼 떨어진 이미지 평면에 투사되므로 P에 대응되는 이미지 좌표는 P''(f*X/Z, f*Y/Z, f)가 되고 이를 픽셀좌표계로 변환하면 우리가 원하는 최종 영상좌표를 얻을 수 있습니다.

좀더 구체적으로 들어가 보면, 카메라의 초점거리는 X축 방향은 fx, Y축 방향은 fy, 이렇게 서로 다르게 측정하여 사용하기 때문에 실제로는 P''을 (fx*X/Z, fy*Y/Z)로 계산하고, 이를 픽셀좌표계(이미지의 좌상단이 원점)에 맞추어 주면 영상중심을 (cx, cy)라 했을 때 x = fx*X/Z + cx, y = fy*Y/Z + cy가 최종 영상좌표가 됩니다(카메라 좌표계의 좌표축 방향을 어떻게 잡느냐에 따라서 변환 수식은 조금씩 달라질 수 있습니다).

역으로 2D 영상좌표 p(x, y)에 대응하는 3D 공간좌표를 구하고자 할 때에는 먼저 (x-cx, y-cy)로 영상좌표 원점을 영상중심으로 옮긴 후 Z거리가 1인 정규이미지 평면에서의 좌표 ((x-cx)/fx, (y-cy)/fy)로 바꿉니다. 다음으로 실제 물체와의 거리 Z를 반영해 주면 ((x-cx)/fx*Z, (y-cy)/fy*Z, Z)가 구하고자 하는 3차원 공간좌표가 됩니다(물론 거리 Z를 모른다면 3차원 좌표는 유일하게 결정되지 않습니다).

이상으로 설명한 2D-3D 변환 모델은 정말 이렇게 단순해? 할 수도 있겠지만 실제 카메라 영상에서 그대로 성립하는 관계입니다. 물론 보다 정확한 변환을 위해서는 렌즈계 왜곡모델이 반영되어야 하겠지만 영상왜곡이 심하지 않은 경우에는 위 변환모델을 그대로 사용해도 무방합니다.

3. 카메라의 3D 위치 및 자세 파악

원래의 주제로 돌아가서 카메라 영상을 보고 이 영상을 획득할 당시의 카메라의 3차원 위치 및 자세정보(팬, 틸트, ...)를 추출하는 문제를 살펴보겠습니다. 단, 실제 계산은 opencv의 solvePnP 함수를 활용하는 것으로 하겠습니다(solvePnP 함수에 대한 예제코드는 solvePnP 함수 사용법 글 참조).

☞ 이미 예전에 solvePnP 함수 사용법에 대한 글을 올린 바가 있지만 해당 예제코드에는 자세정보를 추출하는 과정에 대한 내용이 없고 또한 수식에 대한 배경 설명이 없기 때문에 저조차도 해당 기능이 필요할 때마다 한참을 생각해야 겨우 정리가 되는 경우가 많았습니다. 그래서 여기에 다시 한번 관련 내용을 정리해 놓고 필요할 때마다 참조하고자 합니다.

먼저, 영상으로부터 카메라의 위치 및 자세를 계산하려면 다음과 같은 전제 조건이 필요합니다.

• 해당 카메라에 대한 내부 파라미터(intrinsic parameters) 및 왜곡계수을 알고 있어야 함: fx, fy, cx, cy, k1, k2, p1, p2
• 물체에 대한 최소 4개 이상의 3D 월드좌표와 이에 대응되는 2D 영상좌표 쌍

예를 들어, 아래 그림과 같이 한변의 길이가 1인 사각형 마크를 바닥에 놓고 카메라로 영상을 획득했을 때 각각의 사각형 꼭지점들에 대한 영상좌표를 구한 후, 사각형 꼭지점들의 3D 월드좌표와 2D 영상좌표 쌍들을 solvePnP 함수에 넣어주면 카메라의 위치 및 자세정보가 나옵니다.

<그림 3>

☞ 위 그림 3에서는 사각형 마크의 월드좌표를 (0,0,0), (1,0,0), (0,1,0), (1,1,0)로 설정해 주었지만 만일 마크를 기준으로 월드좌표계를 설정하지 않고 별도의 월드좌표계를 설정(예를 들어 방의 한쪽 모서리를 원점으로 설정)하고자 할 때에는 해당 좌표계를 기준으로 한 좌표를 마크의 월드좌표로 입력해 주면 됩니다. 그러면 solvePnP 함수에서는 설정한 월드좌표계를 기준으로 한 카메라의 위치 및 자세정보를 반환하게 됩니다.

solvePnP 함수는 기본적으로 3D 월드좌표를 3D 카메라좌표로 변환시키는 변환정보(rvec, tvec)을 반환하며 이로부터 회전변환 R과 평행이동 T를 아래와 같이 계산할 수 있습니다..

Mat rvec, tvec; // rotation & translation vectors

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

Mat R, T;

Rodrigues(rvec, R);

T = tvec;

☞ opencv의 solvePnP 함수가 반환하는 rvec은 회전변환에 대한 Rodrigues 표현이기 때문에 실제 회전변환 행렬 R은 Rodrigues() 함수를 통해 추출해야 합니다. Rodrigues 회전변환 표현법에 대해서는 solvePnP 함수 사용법과 Rodrigues 표현법 글을 참조하기 바랍니다.

즉, 3차원 공간상의 한점 P에 대한 월드좌표를 Pw = [xw, yw, zw]^T, 카메라 좌표계에서 봤을 때의 좌표를 Pc = [xc, yc, zc]^T, solvePnP 함수가 반환하는 회전변환 행렬을 R, 평행이동 벡터를 T라고 했을 때 다음과 같은 변환 관계식이 성립합니다.

 --- (1)

이 때, 카메라의 3D 위치(월드좌표)는 카메라 좌표계의 원점에 대응하는 월드좌표이므로 다음과 같이 계산됩니다.

 --- (2)

다음으로 카메라의 3D 자세정보를 계산해 보겠습니다.

먼저, 카메라의 팬(pan)과 틸트(tilt)는 카메라의 광학축에 대한 월드좌표를 구하면 됩니다. 자세정보를 구하는데 있어서 평행이동은 관계가 없는 요소이기 때문에 회전변환만을 고려하여 카메라 광학축 벡터 Zc = [0, 0, 1]^T에 대한 월드좌표 Zw를 다음과 같이 계산합니다.

 --- (3)

이렇게 계산된 광학축에 대한 월드좌표를 Zw = [zx, zy, zz]^T라 하면 카메라의 팬, 틸트는 다음과 같이 계산됩니다 (단, 팬, 틸트는3D 좌표계 변환 방법 (예: 월드좌표계 - 카메라 좌표계) 글에 설명된 정의를 따른다고 가정).

 --- (4)

pitch, roll, yaw 관점에서 보면 계산된 틸트(tilt)는 pitch, 팬(pan)은 yaw에 해당합니다.

다음으로, 카메라의 roll 각은 카메라 좌표계의 X축 벡터 Xc = [1, 0, 0]^T에 대한 월드좌표 벡터 Xw = [xx, xy, xz]^T와 월드좌표계의 X축을 Z축을 중심으로 θpan만큼 회전시킨 Xpan 사이의 회전각으로 계산됩니다 (단, roll은 카메라 광학축을 기존으로 한 회전각으로서, 카메라와 같은 방향을 바라볼 때 반시계 방향이 + 방향)

 --- (5)

단, sign() 함수는 () 안의 값이 양수면 1, 음수면 -1인 부호함수.

이상과 같이 어떤 물체에 대한 월드좌표-영상좌표 대응쌍을 4개만 알면 카메라에 대한 모든 위치 및 3D 자세정보를 파악할 수 있습니다.

이상의 내용을 코드로 정리하면 다음과 같습니다.

// 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

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

// extract rotation matrix

Mat R;

Rodrigues(rvec, R);

Mat R_inv = R.inv();

// camera position (X,Y,Z)

Mat Cam_pos = -R_inv*tvec;

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

X = p[0];

Y = p[1];

Z = p[2];

// pan(yaw) & tilt(pitch)

double unit_z[] = {0,0,1};

Mat Zc(3, 1, CV_64FC1, unit_z);

Mat Zw = R_inv*Zc; // world coordinate of optical axis

double* zw = (double *)Zw.data;

pan = atan2(zw[1], zw[0]) - CV_PI/2;

tilt = atan2(zw[2], sqrt(zw[0]*zw[0]+zw[1]*zw[1]));

// roll

double unit_x[] = {1,0,0};

Mat Xc(3, 1, CV_64FC1, unit_x);

Mat Xw = R_inv*Xc; // world coordinate of camera X axis

double* xw = (double *)Xw.data;

double xpan[] = {cos(pan), sin(pan), 0};

roll = acos(xw[0]*xpan[0] + xw[1]*xpan[1] + xw[2]*xpan[2]); // inner product

if(xw[2]<0) roll = -roll;

4. 참고사항: 호모그래피(Homography) (2014.4.22일 수정)

3차원 공간상에 한 평면형(planar) 물체가 있고 이 물체를 서로다른 두 위치 A, B에서 카메라로 찍은 두 영상이 있습니다. 이 때, 이 물체 상의 점 P를 A 영상 위치에서 바라본 3차원 카메라 좌표 X1과 B 영상 위치에서 바라본 카메라 좌표 X2 사이에는 어떤 선형변환관계 H가 성립하는데, 이 변환관계를 호모그래피(homography)라 부릅니다 (카메라 좌표계의 정의에 대해서는 [영상 Geometry #1] 좌표계 글 참조).

 --- (6)

이 때, 점 P를 A 카메라 위치에서 정규 이미지 평면에 투영시킨 이미지 좌표를 x1, B 위치에서 투영시킨 좌표를 x2, A 위치에서의 P의 depth를 d1, B 위치에서의 depth를 d2라 하면 X1 = d1x1, X2 = d2x2이므로 두 이미지 좌표 사이에도 다음과 같이 동일한 호모그래피 관계식이 성립합니다 (단, x1, x2는 (u, v, 1) 등과 같은 homogeneous 좌표 표현).

 --- (7)

단, ∼는 스케일(scale)을 무시했을 때 동일하다는 의미입니다 (equality up to scale).

즉, 호모그래피(homography)는 평면형(palanar) 물체에 대한 3D 카메라 좌표들 사이, 또는 2D 이미지 좌표들 사이에 성립하는 매핑관계 혹은 변환관계를 지칭하는 용어로서 보통 4개의 매칭쌍이 있으면 두 영상 사이의 호모그래피를 결정할 수 있습니다.

호모그래피(homography)에서 중요한 전제조건은 물체가 체스판 등과 같은 평면형(planar) 물체여야 한다는 점입니다. 만일 대상 물체가 주사위 등과 같이 3차원의 부피를 갖는 입체형 물체라면 이 물체에 대한 영상좌표들 사이에는 호모그래피(homography)가 성립하지 않습니다. 이러한 경우 즉, 평면형 물체가 아닌 경우의 영상좌표들 사이에는 호모그래피(homography) 대신 fundamental matrix나 essential matrix를 이용해야 합니다.

정리해 보면, '호모그래피(homography), fundamental matrix, essential matrix는 모두 2D 이미지 좌표 또는 3D 카메라 좌표 사이의 변환관계를 나타내는 용어이다. 단, 호모그래피는 물체가 평면형 물체일 경우에만 성립하고 일반적인 경우에는 fundamental matrix나 essential matrix를 사용해야 한다' 입니다.

☞ 기존 내용에서 호모그래피의 정의 부분에 오류가 있어서 4.22일자로 내용을 수정하였습니다.

by 다크 프로그래머