A L I D A
해당 내용은 공부 목적으로 작성된 글입니다.
잘못된 부분이나 수정할 부분이 있다면 댓글이나 메일로 알려주시면 확인 후 수정하도록 하겠습니다.
본 포스트는 MVG part1 에서 설명한 Zhang's method(ICCV 1999)[1] 부분을 C++ 코드로 구현한 내용을 리뷰한다. 예제 코드는 우분투 18.04 환경에서 테스트하였으며 cmake 3.16, opencv 4.2, eigen 3.3.7 버전을 사용하여 정상적으로 작동하는 것을 확인하였다. 예제 코드는 링크를 클릭하여 다운로드하면 된다.
Zhang's Calibration
Input Image Setup
20mm 간격으로 구성된 7x9 체커보드를 인쇄하여 평평한 보드에 붙인 후 다양한 각도에서 촬영한 5장의 사진을 준비한다. 간단한 예제 구현을 위해 촬영에 사용된 카메라의 lens distortion은 없다고 가정한다.
Matlab Example
위에서 촬영한 5장의 사진을 matlab의 cameraCalibrator 툴에 넣어주면 자동으로 체크보드의 좌표를 인식하여 위와 같이 카메라 행렬 $\mathbf{P}_{i}, i=1,2,3,4,5$를 추정해준다. 본 포스트에서는 이를 통해 추정한 $\mathbf{P}_{i,mat} = \mathbf{K}_{mat}[\mathbf{R}_{i,mat} | \mathbf{t}_{i,mat}], \quad i=1,2,3,4,5$를 ground truth로 가정하여 zhang's method의 성능을 평가한다. ground truth 값은 다음과 같다.
$$\mathbf{K}_{mat} = \begin{bmatrix} 3331.893 & 0 & 1939.063 \\ 0 & 3329.977 & 1000.860 \\ 0 & 0 & 1 \end{bmatrix}$$
$$\mathbf{R}_{1,mat} = \begin{bmatrix} 0.998 & -0.055 & 0.001 \\ 0.045 & 0.827 & 0.560 \\ -0.032 & -0.559 & 0.828 \end{bmatrix}, \mathbf{t}_{1,mat} = \begin{bmatrix} -71.23 \\ -59.702 \\ 427.161 \end{bmatrix}$$
$$\mathbf{R}_{2,mat} = \begin{bmatrix} 0.995 & 0.036 & -0.089 \\ -0.043 & 0.996 & -0.068 \\ 0.086 & 0.072 & 0.993 \end{bmatrix}, \mathbf{t}_{2,mat} = \begin{bmatrix} -29.158 \\ -9.54 \\ 422.498 \end{bmatrix}$$
$$\mathbf{R}_{3,mat} = \begin{bmatrix} 0.994 & -0.081 & 0.067 \\ 0.0137 & 0.732 & 0.681 \\ -0.104 & -0.676 & 0.729 \end{bmatrix}, \mathbf{t}_{3,mat} = \begin{bmatrix} -49.457 \\ -37.856 \\ 423.639 \end{bmatrix}$$
$$\mathbf{R}_{4,mat} = \begin{bmatrix} 0.985 & -0.012 & 0.171 \\ 0.022 & 0.998 & -0.058 \\ -0.17 & 0.061 & 0.983 \end{bmatrix}, \mathbf{t}_{4,mat} = \begin{bmatrix} -93.934 \\ -40.686 \\ 382.775 \end{bmatrix}$$
$$\mathbf{R}_{5,mat} = \begin{bmatrix} 0.986 & -0.051 & 0.155 \\ -0.005 & 0.937 & 0.347 \\ -0.163 & -0.343 & 0.924 \end{bmatrix}, \mathbf{t}_{5,mat} = \begin{bmatrix} -67.523 \\ 2.961 \\ 658.088 \end{bmatrix}$$
Zhang's Calibration
체커보드를 통해 카메라 행렬 $\mathbf{P}$를 추정하는 알고리즘을 Zhang's Method라고 한다. 월드 상의 체커보드 평면 $\pi_{0}$가 주어졌을 때 월드 상의 원점을 체커보드의 좌측 상단으로 설정하고 체커보드의 평면이 $Z=0$인 평면으로 설정한다.
\begin{equation}
\begin{aligned}
\pi_{0} = \{ \mathbf{X}=(X,Y,Z)^{\intercal} \ | \ Z=0 \}
\end{aligned}
\end{equation}
이에 따라 체커보드 평면 $\pi_{0}$위의 임의의 점 $\mathbf{X}_{i}$들은 $Z=0$인 점이 된다.
\begin{equation}
\begin{aligned}
\mathbf{X}_{i} = (\ast,\ \ast,\ 0)^{\intercal}
\end{aligned}
\end{equation}
체커보드 상의 한 점 $\mathbf{X}=(X,\ Y,\ 0,\ 1)^{\intercal}$를 프로젝션하면
\begin{equation}
\begin{aligned}
\mathbf{PX} & = \mathbf{P} \begin{pmatrix} X\\Y\\0\\1 \end{pmatrix} = \mathbf{K}[\mathbf{R} \ | \ \mathbf{t}] \begin{pmatrix} X\\Y\\0\\1 \end{pmatrix} \\
& = \mathbf{K}[\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}]\mathbf{X}
\end{aligned}
\end{equation}
가 된다. $z=0$이므로 행렬 $\mathbf{R}$의 세 번째 열벡터(column vector)는 0이 된다. 행렬 $\mathbf{K}[\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}] \in \mathbb{R}^{3\times 3}$는 체커보드 평면 $\pi_{0}$에서 이미지 평면 $\pi$로 변환하는 Homography $\mathbf{H}$로 볼 수 있다.
\begin{equation}
\begin{aligned}
\mathbf{H} = \mathbf{K}[\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}]
\end{aligned}
\end{equation}
체커보드는 패턴의 길이와 갯수를 모두 알고 있기 때문에 자동적으로 체커보드 평면 $\pi_{0}$ 상의 점 $\mathbf{x}_{i},i=1,\cdots$를 알 수 있다. 다음으로 Feature Extraction 알고리즘을 사용하면 이미지 평면 $\pi$ 상에서 본 $\pi_{0}$의 점 $\mathbf{x}^{\prime}_{i}, i=1,\cdots$들을 알 수 있다. 따라서 대응점 쌍 $\mathbf{x}_{i} \in \pi_{0} \leftrightarrow \mathbf{x}_{i}^{\prime} \in \pi$을 얻을 수 있다. 이를 통해 $\pi_{0} \mapsto \pi$로 가는 Homography $\mathbf{H}$를 계산해보면
\begin{equation}
\begin{aligned}
& \mathbf{H} = \begin{bmatrix} \mathbf{h}_{1,col} & \mathbf{h}_{2,col} & \mathbf{h}_{3,col} \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{r}_{1,col} & \mathbf{r}_{2,col} & \mathbf{t} \end{bmatrix} \\
\end{aligned}
\end{equation}
가 된다. 이를 정리하면
\begin{equation}
\begin{aligned}
& \mathbf{K}^{-1}\begin{bmatrix} \mathbf{h}_{1,col} & \mathbf{h}_{2,col} & \mathbf{h}_{3,col} \end{bmatrix} = \begin{bmatrix} \mathbf{r}_{1,col} & \mathbf{r}_{2,col} & \mathbf{t} \end{bmatrix}
\end{aligned}
\end{equation}
가 된다. 이 때, 직교행렬 $\mathbf{R}$의 열벡터인 $\mathbf{r}_{1,col}$과 $\mathbf{r}_{2,col}$은 서로 직교하므로 $\mathbf{r}_{1,col}^{\intercal}\mathbf{r}_{2,col}=0$이 성립한다. 해당 제약조건을 이용하면 $\mathbf{K}^{-1}\mathbf{h}_{1,col} = \mathbf{r}_{1,col}$이고 $\mathbf{K}^{-1}\mathbf{h}_{2,col} = \mathbf{r}_{2,col}$이므로 직교조건에 의해
\begin{equation}
\label{eq:1386}
\begin{aligned}
\mathbf{h}_{1,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{2,col} = 0
\end{aligned}
\end{equation}
이 성립한다. 또한 직교행렬 조건에 의해 스케일을 제외하고(up to scale) $\mathbf{r}_{1,col}^{\intercal}\mathbf{r}_{1,col} = \mathbf{r}_{2,col}^{\intercal}\mathbf{r}_{2,col}$이므로
\begin{equation}
\label{eq:1395}
\begin{aligned}
& \mathbf{h}_{1,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{1,col} = \mathbf{h}_{2,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{2,col}
\end{aligned}
\end{equation}
공식이 성립한다. 한 개의 체커보드 사진으로 부터 위와 같은 두 개의 방정식을 얻을 수 있다. 일반적인 카메라 캘리브레이션 행렬 $\mathbf{K}$는
\begin{equation}
\begin{aligned}
\mathbf{K} = \begin{bmatrix} f_{x} & s & x_{0} \\ & f_{y} & y_{0} \\ &&1 \end{bmatrix}
\end{aligned}
\end{equation}
과 같이 5개의 변수를 가지고 있으므로 $\mathbf{K}^{-\intercal}\mathbf{K}^{-1}$ 또한 5개의 변수를 가지고 있는 행렬이 된다. 따라서 최소 세 개 이상의 Homography $\mathbf{H}_{j}, \ j=1,2,3$가 주어졌을 때 $\mathbf{K}^{-\intercal}\mathbf{K}^{-1}$를 결정할 수 있다.
- 5개의 파라미터를 갖고 있는 행렬 $\mathbf{K}$를 구하기 위해 최소 세 개 이상의 체커보드 이미지를 획득한다. 하나의 이미지 당 두 개의 방정식을 얻을 수 있으므로 세 장 이상을 획득해야 한다. 각각의 이미지마다 Homography $\mathbf{H}_{j},\ j=1,2,3$을 구할 수 있고 공식 (1), (2)를 구한다.
- $\mathbf{S} =\mathbf{K}^{-\intercal}\mathbf{K}^{-1}$로 설정한다. 행렬 $\mathbf{S}$에 Cholesky Decomposition 또는 특이값 분해(SVD)를 수행하여 $\mathbf{K}^{-1}$을 구한다. 행렬 $\mathbf{S}$는 대칭이면서 Positivie Definite 행렬이므로 $\mathbf{U}^{\intercal}\mathbf{DU}$와 같이 분해되며 대각행렬 $\mathbf{D}$의 제곱근 행렬이 존재한다.
\begin{equation}
\begin{aligned}
\text{SVD}(\mathbf{S}) = \mathbf{U}^{\intercal}\mathbf{D}\mathbf{U} = (\mathbf{U}\sqrt{\mathbf{D}})(\mathbf{U}\sqrt{\mathbf{D}})^{\intercal}
\end{aligned}
\end{equation}
이를 통해 $\mathbf{K}$를 구할 수 있다.
- $\mathbf{K}^{-1}\mathbf{H} = [\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}]$ 식을 통해 $\mathbf{r}_{1,col}, \mathbf{r}_{2,col}, \mathbf{t}$를 구한 다음
\begin{equation}
\begin{aligned}
\mathbf{r}_{3,col} = \mathbf{r}_{1} \times \mathbf{r}_{2}
\end{aligned}
\end{equation}
를 통해 구한다. 결론적으로 각각의 Homography $\mathbf{H}_{j},\ j=1,2,3$을 통해 카메라의 회전 $\mathbf{R}$과 이동 $\mathbf{t}$ 그리고 내부 파라미터 행렬 $\mathbf{K}$를 구할 수 있다.
Code Review
예제 코드는 링크를 클릭하여 다운로드하면 된다. 전체 코드는 다음과 같다.
#include "util.h"
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
void GetChessboardCorners(
std::vector<Eigen::Matrix<double, 63, 2>> &pixel_corners) {
Eigen::Matrix<double, 63, 2> c1, c2, c3, c4, c5;
...
pixel_corners[0] = c1;
pixel_corners[1] = c2;
pixel_corners[2] = c3;
pixel_corners[3] = c4;
pixel_corners[4] = c5;
}
std::vector<Eigen::Matrix3d>
ComputeHomography8Point(std::vector<Eigen::Matrix<double, 63, 2>> pixel_corners,
Eigen::Matrix<double, 63, 4> world_points) {
std::vector<Eigen::Matrix3d> Hs(5);
Eigen::MatrixXd A(2 * 63, 9);
for (int i = 0; i < 5; i++) {
for (int j = 0; j < 63; j++) {
double X = world_points.row(j)(0);
double Y = world_points.row(j)(1);
double W = world_points.row(j)(3);
double u = pixel_corners[i].row(j)(0);
double v = pixel_corners[i].row(j)(1);
Eigen::Matrix<double, 9, 1> a1, a2;
a1 << X, Y, W, 0, 0, 0, -u * X, -u * Y, -u * W;
a2 << 0, 0, 0, X, Y, W, -v * X, -v * Y, -v * W;
int k = 2 * j;
A.row(k) = a1.transpose();
A.row(k + 1) = a2.transpose();
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU |
Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
Eigen::Matrix<double, 9, 1> h = V.col(V.cols() - 1);
Eigen::Matrix3d H = Eigen::Map<Eigen::Matrix3d>(h.data());
H = H / H(2, 2);
Hs[i] = H.transpose();
}
return Hs;
}
int main() {
std::vector<cv::Mat> images;
for (int i = 1; i <= 5; i++) {
std::string fn_img = util::GetProjectPath() +
"/picture/zhang_calibration/" + std::to_string(i) +
".jpg";
cv::Mat img = cv::imread(fn_img, cv::IMREAD_GRAYSCALE);
images.push_back(img);
}
std::vector<Eigen::Matrix<double, 63, 2>> pixel_corners(5);
GetChessboardCorners(pixel_corners);
Eigen::Matrix<double, 63, 4> world_points;
int gap = 20; // [mm]
for (int r = 0; r < 9; r++) {
for (int c = 0; c < 7; c++) {
world_points.row(7 * r + c) =
Eigen::Vector4d(r * gap, c * gap, 0, 1).transpose();
}
}
std::vector<Eigen::Matrix3d> Hs;
Hs = ComputeHomography8Point(pixel_corners, world_points);
Eigen::Matrix<double, 10, 6> A;
for (int i = 0; i < 5; i++) {
Eigen::Matrix<double, 3, 3> H = Hs[i];
Eigen::Matrix<double, 6, 1> a1, a2;
double h1 = H(0, 0);
double h2 = H(0, 1);
double h3 = H(0, 2);
double h4 = H(1, 0);
double h5 = H(1, 1);
double h6 = H(1, 2);
double h7 = H(2, 0);
double h8 = H(2, 1);
double h9 = H(2, 2);
// H = [h1, h2, h3,
// h4, h5, h6,
// h7, h8, h9]
// [ h1*h2, h1*h5 + h2*h4, h1*h8 + h2*h7, h4*h5, h4*h8 + h5*h7, h7*h8]
// [ h1^2 - h2^2, 2*h1*h4 - 2*h2*h5, 2*h1*h7 - 2*h2*h8, h4^2 - h5^2, 2*h4*h7
// - 2*h5*h8, h7^2 - h8^2]
a1 << h1 * h2, h1 * h5 + h2 * h4, h1 * h8 + h2 * h7, h4 * h5,
h4 * h8 + h5 * h7, h7 * h8;
a2 << h1 * h1 - h2 * h2, 2 * h1 * h4 - 2 * h2 * h5,
2 * h1 * h7 - 2 * h2 * h8, h4 * h4 - h5 * h5, 2 * h4 * h7 - 2 * h5 * h8,
h7 * h7 - h8 * h8;
int j = 2 * i;
A.row(j) = a1;
A.row(j + 1) = a2;
}
Eigen::JacobiSVD<Eigen::Matrix<double, 10, 6>> svd(
A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<double, 6, 6> V = svd.matrixV();
Eigen::Matrix<double, 6, 1> v = V.col(V.cols() - 1);
Eigen::Matrix3d KtinvKinv;
KtinvKinv << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5);
Eigen::Matrix3d pseudoKinv = KtinvKinv.llt().matrixL();
Eigen::Matrix3d K = pseudoKinv.inverse().transpose();
K = K / K(2, 2);
Eigen::Matrix3d Kinv = K.inverse();
std::vector<Eigen::Matrix3d> Rs(5);
std::vector<Eigen::Vector3d> ts(5);
std::cout << "K\n" << K << std::endl;
std::cout << "--------------------------------------" << std::endl;
for (int i = 0; i < 5; i++) {
Eigen::Vector3d r1 = Kinv * Hs[i].col(0);
Eigen::Vector3d r2 = Kinv * Hs[i].col(1);
Eigen::Vector3d r3 = r1.cross(r2);
Eigen::Matrix3d R;
R << r1, r2, r3;
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU |
Eigen::ComputeFullV);
R = svd.matrixU() * svd.matrixV().transpose();
Rs[i] = R;
double lambda = 1 / r1.norm();
Eigen::Vector3d t = lambda * Kinv * Hs[i].col(2);
ts[i] = t;
std::cout << "i: " << i << std::endl;
std::cout << "R\n" << R << std::endl;
std::cout << "t\n" << t << std::endl;
std::cout << "--------------------------------------" << std::endl;
}
return 0;
}
How to run
본 예제코드는 cmake 프로젝트로 작성하였으며 Ubuntu 18.04, cmake 3.16, opencv 4.2, eigen 3.3.7 버전에서 정상 작동하는 것을 확인하였다. 다음 순서로 프로그램을 빌드 및 실행하면 된다.
# clone the repository
$ git clone https://github.com/gyubeomim/mvg-example
$ cd mvg-example/zhang-calibration
# cmake build
$ mkdir build
$ cd build
$ cmake ..
$ make
# run example
$ ./zhang
Code #1
void GetChessboardCorners(
std::vector<Eigen::Matrix<double, 63, 2>> &pixel_corners) {
Eigen::Matrix<double, 63, 2> c1, c2, c3, c4, c5;
..
pixel_corners[0] = c1;
pixel_corners[1] = c2;
pixel_corners[2] = c3;
pixel_corners[3] = c4;
pixel_corners[4] = c5;
}본 예제 코드에서는 체크보드의 값을 별도의 detection 알고리즘을 통해 구하는 것이 아닌 matlab cameraCalibrator를 통해 구한 체크보드의 2D 픽셀 좌표를 사용한다. 7x9 크기의 체크보드에 픽셀 좌표 $[x,y]$를 사용하므로 카메라 이미지 한 장 당 63개의 픽셀 좌표값을 입력한다. 총 5개의 카메라 이미지에 대해 이를 수행한다.
Code #2
std::vector<Eigen::Matrix3d>
ComputeHomography8Point(std::vector<Eigen::Matrix<double, 63, 2>> pixel_corners,
Eigen::Matrix<double, 63, 4> world_points) {
std::vector<Eigen::Matrix3d> Hs(5);
Eigen::MatrixXd A(2 * 63, 9);
for (int i = 0; i < 5; i++) {
for (int j = 0; j < 63; j++) {
double X = world_points.row(j)(0);
double Y = world_points.row(j)(1);
double W = world_points.row(j)(3);
double u = pixel_corners[i].row(j)(0);
double v = pixel_corners[i].row(j)(1);
Eigen::Matrix<double, 9, 1> a1, a2;
a1 << X, Y, W, 0, 0, 0, -u * X, -u * Y, -u * W;
a2 << 0, 0, 0, X, Y, W, -v * X, -v * Y, -v * W;
int k = 2 * j;
A.row(k) = a1.transpose();
A.row(k + 1) = a2.transpose();
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU |
Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
Eigen::Matrix<double, 9, 1> h = V.col(V.cols() - 1);
Eigen::Matrix3d H = Eigen::Map<Eigen::Matrix3d>(h.data());
H = H / H(2, 2);
Hs[i] = H.transpose();
}
return Hs;
}matlab cameraCalibrator를 통해 얻은 2D 이미지 픽셀 좌표 $\mathbf{x}_{i}$(pixel_corners 변수)와 zhang's calibration의 특징으로 인한 체크보드의 3D 월드 상의 좌표 $\mathbf{X}_{i}$ (world_points 변수)를 사용하여 $\mathbf{H} = \mathbf{K}[\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}]$를 구할 수 있다.
$$\mathbf{x}_{i} = \mathbf{H}\mathbf{X}_{i}$$
- $i$ : the number of pixels in checkerboard
이를 자세히 표현하면 다음과 같다.
$$\begin{bmatrix} x_{i} \\ y_{i} \\ 1 \end{bmatrix} = \begin{bmatrix} h_1 & h_2 & h_3 \\ h_4 & h_5 & h_6 \\ h_7 & h_8 & h_9 \end{bmatrix} \begin{bmatrix} X_{i} \\ Y_{i} \\ 1 \end{bmatrix}$$
zhang method의 특징으로 $\mathbf{r}_{3,col}$ 변수가 사라지면서 $Z_{i}$ 값 또한 소거되는 것을 알 수 있다. 위 방정식을 direct linear transform (DLT) 형태로 변경하면 다음과 같다. $\mathbf{x}_{i} \times \mathbf{H}\mathbf{X}_{i} = 0$의 특징을 사용하여 전개하면 다음과 같다.
$$\mathbf{x}_{i} = \mathbf{H}\mathbf{X}_{i} \rightarrow \mathbf{a}_{xi}^{\intercal}\mathbf{h} = 0, \mathbf{a}_{yi}^{\intercal}\mathbf{h} = 0$$
$$\begin{bmatrix} -X_i& -Y_i& -1& 0&0&0& x_i X_i & x_i Y_i &x_i \\ 0&0&0&-X_i&-Y_i&-1&y_i X_i&y_i Y_i & y_i \end{bmatrix} \begin{bmatrix} h_1 \\ h_2 \\ \vdots \\ h_9 \end{bmatrix} = 0$$
- $\mathbf{a}_{xi}^{\intercal} = \begin{bmatrix} -X_i& -Y_i& -1& 0&0&0& x_i X_i & x_i Y_i &x_i \end{bmatrix}$
- $\mathbf{a}_{yi}^{\intercal} = \begin{bmatrix} 0&0&0&-X_i&-Y_i&-1&y_i X_i&y_i Y_i & y_i \end{bmatrix}$
- $\mathbf{h} = \begin{bmatrix} h_1 & h_2 & \dots & h_9 \end{bmatrix}^{\intercal}$
위 식은 단일 이미지에 대한 DLT 공식을 의미한다. 따라서 위 식을 5개의 이미지에 대해 설정한 후 해당 선형시스템의 null space를 찾으면 이는 곧 $\mathbf{h}$의 해가 된다. 이를 통해 $\mathbf{H}$를 구할 수 있다.
Code #3
int main() {
std::vector<cv::Mat> images;
for (int i = 1; i <= 5; i++) {
std::string fn_img = util::GetProjectPath() +
"/picture/zhang_calibration/" + std::to_string(i) +
".jpg";
cv::Mat img = cv::imread(fn_img, cv::IMREAD_GRAYSCALE);
images.push_back(img);
}
std::vector<Eigen::Matrix<double, 63, 2>> pixel_corners(5);
GetChessboardCorners(pixel_corners);
Eigen::Matrix<double, 63, 4> world_points;
int gap = 20; // [mm]
for (int r = 0; r < 9; r++) {
for (int c = 0; c < 7; c++) {
world_points.row(7 * r + c) =
Eigen::Vector4d(r * gap, c * gap, 0, 1).transpose();
}
}
std::vector<Eigen::Matrix3d> Hs;
Hs = ComputeHomography8Point(pixel_corners, world_points);앞서 촬영한 이미지 5장을 불러온다. 이 때 zhang's method 특징에 따라 월드 상의 체크보드 좌표는 간단하게 구할 수 있다(world_points 변수). 다음으로 앞서 Code #1, #2 섹션에서 설명한 코드를 수행하여 $\mathbf{H}$ 값을 구한다(Hs 변수).
Code #4
Eigen::Matrix<double, 10, 6> A;
for (int i = 0; i < 5; i++) {
Eigen::Matrix<double, 3, 3> H = Hs[i];
Eigen::Matrix<double, 6, 1> a1, a2;
double h1 = H(0, 0);
double h2 = H(0, 1);
double h3 = H(0, 2);
double h4 = H(1, 0);
double h5 = H(1, 1);
double h6 = H(1, 2);
double h7 = H(2, 0);
double h8 = H(2, 1);
double h9 = H(2, 2);
// H = [h1, h2, h3,
// h4, h5, h6,
// h7, h8, h9]
// [ h1*h2, h1*h5 + h2*h4, h1*h8 + h2*h7, h4*h5, h4*h8 + h5*h7, h7*h8]
// [ h1^2 - h2^2, 2*h1*h4 - 2*h2*h5, 2*h1*h7 - 2*h2*h8, h4^2 - h5^2, 2*h4*h7
// - 2*h5*h8, h7^2 - h8^2]
a1 << h1 * h2, h1 * h5 + h2 * h4, h1 * h8 + h2 * h7, h4 * h5,
h4 * h8 + h5 * h7, h7 * h8;
a2 << h1 * h1 - h2 * h2, 2 * h1 * h4 - 2 * h2 * h5,
2 * h1 * h7 - 2 * h2 * h8, h4 * h4 - h5 * h5, 2 * h4 * h7 - 2 * h5 * h8,
h7 * h7 - h8 * h8;
int j = 2 * i;
A.row(j) = a1;
A.row(j + 1) = a2;
}
Eigen::JacobiSVD<Eigen::Matrix<double, 10, 6>> svd(
A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix<double, 6, 6> V = svd.matrixV();
Eigen::Matrix<double, 6, 1> v = V.col(V.cols() - 1);
Eigen::Matrix3d KtinvKinv;
KtinvKinv << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5);
Eigen::Matrix3d pseudoKinv = KtinvKinv.llt().matrixL();
Eigen::Matrix3d K = pseudoKinv.inverse().transpose();
K = K / K(2, 2);
Eigen::Matrix3d Kinv = K.inverse();다음으로 $\mathbf{K}$를 구해야한다. 앞서 설명한 두 공식을 다시 살펴보면
\begin{equation}
\begin{aligned}
\mathbf{h}_{1,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{2,col} = 0
\end{aligned}
\end{equation}
\begin{equation}
\begin{aligned}
& \mathbf{h}_{1,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{1,col} = \mathbf{h}_{2,col}^{\intercal}\mathbf{K}^{-\intercal}\mathbf{K}^{-1}\mathbf{h}_{2,col}
\end{aligned}
\end{equation}
과 같고 이를 DLT 형태로 다시 표현하면 다음과 같다. 이 때, $\mathbf{K}^{-\intercal}\mathbf{K}^{-1} = \mathbf{A}$로 치환하여 표현하면
$$\mathbf{h}^{\intercal}_{1,col} \mathbf{A} \mathbf{h}_{2,col} = 0 \rightarrow \mathbf{v}^{\intercal}_{12} \mathbf{a} = 0$$
$$\mathbf{h}^{\intercal}_{1,col} \mathbf{A} \mathbf{h}_{1,col} = \mathbf{h}^{\intercal}_{2,col} \mathbf{A} \mathbf{h}_{2,col} \rightarrow \mathbf{v}_{11}^{\intercal} \mathbf{a} = \mathbf{v}_{22}^{\intercal} \mathbf{a}$$
$$\therefore \begin{bmatrix} \mathbf{v}_{12}^{\intercal} \\ \mathbf{v}_{11}^{\intercal} - \mathbf{v}_{22}^{\intercal} \end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_9 \end{bmatrix} = 0$$
- $\mathbf{a} = \begin{bmatrix} a_1 & a_2 & \dots & a_9 \end{bmatrix}^{\intercal}$
이를 자세히 풀어쓰면 다음과 같다.
$$\begin{bmatrix} h_1 h_2 & h_1 h_5 + h_2 h_4 & h_1 h_8 + h_2 h_7 & h_4 h_5 & h_4 h_8 + h_5 h_7 & h_7 h_8 \\ h_1^2 - h_2^2 & 2h_1 h_4 - 2h_2 h_5 & 2h_1 h_7 - 2h_2 h_8 & h_4^2 - h_5^2 & 2h_4 h_7 - 2h_5 h_8 & h_7^2 - h_8^2\end{bmatrix} \begin{bmatrix} a_1 \\ a_2 \\ \vdots \\ a_9 \end{bmatrix} = 0$$
위 식은 단일 이미지에 대한 DLT 공식을 의미한다. 따라서 위 식을 5개의 이미지에 대해 설정한 후 해당 선형시스템의 null space를 찾으면 이는 곧 $\mathbf{A} = \mathbf{K}^{-\intercal}\mathbf{K}^{-1}$의 해가 된다. $\mathbf{K}^{-\intercal}\mathbf{K}^{-1}$ 값을 구했으면 cholesky decomposition 알고리즘을 사용하여 $\mathbf{K}$를 구할 수 있다.
Code #5
std::vector<Eigen::Matrix3d> Rs(5);
std::vector<Eigen::Vector3d> ts(5);
std::cout << "K\n" << K << std::endl;
std::cout << "--------------------------------------" << std::endl;
for (int i = 0; i < 5; i++) {
Eigen::Vector3d r1 = Kinv * Hs[i].col(0);
Eigen::Vector3d r2 = Kinv * Hs[i].col(1);
Eigen::Vector3d r3 = r1.cross(r2);
Eigen::Matrix3d R;
R << r1, r2, r3;
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU |
Eigen::ComputeFullV);
R = svd.matrixU() * svd.matrixV().transpose();
Rs[i] = R;
double lambda = 1 / r1.norm();
Eigen::Vector3d t = lambda * Kinv * Hs[i].col(2);
ts[i] = t;
std::cout << "i: " << i << std::endl;
std::cout << "R\n" << R << std::endl;
std::cout << "t\n" << t << std::endl;
std::cout << "--------------------------------------" << std::endl;
}
return 0;
}다음으로 $\mathbf{R}, \mathbf{t}$를 구한다. 앞서 설명한 공식
$$\mathbf{K}^{-1}\mathbf{H} = [\mathbf{r}_{1,col} \ \mathbf{r}_{2,col} \ \mathbf{t}]$$
을 사용하여 $\mathbf{r}_{1,col} \ \mathbf{r}_{2,col}$를 구하고 이를 통해 $\mathbf{r}_{3,col}$을 구한다.
\begin{equation}
\begin{aligned}
\mathbf{r}_{3,col} = \mathbf{r}_{1} \times \mathbf{r}_{2}
\end{aligned}
\end{equation}
결론적으로 zhang's calibration을 사용하여 구한 $\mathbf{K}_{zh}, \mathbf{R}_{i, zh}, \mathbf{t}_{i,zh}$과 matlab cameraCalibrator를 통해 구한 ground truth를 비교하면 다음과 같다.
$$\mathbf{K}_{zh} = \begin{bmatrix} 3362.36 & 11.9486 & 1921.86 \\ 0 & 3363.79 & 984.521 \\ 0 & 0 & 1 \end{bmatrix}$$
$$\mathbf{K}_{mat} = \begin{bmatrix} 3331.893 & 0 & 1939.063 \\ 0 & 3329.977 & 1000.860 \\ 0 & 0 & 1 \end{bmatrix}$$
$$\mathbf{R}_{1,zh} = \begin{bmatrix} 0.998 & -0.0610 & 0.0084 \\
0.0457 & 0.826 & 0.561\\
-0.041 & -0.559 & 0.827 \end{bmatrix}, \mathbf{t}_{1,zh} = \begin{bmatrix} -68.89 \\
-57.46\\
428.339 \end{bmatrix}$$
$$\mathbf{R}_{1,mat} = \begin{bmatrix} 0.998 & -0.055 & 0.001 \\ 0.045 & 0.827 & 0.560 \\ -0.032 & -0.559 & 0.828 \end{bmatrix}, \mathbf{t}_{1,mat} = \begin{bmatrix} -71.23 \\ -59.702 \\ 427.161 \end{bmatrix}$$
$$\mathbf{R}_{2,zh} = \begin{bmatrix} 0.995& 0.035 &-0.082\\
-0.040 & 0.997 & -0.0598\\
0.0801 & 0.0629 & 0.994 \end{bmatrix}, \mathbf{t}_{2,zh} = \begin{bmatrix} -27.05\\
-7.56\\
423.143 \end{bmatrix}$$
$$\mathbf{R}_{2,mat} = \begin{bmatrix} 0.995 & 0.036 & -0.089 \\ -0.043 & 0.996 & -0.068 \\ 0.086 & 0.072 & 0.993 \end{bmatrix}, \mathbf{t}_{2,mat} = \begin{bmatrix} -29.158 \\ -9.54 \\ 422.498 \end{bmatrix}$$
$$\mathbf{R}_{3,zh} = \begin{bmatrix} 0.992& -0.0910& 0.0813\\
0.00999 & 0.724 & 0.689\\
-0.121 & -0.686 & 0.720 \end{bmatrix}, \mathbf{t}_{3,zh} = \begin{bmatrix} -47.08\\
-35.62\\
425.825 \end{bmatrix}$$
$$\mathbf{R}_{3,mat} = \begin{bmatrix} 0.994 & -0.081 & 0.067 \\ 0.0137 & 0.732 & 0.681 \\ -0.104 & -0.676 & 0.729 \end{bmatrix}, \mathbf{t}_{3,mat} = \begin{bmatrix} -49.457 \\ -37.856 \\ 423.639 \end{bmatrix}$$
$$\mathbf{R}_{4,zh} = \begin{bmatrix} 0.986 & -0.0151 & 0.162\\
0.0231 & 0.998 & -0.0479\\
-0.161 & 0.0510 & 0.985 \end{bmatrix}, \mathbf{t}_{4,zh} = \begin{bmatrix} -92.03\\
-38.93\\
383.93 \end{bmatrix}$$
$$\mathbf{R}_{4,mat} = \begin{bmatrix} 0.985 & -0.012 & 0.171 \\ 0.022 & 0.998 & -0.058 \\ -0.17 & 0.061 & 0.983 \end{bmatrix}, \mathbf{t}_{4,mat} = \begin{bmatrix} -93.934 \\ -40.686 \\ 382.775 \end{bmatrix}$$
$$\mathbf{R}_{5,zh} = \begin{bmatrix} 0.985 & -0.0585 & 0.160\\
-0.00921 & 0.919 & 0.392\\
-0.170 & -0.387 & 0.905 \end{bmatrix}, \mathbf{t}_{5,zh} = \begin{bmatrix} -64.11\\
6.19\\
664.02 \end{bmatrix}$$
$$\mathbf{R}_{5,mat} = \begin{bmatrix} 0.986 & -0.051 & 0.155 \\ -0.005 & 0.937 & 0.347 \\ -0.163 & -0.343 & 0.924 \end{bmatrix}, \mathbf{t}_{5,mat} = \begin{bmatrix} -67.523 \\ 2.961 \\ 658.088 \end{bmatrix}$$
References
'Engineering' 카테고리의 다른 글
| [MVG] Scene Plane-based Homography 예제코드 및 설명 (C++) (0) | 2022.07.07 |
|---|---|
| [MVG] Vanishing Point-based Metric Rectification 예제코드 및 설명 (C++) (0) | 2022.07.03 |
| [MVG] Affine & Metric Rectification 예제코드 및 설명 (C++) (0) | 2022.06.22 |
| [SLAM] g2o - Alternative Parameterizations 논문 섹션 리뷰 (0) | 2022.06.09 |
| [SLAM] g2o example 코드 리뷰 (0) | 2022.06.09 |
