[MVG] Vanishing Point-based Metric Rectification 예제코드 및 설명 (C++)

해당 내용은 공부 목적으로 작성된 글입니다.

잘못된 부분이나 수정할 부분이 있다면 댓글이나 메일로 알려주시면 확인 후 수정하도록 하겠습니다.

 

 

본 포스트는 MVG part1 에서 설명한 "Vanishing points and vanishing lines" 부분을 활용하여 metric rectification을 수행한 해당 github을 C++ 코드로 구현한 내용을 리뷰한다. 예제 코드는 우분투 18.04 환경에서 테스트하였으며 cmake 3.16, opencv 4.2, eigen 3.3.7 버전을 사용하여 정상적으로 작동하는 것을 확인하였다. 예제 코드는 링크를 클릭하여 다운로드하면 된다.

 

Vanishing Point-based Metric Rectification

Input Image Setup

실제 월드 상에서 직사각형인 물체를 촬영한다. 그리고 직사각형 물체의 네 꼭지점 픽셀 좌표를 알고있다고 가정한다. 위 예제 사진에서는 TV를 직사각형 물체로써  촬영하였으며 카메라의 lens distortion은 없다고 가정하였다. 네 꼭지점의 좌표는 다음과 같다.

 

Affine Rectification

직사각형 물체의 네 꼭지점 좌표를 알고 있다고 가정하면 이를 통해 소실점(vanishing point) $\mathbf{v}_{1},\mathbf{v}_{2}$과 소실선(vanishing line) $\mathbf{l}$을 구한다. 앞서 포스팅에서 설명한 내용과 동일하게 소실선 $\mathbf{l}$은 무한대 직선의 이미지 (image of line at infinity)이므로 이를 다시 무한대 직선 $\mathbf{l}_{\infty}$으로 보내주는 affine rectification 행렬 $\mathbf{H}_{ar}$을 구해야 한다. $\mathbf{H}_{ar}$을 구하는 자세한 내용은 위 포스팅에 설명되어 있으므로 본 포스트에서는 생략한다.

 

Affine Rectification in Projective Space

affine rectification 과정을 $\mathbb{P}^{2}$ 공간에서 보면 다음과 같다.

실제 월드 상의 물체가 특정한 평면 $\pi$ 위에 있다고 했을 때, 이를 일반적으로 scene plane $\pi$이라고 부른다. 소실선 $\mathbf{l}$은 원점을 지나면서 $\pi$와 평행한 평면 $\pi'$가 이미지 평면  $\pi_{\mathbf{c}}$와 접하는 직선을 의미한다. 이러한 $\mathbf{l}$을 다시 $\mathbf{l}_{\infty}$로 보내기 위해서는 카메라의 주축(principal axis) $Z$가 $\pi$의 법선벡터와 평행해야 한다. 따라서 affine rectification 과정을 $\mathbb{P}^{2}$ 공간에서 보면 카메라의 주축 $Z$를 scene plane $\pi$의 법선벡터와 평행하게 만드는 작업이 된다.

 

Metric Rectification using Vanishing Points

affine rectification까지 진행된 경우 카메라의 주축 $Z$은 scene plane 평면 $\pi$의 법선벡터와 평행하게 된다. 다음으로 affine rectification이 적용된 소실점 $\mathbf{v}'_{1}, \mathbf{v}'_{2}$와 카메라의 $X,Y$축을 평행하도록 하는 rotation matrix $\mathbf{R}^{-1}$를 구함으로써 metric rectification을 수행할 수 있다. 이 때, $\mathbf{v}'_{1} = [v_{1x}, v_{1y}, 0]^{\intercal}, \mathbf{v}'_{2} = [v_{2x}, v_{2y}, 0]^{\intercal}$을 의미하므로 이는 평행인 직선들의 방향벡터가 된다.

- $\mathbf{v}_{i} = [v_{ix}, v_{iy}, v_{iz}]^{\intercal}$ : original vanishing point

- $\mathbf{v}'_{i} = [v_{ix}, v_{iy}, 0]^{\intercal}$ : affine rectified vanishing point (direction of parallel line)

 

$\mathbf{v}'_{1}, \mathbf{v}'_{2}$ 두 방향벡터 중 $Y$축과 가장 가까운 방향벡터 $i$를 먼저 구한 후 이와 수직한 나머지 방향벡터 $j$를 구해서 rotation matrix $\mathbf{R}$을 구한다.  다음으로 이를 다시 역으로 곱해주면 최종적인 metric rectification이 수행된다.

