해당 내용은 공부 목적으로 작성된 글입니다.
잘못된 부분이나 수정할 부분이 있다면 댓글이나 메일로 알려주시면 확인 후 수정하도록 하겠습니다.
본 포스트는 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 툴에 넣어주면 자동으로 체크보드의 좌표를 인식하여 위와 같이 카메라 행렬
Zhang's Calibration

체커보드를 통해 카메라 행렬
이에 따라 체커보드 평면
체커보드 상의 한 점
가 된다.
체커보드는 패턴의 길이와 갯수를 모두 알고 있기 때문에 자동적으로 체커보드 평면
가 된다. 이를 정리하면
가 된다. 이 때, 직교행렬
이 성립한다. 또한 직교행렬 조건에 의해 스케일을 제외하고(up to scale)
공식이 성립한다. 한 개의 체커보드 사진으로 부터 위와 같은 두 개의 방정식을 얻을 수 있다. 일반적인 카메라 캘리브레이션 행렬
과 같이 5개의 변수를 가지고 있으므로
- 5개의 파라미터를 갖고 있는 행렬
-
이를 통해
-
를 통해 구한다. 결론적으로 각각의 Homography
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 크기의 체크보드에 픽셀 좌표
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 이미지 픽셀 좌표
-
이를 자세히 표현하면 다음과 같다.
zhang method의 특징으로
-
-
-
위 식은 단일 이미지에 대한 DLT 공식을 의미한다. 따라서 위 식을 5개의 이미지에 대해 설정한 후 해당 선형시스템의 null space를 찾으면 이는 곧
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 섹션에서 설명한 코드를 수행하여
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();
다음으로
과 같고 이를 DLT 형태로 다시 표현하면 다음과 같다. 이 때,
-
이를 자세히 풀어쓰면 다음과 같다.
위 식은 단일 이미지에 대한 DLT 공식을 의미한다. 따라서 위 식을 5개의 이미지에 대해 설정한 후 해당 선형시스템의 null space를 찾으면 이는 곧
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;
}
다음으로
을 사용하여
결론적으로 zhang's calibration을 사용하여 구한
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 |