2D-3Dの対応から射影行列を求める方法（非線形最小化）

射影行列を求める方法として、以前DLTアルゴリズムを紹介した（2D-3Dの対応から射影行列を求める方法（DLTアルゴリズム））。この方法は、線形演算で高速に射影行列を求めることができるという特長を持つが、ノイズに弱いという欠点がある。

より高精度な推定手法として、再投影誤差を最小化することにより射影行列を求める方法がある。再投影誤差とは、三次元点\(X_{i}\)を射影行列\(P\)を用いて投影した点\(\hat{x_{i}}\)と観測座標\(x_{i}\)との差\(d\left(x_{i}, \hat{x_{i}}\right)\)である。再投影誤差最小化による手法では、この差を最小とする射影行列を求める方法である。
具体的には、以下の誤差関数を最小化する。
\[E=\sum_{i}d\left(x_{i}, \hat{x_{i}}\right)\]

ただし、再投影誤差の最小化は非線形な問題であるため、Levenberg-Marquardt法などの非線形最小化アルゴリズムを利用する必要がある。ここでは、以前紹介した非線形最小化ライブラリCeres-Solverを用いた実装方法について紹介する。（非線形最小化ライブラリCeres Solverの導入方法（Windows+Visual Studio 2012））

以下のプログラムでは、非線形最小化の前にDLTアルゴリズムなどによって初期解が得られていることを前提としている。

まず、Ceres-Solverを利用するために以下のヘッダファイルの指定とnamespaceの利用を宣言しておく。

#include <ceres/ceres.h>
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
次に、誤差関数を以下のように定義する。

struct ErrorFunction{
  ErrorFunction(cv::Point2d ip, cv::Point3d wp){
    p2d[0] = ip.x; p2d[1] = ip.y;
    p3d[0] = wp.x; p3d[1] = wp.y; p3d[2] = wp.z; p3d[3] = 1.0;
  }
  template<typename T>
  bool operator () (const T* const p, T* residual) const {
    T projected[3];
    //射影行列を用いて投影
    projected[0] = T(p3d[0])*p[0] + T(p3d[1])*p[1] + T(p3d[2])*p[2] + T(p3d[3])*p[3];
    projected[1] = T(p3d[0])*p[4] + T(p3d[1])*p[5] + T(p3d[2])*p[6] + T(p3d[3])*p[7];
    projected[2] = T(p3d[0])*p[8] + T(p3d[1])*p[9] + T(p3d[2])*p[10] + T(p3d[3]);

    //観測座標と投影座標の差
    residual[0] = projected[0]/projected[2] - p2d[0];
    residual[1] = projected[1]/projected[2] - p2d[1];
    return true;
  }
  double p2d[2];
  double p3d[4];
};

あとは、以下のように三次元点\(X_{i}\)、観測座標\(x_{i}\)、射影行列\(P\)の初期値を与えて最適化を行えば良い。ただし、以下では、射影行列の右下の要素は1.0とし行列の各要素はdouble型の大きさ11の配列p、三次元座標および観測座標はstd::vector<cv::Point3d> objectPoints、std::vector<cv::Point2d> imagePointsとして定義されている。

//データの準備
Problem problem;
for (int i = 0; i < imagePoints.size(); ++i)
{
  CostFunction* cost = new AutoDiffCostFunction<ErrorFunction, 2, 11>(new ErrorFunction(imagePoints[i], objectPoints[i]));
    problem.AddResidualBlock(cost, NULL, p);
}
//最適化のためのオプションを設定
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = false;
//最適化過程の情報保持用
Solver::Summary summary;
//最適化開始
Solve(options, &problem, &summary);

誤差の変化などを出力したい場合は、以下のようにすることで出力することができる。

std::cout << "estimated(non-linear):\n" << projectionMat_nonLinear << std::endl;

以下は、観測座標に誤差がある場合のDLTおよび非線形最小化によって求められた射影行列の結果である。再投影誤差を最小化することで、真の射影行列に近づいていることが分かる。

ground truth:
[-60.56854572847249, -10.94916539850204, 10.94916539850204, 505.596540320529;
 10.94916539850204, 0.973994752146368, 61.54254048061885, 365.7275502416688;
 -0.0364972179950068, 0.2051418016020629, 0.003246649173821227, 1]
estimated(DLT):
[-35.37801230210039, -6.463851068988898, 6.288108953325002, 484.8046341606145;
 6.415055433913255, 0.5339696197970277, 35.91799408714293, 323.5692299990885;
 -0.02129335308044553, 0.1193149353323555, 0.001666295240857519, 1]
estimated(non-linear):
[-59.00519703938936, -10.38639338286536, 10.5615105748027, 500.0719917075592;
 10.62519600199016, 1.115018470237216, 59.84773511676847, 373.3478180089721;
 -0.03560793614882357, 0.1996250318214967, 0.002945705927064938, 1]

コメント

コメントを投稿

このブログの人気の投稿

COLMAPでキャリブレーション済みのデータを使う

