MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

GoogleのC++最適化ライブラリCeres Solverを使ったバンドル調整サンプルコード

目次

はじめに

今回も、GoogleのC++最適化ライブラリCeres Solverを使ってみよう

という記事です。

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

今回は、画像処理やロボットのアプリケーションで有名な

バンドル調整という問題を、

このCeres Solverを使って解くサンプルコードを解説したいと思います。

 

バンドル調整とは?

バンドル調整(Bundle Adjustment)とは、

画像処理の分野でよく利用される技術の一つです

 

画像データは基本的にそれぞれのピクセルの色情報を含みますが、

一枚の画像では写真に写っている幾何学的情報は含まれていません。

バンドル調整は、一般的に複数枚の画像のデータから、

その写真に写っている物体の幾何学情報を計算する手法を指します。

 

バンドル調整にも、様々な手法が提案されていますが、

基本的には、カメラや画像のモデルを規定し、

そのモデルのパラメータ群を変数として、

大規模な非線形最適化を実施することにより、

精密なモデルパラメータを推定します。

 

このバンドル調整の技術を使うことにより、

複数枚の画像から、対象物の三次元復元を実現する

Structure From Motion:SFMを実現したり、

自律移動の分野において、

精密な位置情報無しに、

大規模な周辺マップを作る

Simultaneous Localization And Mapping(SLAM)

などを実現することができます

myenigma.hatenablog.com

myenigma.hatenablog.com

 

バンドル調整の詳しい説明は下記の資料を参照ください。

 

Ceres Solverを使ったバンドル調整コード

元々、Ceres Solverが作られた理由の一つが、

大規模なバンドル調整問題を解くためだったようです。

 

画像内における特徴点の位置と、

その対応関係が分かる場合、

バンドル調整の目的は、

再投影誤差を最小にするような

特徴点の位置とカメラ位置を決めることになります。

 

一般的にバンドル調整の最適化問題は、

非線形最小二乗問題に帰着させることができ、

コスト関数は、観測された特徴点の画像平面上での二次元位置と、

パラメータから計算された特徴点の3次元位置を画像平面に投影した時の位置の

ノルム誤差を最小にすることになります。

Ceres Solverはこのバンドル調整を解くために特別な機能を有しています。

 

今回は、下記のリンク先で公開されているデータセットを使って、

バンドル調整用を解いてみたいと思います。

 

まず初めに、以前の記事で紹介した時と同じように、

再投影誤差を残差とするような、

コスト構造体を作ります。

  

コスト構造体の構造は、

以前の記事のExponentialResidualとかなり似ています。

バンドル調整のコスト関数は、

3次元の点群位置情報と、9個のカメラパラメータによって決まります。

この9個のカメラパラメータは、3つの回転ベクトル、3つの並進ベクトル、

一つの焦点距離パラメータ、そして2つの歪みパラメータです。

これらのパラメータの詳細は下記の資料を参照下さい。

 

まず、下記で再投影誤差のコスト関数構造体を作ります。

struct SnavelyReprojectionError {
  SnavelyReprojectionError(double observed_x, double observed_y)
      : observed_x(observed_x), observed_y(observed_y) {}

  template <typename T>
  bool operator()(const T* const camera,
                  const T* const point,
                  T* residuals) const {
    // camera[0,1,2] are the angle-axis rotation.
    T p[3];
  //観測点の座標変換(回転)
    ceres::AngleAxisRotatePoint(camera, point, p);
  //観測点の座標変換(並進)
    // camera[3,4,5] are the translation.
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

  //z座標補正
    // Compute the center of distortion. The sign change comes from
    // the camera model that Noah Snavely's Bundler assumes, whereby
    // the camera coordinate system has a negative z axis.
    T xp = - p[0] / p[2];
    T yp = - p[1] / p[2];

  // 歪み補正
    // Apply second and fourth order radial distortion.
    const T& l1 = camera[7];
    const T& l2 = camera[8];
    T r2 = xp*xp + yp*yp;
    T distortion = T(1.0) + r2  * (l1 + l2  * r2);

    // 最終投影位置の計算
    const T& focal = camera[6];
    T predicted_x = focal * distortion * xp;
    T predicted_y = focal * distortion * yp;

    //観測点との誤差の計算
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    return true;
  }

   // クラスの中にコスト関数を生成するコードを隠蔽する
   // Factory to hide the construction of the CostFunction object from
   // the client code.
   static ceres::CostFunction* Create(const double observed_x,
                                      const double observed_y) {
     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(
                 new SnavelyReprojectionError(observed_x, observed_y)));
   }

  double observed_x;
  double observed_y;
};

上記のコードでは、これまでmain関数の中で設定していた、

コスト関数のオブジェクトの生成をクラス内で定義するようにして、

その部分を隠蔽しています。

 

最後に、バンドル調整をするための最適化オブジェクトを下記のコードで生成します。

ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
  ceres::CostFunction* cost_function =
      SnavelyReprojectionError::Create(
           bal_problem.observations()[2 * i + 0],
           bal_problem.observations()[2 * i + 1]);
  problem.AddResidualBlock(cost_function,
                           NULL /* squared loss */,
                           bal_problem.mutable_camera_for_observation(i),
                           bal_problem.mutable_point_for_observation(i));
}

曲線当てはめの時と同じように、

それぞれの観測値に対して、コスト関数を追加するだけです。

あとは、最後に最適化を実施します。

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";

 

以上のコードは下記のディレクトリで参照できます。

また、より複雑なバンドル調整のコードは下記で参照できます。

 

Ceres Solver関連記事

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com