$$\mathbf{R} = \begin{bmatrix} v_{jx} & v_{ix} & 0 \\ v_{jy} & v_{iy} & 0 \\ 0 & 0 & 1  \end{bmatrix}$$

 

이 때, 이미지 평면 $\pi_{\mathbf{C}}$ 상에 있는 $\mathbf{v}'_{1}, \mathbf{v}'_{2}$는 affine property를 지니므로 서로 직교하지 않는다. rotation matrix $\mathbf{R}$ 상의 첫 번째 열과 두 번째 열의 내적은 $\mathbf{r}_{1,col}^{\intercal} \cdot \mathbf{r}_{2,col} \neq 0$이 되어 rotation matrix의 성질을 만족하지 않는다. 따라서 엄밀히 말하면 $\mathbf{R}$은 rotation matrix가 아닌 homography 행렬이지만 편의를 위해 본 포스팅에서는 rotation matrix라고 명명한다. 

 

metric rectification 까지 완료된 이미지는 실제 월드상의 물체가 스케일 크기만 제외하고(up to scale) 동일한 이미지가 된다. metric rectificatino에 대한 자세한 아이디어는 해당 논문을 참고하였다. 최종적으로 metric rectification된 이미지는 다음과 같다.

 

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 ApplyHomography(cv::Mat &img, cv::Mat &img_out, Eigen::Matrix3d H) {
  cv::Mat _H;
  cv::eigen2cv(H, _H);

  cv::warpPerspective(img, img_out, _H,
                      cv::Size(img.size().width * 4, img.size().height * 2));
}