イメージ
COLMAPで三次元復元を行う際に既知のカメラパラメータを利用する方法についてメモしておく。多くの場合、SfMでカメラパラメータも含めて推定することで十分だが、例えばCMUの Panoptic dataset のようにカメラキャリブレーション結果が提供されているようなものに対して、COLMAPを適用してみたい場合は提供されているカメラパラメータを利用する方が良い場合がある。実際に、Panoptic datasetでCOLMAPを実行するとマッチングが上手くいかず推定に失敗する場合が多くある。 既知のカメラパラメータを利用する方法については、COLMAPのFAQの中で回答があるので、ここにある通りにcamera.txt, images.txt, point3D.txtを準備すると良い。 Reconstruct sparse/dense model from known camera poses https://colmap.github.io/faq.html#reconstruct-sparse-dense-model-from-known-camera-poses 以下はCOLAMPで画像群を読み込み、特徴点の対応関係の取得まで終わっている（マッチング結果のデータベースhogehoge.dbがある）ことを前提としている。 camera.txtの生成 camera.txtには各カメラのカメラモデルおよびカメラ内部パラメータを記載する。フォーマットは以下の通り。 カメラID カメラモデル 解像度x 解像度y fx fy cx cy 歪パラメータ 内部パラメータの表現については利用するカメラモデルに依存するので、以下のURLを参照しながら必要に応じて使い分ける。 Camera Models https://colmap.github.io/cameras.html camera_models.h https://colmap.github.io/cameras.html image.txtの生成 image.txtには各画像に対応するカメラ外部パラメータを記載する。フォーマットは以下の通り。 画像ID qw qx qy qz tx ty tz カメラID　画像ファイル名 ここで(qw, qx, qy, qz)は3次元回転のクウォータニオン表現で(tx, ty, tz)は
続きを読む

5点アルゴリズムによるカメラ位置・姿勢の推定

2枚の画像間のカメラ位置・姿勢を推定する際に、それぞれの画像を撮影したカメラの内部パラメータが既知の場合には5点アルゴリズムが利用できる。OpenCVにも5点アルゴリズムでカメラ位置・姿勢を推定するための関数 cv::findEssentialMat が用意されている。 使い方は、対応点の座標を以下のように指定するだけ。 cv::Mat essentialMat = cv::findEssentialMat(p1, p2); ただし、デフォルトでは焦点距離1.0、画像中心座標(0, 0)で計算するようになっているので、下記のコードのように内部パラメータ行列を用いて座標変換を行うか焦点距離、画像中心座標を指定する必要がある。座標変換を行わずに利用したい場合は、焦点距離、画像中心座標を第3,4引数に設定すれば良い。 推定したEssential Matrixからカメラの並進と回転を求めるためには、 cv::recoverPose で並進ベクトルと回転ベクトルに分解することができる。 以下のように、2枚の画像から対応点を求めて、5点アルゴリズムでカメラ位置・姿勢を推定することができる。 #include <iostream> #include <vector> #include <opencv2/opencv.hpp> #include <opencv2/nonfree.hpp> int main(int argc, char** argv) {   cv::initModule_nonfree(); //モジュールの初期化   //カメラパラメータの読み込みとレンズ歪の除去   cv::Mat img1; //入力画像1   cv::Mat img2; //入力画像2   cv::Mat K;   cv::Mat distCoeffs;   cv::FileStorage fs("camera.xml", CV_STORAGE_READ);   fs["intrinsicMat"] >> K;   fs["distCoeffs"] >> distCoeffs;   cv::undistort(cv::imrea
続きを読む

2D-3D対応からのカメラ位置・姿勢の推定

画像からの三次元復元処理や拡張現実感のための位置合わせ処理において、画像を撮影したカメラの位置・姿勢が必要になることがある。ここでは、カメラの内部パラメータ（焦点距離、画像中心座標、アスペクト比など）は既知とし、カメラの外部パラメータ（並進と回転）を求める方法を紹介する。 カメラの位置・姿勢は、三次元位置が既知の点Pとその画像上での観測座標pの複数の対応から求めることが出来る。このような問題はPerspective-n-Pont Problem（PnP問題）として知られている。OpenCVにはPnP問題を解くための関数cv::SolvePnPが用意されている。この関数の入力は三次元位置が既知の点の配列(objectPoints)とその画像上での観測座標の配列(imagePoints)、カメラの内部パラメータ行列、歪係数を入力とし、カメラの外部パラメータ（回転ベクトル、並進ベクトル）を推定する。 対応点の組み合わせに誤対応が含まれている場合は、cv::SolvePnPRansacを用いることでロバスト推定手法であるRANSACを用いた誤対応の排除が可能である。 また、推定手法としてLevenberg-Marquardt法による最適化(cv::ITERATIVE)や高速な推定手法であるEPnP(cv::EPNP)を選択することができる。経験的にはcv::ITERATIVEの方が安定した推定を実現できるように思う。 さらに、回転ベクトル、並進ベクトルの初期値が得られている場合には、第7引数をtrueにし、出力用のrvec, tvecに値を設定しておくことで、最適化の際の初期値として使用することができる。拡張現実感などの場合には、直前のフレームでの推定結果などを初期値として用いることで推定結果の安定性を向上させることができる。 #include <iostream> #include <vector> #include <opencv2/opencv.hpp> void genRT(cv::Mat& rvect, cv::Mat& tvect); void getCorrespondingPairs(cv::Mat K, cv::Mat d, cv::Mat rvec, cv::Mat tvec, std:
続きを読む