2014年6月21日土曜日

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]

0 件のコメント:

コメントを投稿