int main() {
  std::string fn_img = util::GetProjectPath() +
                       "/picture/test.png";
  cv::Mat img = cv::imread(fn_img, cv::IMREAD_COLOR);

  // four points.
  Eigen::Vector3d p1 = Eigen::Vector3d(620, 30, 1);
  Eigen::Vector3d p2 = Eigen::Vector3d(2144, 795, 1);
  Eigen::Vector3d p3 = Eigen::Vector3d(520, 1964, 1);
  Eigen::Vector3d p4 = Eigen::Vector3d(2248, 1834, 1);

  // four lines.
  Eigen::Matrix<double, 4, 3> lines;
  lines.row(0) = p1.cross(p2);
  lines.row(1) = p3.cross(p4);
  lines.row(2) = p1.cross(p3);
  lines.row(3) = p2.cross(p4);

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd1(
      lines.block<2, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V = svd1.matrixV();

  // vanishing point b/w line #1 and #2.
  Eigen::Vector3d vp1 = V.col(V.cols() - 1);
  vp1 = vp1 / vp1(2);

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd2(
      lines.block<2, 3>(2, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V2 = svd2.matrixV();

  // vanishing point b/w line #3 and #4.
  Eigen::Vector3d vp2 = V2.col(V2.cols() - 1);
  vp2 = vp2 / vp2(2);

  Eigen::Matrix<double, 2, 3> vpoints;
  vpoints << vp1.transpose(), vp2.transpose();

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd3(
      vpoints, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V3 = svd3.matrixV();

  // vanishnig line of vp1 and vp2.
  Eigen::Vector3d vline = V3.col(V3.cols() - 1);
  vline = vline / vline(2);

  // get affine homography.
  Eigen::Matrix3d H_aff;
  H_aff << 1, 0, 0, 0, 1, 0, vline.transpose();

  Eigen::Vector3d Hv1 = H_aff * vp1;
  Hv1 = Hv1 / Hv1.norm();

  Eigen::Vector3d Hv2 = H_aff * vp2;
  Hv2 = Hv2 / Hv2.norm();

  // find directions corresponding to vanishing points.
  Eigen::Matrix<double, 2, 4> dir;
  dir << Hv1(0), -Hv1(0), Hv2(0), -Hv2(0), Hv1(1), -Hv1(1), Hv2(1), -Hv2(1);

  std::vector<double> thetas;
  thetas.push_back(std::abs(std::atan2(dir(0, 0), dir(1, 0))));
  thetas.push_back(std::abs(std::atan2(dir(0, 1), dir(1, 1))));
  thetas.push_back(std::abs(std::atan2(dir(0, 2), dir(1, 2))));
  thetas.push_back(std::abs(std::atan2(dir(0, 3), dir(1, 3))));
  std::vector<double> thetas2;
  thetas2.push_back(std::atan2(dir(0, 2), dir(1, 2)));
  thetas2.push_back(std::atan2(dir(0, 3), dir(1, 3)));

  // find direction closest to vertical axis.
  int hidx = std::min_element(thetas.begin(), thetas.end()) - thetas.begin();
  int vidx = std::max_element(thetas2.begin(), thetas2.end()) - thetas2.begin();

  if(hidx <=2)
	  vidx += 2;

  Eigen::Matrix3d H_metric;
  H_metric << dir(0, vidx), dir(0, hidx), 0, dir(1, vidx), dir(1, hidx), 0, 0,
      0, 1;

  if (H_metric.determinant() < 0) {
    H_metric.row(0) *= -1;
  }
	

  // affine rectification.
  cv::Mat img_aff;
  ApplyHomography(img, img_aff, H_aff);
  cv::resize(img_aff, img_aff, cv::Size(), 0.10, 0.10);
  cv::imshow("affine", img_aff);
  cv::moveWindow("affine", 0, 0);

  // metric rectification.
  cv::Mat img_metric;
  ApplyHomography(img, img_metric, H_metric.inverse() * H_aff);
  cv::resize(img_metric, img_metric, cv::Size(), 0.10, 0.10);
  cv::imshow("metric", img_metric);
  cv::moveWindow("metric", 0, 500);
  cv::waitKey(0);

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

# cmake build
$ mkdir build
$ cd build
$ cmake ..
$ make

# run example
$ ./vanish

 

Code #1

#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 ApplyHomography(cv::Mat &img, cv::Mat &img_out, Eigen::Matrix3d H) {
  cv::Mat _H;
  cv::eigen2cv(H, _H);

  cv::warpPerspective(img, img_out, _H,
                      cv::Size(img.size().width * 4, img.size().height * 2));
}

이미지에 homography를 적용하는 함수이다. eigen 라이브러리를 통해 구한 homography $\mathbf{H}$를 opencv 타입으로 변경한 다음 이미지에 적용한다.

 

Code #2

int main() {
  std::string fn_img = util::GetProjectPath() +
                       "/picture/test.png";
  cv::Mat img = cv::imread(fn_img, cv::IMREAD_COLOR);

  // four points.
  Eigen::Vector3d p1 = Eigen::Vector3d(620, 30, 1);
  Eigen::Vector3d p2 = Eigen::Vector3d(2144, 795, 1);
  Eigen::Vector3d p3 = Eigen::Vector3d(520, 1964, 1);
  Eigen::Vector3d p4 = Eigen::Vector3d(2248, 1834, 1);

  // four lines.
  Eigen::Matrix<double, 4, 3> lines;
  lines.row(0) = p1.cross(p2);
  lines.row(1) = p3.cross(p4);
  lines.row(2) = p1.cross(p3);
  lines.row(3) = p2.cross(p4);

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd1(
      lines.block<2, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V = svd1.matrixV();

  // vanishing point b/w line #1 and #2.
  Eigen::Vector3d vp1 = V.col(V.cols() - 1);
  vp1 = vp1 / vp1(2);

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd2(
      lines.block<2, 3>(2, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V2 = svd2.matrixV();

  // vanishing point b/w line #3 and #4.
  Eigen::Vector3d vp2 = V2.col(V2.cols() - 1);
  vp2 = vp2 / vp2(2);

  Eigen::Matrix<double, 2, 3> vpoints;
  vpoints << vp1.transpose(), vp2.transpose();

  Eigen::JacobiSVD<Eigen::Matrix<double, 2, 3>> svd3(
      vpoints, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d V3 = svd3.matrixV();

  // vanishnig line of vp1 and vp2.
  Eigen::Vector3d vline = V3.col(V3.cols() - 1);
  vline = vline / vline(2);

  // get affine homography.
  Eigen::Matrix3d H_aff;
  H_aff << 1, 0, 0, 0, 1, 0, vline.transpose();

이미 알고있는 네 개의 직사각형 꼭지점 픽셀 좌표를 통해 vanishing point $\mathbf{v}_{1}$, $\mathbf{v}_{2}$를 구한다(vp1, vp2 변수). 그리고 이를 통해 vanishing line $\mathbf{l}$을 구한다(vline 변수). 최종적으로 vanishing line $\mathbf{l}$을 무한대 직선 $\mathbf{l}_{\infty}$로 보내는 affine rectification homography $\mathbf{H}_{ar}$을 계산한다. 

$$\mathbf{H}_{ar} = \begin{bmatrix} 1&0&0 \\ 0&1&0 \\ a & b & c \end{bmatrix}$$

- $\mathbf{l} = [a,b,c]^{\intercal}$ : vanishing line (=image of line at infinity)

- $\mathbf{l}_{\infty} = [0, 0, 1]^{\intercal}$ : line at infinity

 

Code #3

 Eigen::Vector3d Hv1 = H_aff * vp1;
  Hv1 = Hv1 / Hv1.norm();

  Eigen::Vector3d Hv2 = H_aff * vp2;
  Hv2 = Hv2 / Hv2.norm();

  // find directions corresponding to vanishing points.
  Eigen::Matrix<double, 2, 4> dir;
  dir << Hv1(0), -Hv1(0), Hv2(0), -Hv2(0), Hv1(1), -Hv1(1), Hv2(1), -Hv2(1);

  std::vector<double> thetas;
  thetas.push_back(std::abs(std::atan2(dir(0, 0), dir(1, 0))));
  thetas.push_back(std::abs(std::atan2(dir(0, 1), dir(1, 1))));
  thetas.push_back(std::abs(std::atan2(dir(0, 2), dir(1, 2))));
  thetas.push_back(std::abs(std::atan2(dir(0, 3), dir(1, 3))));
  std::vector<double> thetas2;
  thetas2.push_back(std::atan2(dir(0, 2), dir(1, 2)));
  thetas2.push_back(std::atan2(dir(0, 3), dir(1, 3)));

  // find direction closest to vertical axis.
  int hidx = std::min_element(thetas.begin(), thetas.end()) - thetas.begin();
  int vidx = std::max_element(thetas2.begin(), thetas2.end()) - thetas2.begin();

  if(hidx <=2)
	  vidx += 2;

  Eigen::Matrix3d H_metric;
  H_metric << dir(0, vidx), dir(0, hidx), 0, dir(1, vidx), dir(1, hidx), 0, 0,
      0, 1;

  if (H_metric.determinant() < 0) {
    H_metric.row(0) *= -1;
  }
	

  // affine rectification.
  cv::Mat img_aff;
  ApplyHomography(img, img_aff, H_aff);
  cv::resize(img_aff, img_aff, cv::Size(), 0.10, 0.10);
  cv::imshow("affine", img_aff);
  cv::moveWindow("affine", 0, 0);

  // metric rectification.
  cv::Mat img_metric;
  ApplyHomography(img, img_metric, H_metric.inverse() * H_aff);
  cv::resize(img_metric, img_metric, cv::Size(), 0.10, 0.10);
  cv::imshow("metric", img_metric);
  cv::moveWindow("metric", 0, 500);
  cv::waitKey(0);

  return 0;
}

다음으로 이미지에서 affine rectified vanishing point $\mathbf{v}'_{1}, \mathbf{v}'_{2}$를 구한다(Hv1, Hv2 변수). 이 때, $\mathbf{v}'_{i} = [v_{ix}, v_{iy}, 0]^{\intercal}$은 맨 마지막 값이 0이 되어 실제 이미지 평면 상에는 존재하지 않고 방향만을 의미하는 벡터가 된다.

 

다음으로 이미지 평면의 $X, Y$ 축으로부터 $\mathbf{v}'_{1}, \mathbf{v}'_{2}$의 각도를 구한다. $\pm 180\deg$를 고려하여 다음과 같이 $\Theta$를 생성한다.

$$\Theta = [\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}] = \begin{bmatrix} \tan^{-1}{\frac{v_{1x}}{v_{1y}}}, &  | \tan^{-1}{\frac{-v_{1x}}{-v_{1y}}} |, &  \tan^{-1}{\frac{v_{2x}}{v_{2y}}}, &  | \tan^{-1}{\frac{-v_{2x}}{v_{-2y}} } | \end{bmatrix}$$

 

그리고 4개의 각도 값들 중 Y축과 가장 근접한 각도 $\theta_{j}$를 선택한다. 다음으로 X축과 가장 근접한 각도 $\theta_{i}$를 구한다. 최종적으로 $\theta_{j}, \theta_{i}$에 상응하는 $\mathbf{v}'_{1}, \mathbf{v}'_{2}$ 값이 rotation matrix $\mathbf{R}$의 성분이 된다.

$$\mathbf{R} = \begin{bmatrix} v_{jx} & v_{ix} & 0 \\ v_{jy} & v_{iy} & 0 \\ 0 & 0 & 1  \end{bmatrix}$$

 

다음으로 affine rectification이 수행된 이미지에 $\mathbf{R}^{-1}$을 적용하면 최종적인 metric rectification이 수행된다.