うたの日コンサート

沖縄に,,,,行きたい行きたい行きた~い.

 ということで沖縄病を発症してまして,BEGINと夏川りみばっかり聞いてます(笑)

 2年くらい前のアナザースカイにBEGINが出演してたんですが,2001年から沖縄慰霊の日の翌日を歌の日として,「うたの日コンサート」というのを毎年開催しているみたいです.調べたところ結構有名なコンサートみたいで,一度行ってみたいと思っているのですが,昨年はコロナでオンラインになってしまい...今年こそは!と思ってひそかに予定をたててたんですが,どうやら延期になってしまったようです...

www.utanohi.jp

ただ,中止ではないみたいなので,タイミングが合えば行ってみたいなあ.

5.バンドル調整 〜 直線探索

 ここでは,最適化に必要となる直線探索を扱います.これが,,,,なかなかに込み入ってて,とっても面倒でした.ただ,Numerical Optimization で紹介されているアルゴリズムの多くが直線探索に依存していたので,,,思い切って突っ込んで調べてみることにしました.簡単に実装はしてみたのですが,処理の流れを追う程度で見て頂ければと思います.実際に直線探索を作る必要がある方は, Ceres Solver とかを参考にしてみてください.

直線探索が必要になるとき

 最急降下法を用いた時の収束の様子をイメージした図が下記になります.

f:id:rkoichi2001:20210515171857p:plain
最適化が収束する様子

 図からもわかるように,上手くいけば反復毎に最適値へと近づいていくことになります.ここで,方向(ステップ方向)は計算式によって与えられるものの,その方向にどれだけ進めばよいか(ステップ幅)は明示的に与えられないため,実験的に確かめて決めていく必要があります.これを実現する方法を直線探索と言って,関数の値が "前提条件" を満たすまで,所定の方向に沿っていろんなステップを試します.

f:id:rkoichi2001:20210515171933p:plain
直線探索の必要性

 
 次に,先ほど言及した "前提条件" ですが,この前提によって直線探索は "Exact Line Search" と "Inexact Line Search" にわかれます.

f:id:rkoichi2001:20210515172013p:plain
終了条件でみた直線探索の分類

 Exact Line Search はステップ方向に沿って厳密に極小値を求めていくため,必然的に実行時間が長くなります.ただ,実際には毎ステップ厳密に極小値を求めなくても最適値にはたどりつくようで,多くの最適化ソフトウェアでは Inexact Line Search が使われているようです.今回は,Numerical Optimization と Ceres Solver を参考に勉強したので,"前提条件" として Strong Wolfe Condition を使った直線探索を見ていきたいと思います.

Strong Wolfe Condition

 Strong Wolfe Condition は下記の条件になります.

f:id:rkoichi2001:20210515172048p:plain
Strong Wolfe Condition

 スライドにも記載のある通り,Curvature Condition を設定することによって,開始点に近すぎる点が次の計算点として選択されることを防いでいます.

Strong Wolfe を実装した直線探索法

 これが思った以上にめんどくさく,とっても時間がかかったんですが(笑),スライドに挙動を起こしてみました.

f:id:rkoichi2001:20210515172111p:plain
Line Search with Strong Wolfe の実装.

 直線探索の実装は,Bracketing Phase と Selection Phase という2つの Phase に分かれます.まず,Bracketing Phase が走り,次に Selection Phase が走るのですが,それぞれの Phase でやっていることは,下記の様になります.
 Bracketing Phase : Strong Wolfe Condition を満たすであろう点の存在する範囲(上記スライドの水色範囲)を探索する.
 Selection Phase : Bracekting Phase で挟み込んだ範囲から,Strong Wolfe Condition を満たす点を探し出す.
 となります.

Bracketing Phase の実装

 ここでは,Bracketing Phase の実装を見ていきます.この Phase では範囲の絞り込みをやっています.いくつかケースを書き出して,図にしてみました.あと,実際にこのアルゴリズムを走らせる場合は数値計算的な部分(有限精度)も考慮する必要がありまして,範囲が狭くなりすぎるとループから抜けられなくなります.Bracket Size Check とスライドに記載した部分が該当の箇所になります.

f:id:rkoichi2001:20210515172211p:plain
Bracketing Phase の実装1
f:id:rkoichi2001:20210515172229p:plain
Bracketing Phase の実装2
bool LineSearchWithStrongWolfe(
    const std::vector<Track>& tracks_src,
    const std::vector<Eigen::Matrix3d>& K_src,
    const std::vector<Eigen::Vector3d>& T_src,
    const std::vector<Eigen::Vector3d>& Rot_src,
    const std::vector<Eigen::Vector3d>& points3d_src,
    const std::map<size_t, size_t>& extrinsic_intrinsic_map, size_t max_itr,
    double min_step, double alpha_max, double c1, double c2,
    double& alpha_star) {
  // X. Temporary buffer.
  std::vector<Eigen::Vector3d> T_tmp = T_src;
  std::vector<Eigen::Matrix3d> K_tmp = K_src;
  std::vector<Eigen::Vector3d> Rot_tmp = Rot_src;
  std::vector<Eigen::Vector3d> points3d_tmp = points3d_src;

  // X. Compute base value & base gradient.
  double phi_0 = ComputeReprojectionError(
      tracks_src, K_tmp, extrinsic_intrinsic_map, Rot_tmp, T_tmp, points3d_tmp);
  Eigen::MatrixXd grad_0 = ComputeGradient(K_tmp, T_tmp, Rot_tmp, points3d_tmp,
                                           tracks_src, extrinsic_intrinsic_map);
  double grad_0_max_norm = grad_0.col(0).lpNorm<Eigen::Infinity>();

  // X. Compute directional derivative.
  Eigen::MatrixXd step_dir = -grad_0.normalized();
  double der_phi_0 = grad_0.col(0).dot(step_dir.col(0));

  // X. Start iteration.
  double phi_i = phi_0;
  double phi_i_1 = phi_0;
  double der_phi_i = der_phi_0;
  double der_phi_i_1 = der_phi_0;

  // X. Initial alpha.
  double alpha_i_1 = 0;
  double alpha_i = std::min(1.0, alpha_max);

  // X. Status
  bool success = false;

  for (size_t itr = 0; itr < max_itr; itr++) {
    // X. Copy from original..
    T_tmp = T_src;
    K_tmp = K_src;
    Rot_tmp = Rot_src;
    points3d_tmp = points3d_src;

    // X. Update parameters based on alpha.
    UpdateParameters(alpha_i * -grad_0, tracks_src, extrinsic_intrinsic_map,
                     K_tmp, T_tmp, Rot_tmp, points3d_tmp);

    // X. Compute function value at alpha_i.
    phi_i = ComputeReprojectionError(tracks_src, K_tmp, extrinsic_intrinsic_map,
                                     Rot_tmp, T_tmp, points3d_tmp);

    // X. Condition 1.
    if (phi_i > phi_0 + c1 * alpha_i * der_phi_0 ||
        (phi_i >= phi_i_1 && itr > 0)) {
      success = Zoom(tracks_src, K_src, T_src, Rot_src, points3d_src,
                     extrinsic_intrinsic_map, alpha_i_1, alpha_i, c1, c2,
                     max_itr, min_step, alpha_star);
      break;
    }

    // X. Compute gradient at alpha_i.
    Eigen::MatrixXd grad_i =
        ComputeGradient(K_tmp, T_tmp, Rot_tmp, points3d_tmp, tracks_src,
                        extrinsic_intrinsic_map);
    der_phi_i = grad_i.col(0).dot(step_dir.col(0));

    // X. Condition 2.
    if (std::abs(der_phi_i) <= -c2 * der_phi_0) {
      alpha_star = alpha_i;
      success = true;
      break;
    }

    // X. Condition 3.
    if (der_phi_i >= 0) {
      success = Zoom(tracks_src, K_src, T_src, Rot_src, points3d_src,
                     extrinsic_intrinsic_map, alpha_i, alpha_i_1, c1, c2,
                     max_itr, min_step, alpha_star);
      break;
    }

    // X. Bracket range can not be found. Retry with different alpha
    alpha_i_1 = alpha_i;
    alpha_i = std::min(2 * alpha_i, alpha_max);
    phi_i_1 = phi_i;
    der_phi_i_1 = der_phi_i;

    // X. Bracket Size Check
    if ((std::abs(alpha_i - alpha_i_1) * grad_0_max_norm < min_step) ||
        (alpha_i_1 == alpha_max)) {
      success = false;
      alpha_star = alpha_i_1;
      break;
    }
  }

  return success;
}


 ※この実装,数値計算的なところも考慮する必要があり,思った以上に込み入ってまして(笑)流れを理解するために見て頂ければと思います.確実な詳細を知りたい方は,Google 先生の下記のコードをご覧あれ.
ceres-solver/line_search.cc at master · ceres-solver/ceres-solver · GitHub

Selection Phase の実装

 ここでは,Selection Phase の実装を見ていきます.Selection Phase は,Braceking Phase で Zoom(αi,αj) と記載した関数の実装になりますが,Zoom 関数に渡される時点で下記の前提が成立している必要があります.前提条件が満たされない場合,Strong Wolfe を満たす点が見つからない可能性があります.

f:id:rkoichi2001:20210515172300p:plain
Zoom 関数の入力に求められる前提条件

ここでも数値計算的な部分(有限精度)を考慮する必要がありまして,範囲が狭くなりすぎるとループから抜けられなくなります.Bracket Size Check とスライドに記載した部分が該当の箇所になります.また Selection Phase では,2次補間や3次補間を使って次の α を選択していましたが,今回は簡単のために中間点を使っています.

f:id:rkoichi2001:20210515172317p:plain
Selection Phase の実装1
f:id:rkoichi2001:20210515172331p:plain
Selection Phase の実装2
f:id:rkoichi2001:20210515172345p:plain
Selection Phase の実装3
f:id:rkoichi2001:20210515172357p:plain
Selection Phase の実装4
bool Zoom(const std::vector<Track>& tracks_src,
          const std::vector<Eigen::Matrix3d>& K_src,
          const std::vector<Eigen::Vector3d>& T_src,
          const std::vector<Eigen::Vector3d>& Rot_src,
          const std::vector<Eigen::Vector3d>& points3d_src,
          const std::map<size_t, size_t>& extrinsic_intrinsic_map,
          double alpha_low, double alpha_high, double c1, double c2,
          size_t max_itr, double min_step, double& alpha_star) {
  // X. Compute base value & base gradient.
  double phi_0 = ComputeReprojectionError(
      tracks_src, K_src, extrinsic_intrinsic_map, Rot_src, T_src, points3d_src);
  Eigen::MatrixXd grad_0 = ComputeGradient(K_src, T_src, Rot_src, points3d_src,
                                           tracks_src, extrinsic_intrinsic_map);
  double grad_0_max_norm = grad_0.col(0).lpNorm<Eigen::Infinity>();

  // X. Compute directional derivative.
  Eigen::MatrixXd step_dir = -grad_0.normalized();
  double der_phi_0 = grad_0.col(0).dot(step_dir.col(0));

  // X. Compute value at alpha_low.
  double phi_lo = ComputeReprojectionErrorWithStepLength(
      -grad_0, alpha_low, tracks_src, K_src, T_src, Rot_src, points3d_src,
      extrinsic_intrinsic_map);

  // X. Initial alpha
  double alpha_low_tmp_ = alpha_low;
  double alpha_high_tmp_ = alpha_high;

  // X. Result
  bool success = false;

  for (size_t itr = 0; itr < max_itr; itr++) {
    // X. Bracket Size Check
    if ((std::abs(alpha_high_tmp_ - alpha_low_tmp_) * grad_0_max_norm <
         min_step)) {
      success = false;
      alpha_star = alpha_low;
      break;
    }

    // X. Find trial value.
    double alpha_j = ComputeTrialValues(alpha_low_tmp_, alpha_high_tmp_);

    // X. Update parameters based on alpha.
    std::vector<Eigen::Vector3d> T_tmp = T_src;
    std::vector<Eigen::Matrix3d> K_tmp = K_src;
    std::vector<Eigen::Vector3d> Rot_tmp = Rot_src;
    std::vector<Eigen::Vector3d> points3d_tmp = points3d_src;
    UpdateParameters(alpha_j * -grad_0, tracks_src, extrinsic_intrinsic_map,
                     K_tmp, T_tmp, Rot_tmp, points3d_tmp);
    double phi_j =
        ComputeReprojectionError(tracks_src, K_tmp, extrinsic_intrinsic_map,
                                 Rot_tmp, T_tmp, points3d_tmp);

    if (phi_j > phi_0 + c1 * alpha_j * der_phi_0 || phi_j >= phi_lo) {
      // X. Shrink upper window.
      alpha_high_tmp_ = alpha_j;
    } else {
      // X. Evaluate derivative value.
      Eigen::MatrixXd grad_j =
          ComputeGradient(K_tmp, T_tmp, Rot_tmp, points3d_tmp, tracks_src,
                          extrinsic_intrinsic_map);
      double der_phi_j = grad_j.col(0).dot(step_dir.col(0));

      // X. Find point satisfying wolfe condition.
      if (std::abs(der_phi_j) <= -c2 * der_phi_0) {
        alpha_star = alpha_j;
        success = true;
        break;
      }

      // X. Shrink upper window.
      if (der_phi_j * (alpha_high_tmp_ - alpha_low_tmp_) >= 0) {
        alpha_high_tmp_ = alpha_low_tmp_;
      }

      // X. Shrink lower window.
      alpha_low_tmp_ = alpha_j;
      phi_lo = phi_j;
    }
  }

  return success;
}

 ※この実装,数値計算的なところも考慮する必要があり,思った以上に込み入ってまして(笑)流れを理解するために見て頂ければと思います.確実な詳細を知りたい方は,Google 先生の下記のコードをご覧あれ.
ceres-solver/line_search.cc at master · ceres-solver/ceres-solver · GitHub

ふ~.ということで,次に行きます!

実験コード

github.com

目次のページ

 バンドル調整はシリーズ物として書いてまして,目次は下記のページになります.
daily-tech.hatenablog.com

4.バンドル調整 〜 勾配ベクトルとヘッシアンの作成

このエントリでは,バンドル調整のヘッシアンが典型的にはどのような構造になるかを見てみます.その後,それに基づいて勾配ベクトルとヘッシアンを作ります.

ニュートン法からヘッシアンを考える.

f:id:rkoichi2001:20210507203309j:plain
ニュートン法の導出とニュートンステップの計算

 上記の通り,とくに行列が大きくなってくると「エイや!」で計算してはいけないようです.ということで,今回の実験におけるヘッシアンがどのような構造になっているか見てみます.

勾配ベクトルとヘッシアンの構造

f:id:rkoichi2001:20210507203449j:plain
勾配ベクトルとヘッシアンの構造

 上記が今回のケースでのヘッシアンですが,右下のブロックに白のセルが目立つと思います.下のスライドで説明していますが,パラメータ間につながりがないもの同志で評価関数を微分すると0になるため,これが該当するヘッシアンのセルが0になっています.

例えば,下記のようなケースで二階微分がゼロになってます.
・評価関数を異なるカメラの外部パラメータで微分したもの.
・評価関数を異なる点で微分したもの.
・評価関数をあるカメラの外部パラメータで微分し,当該カメラと異なる内部パラメータで微分したもの.

f:id:rkoichi2001:20210507204332j:plain
ヘッシアンの構造

次に,右下のブロックが Sparse であることを生かして,方程式を解いていきます.

シューアの補行列を使ってニュートンステップの計算

※ここでの議論では,ヘッシアンの正定性はすでに仮定してあります.

f:id:rkoichi2001:20210507210223j:plain
シューアの補行列をつかったニュートンステップ1

シューアの補行列を用いて,巨大な一つの線形方程式群を二つに分割できました.分割するだけだと計算量的にあまりメリットはないのかもしれませんが,ここで右下ブロックの Sparsity を使います.

f:id:rkoichi2001:20210507210244j:plain
シューアの補行列をつかったニュートンステップ2

勾配ベクトルの実装

ちなみに,実際の勾配ベクトルの計算は次の様になりました.ヘッシアンも同じような形で計算したのですが,多いので省きます(笑)

勾配ベクトルの計算:
https://github.com/koichiHik/blog_bundle_adjust/blob/main/src/optimization/src/differential_operators.cc

ヘッシアンの計算:
https://github.com/koichiHik/blog_bundle_adjust/blob/main/src/optimization/src/compute_hessian.cc

ヘッシアンに関しては,自分はメモリをすべて確保してから単純にそれぞれの要素を加えていったのですが,0の要素がいっぱいできてしまうのでこれは非効率で,本当は非ゼロの要素にだけメモリを割り当ててニュートンステップを計算するみたいです.
詳しくは下記の論文をどうぞ...

SBA: A software package for generic sparse bundle adjustment

勾配の計算部分を少しだけ...

  // すべてのカメラでループ.
  for (size_t cam_ext_idx = 0; cam_ext_idx < cam_ext_num; cam_ext_idx++) {

    // すべての3次元点でループ.      
    for (size_t trk_idx = 0; trk_idx < track_num; trk_idx++) {

      // 当該カメラに3次元点が写っていれば.
      if (tracks[trk_idx].count(cam_ext_idx)) {

        Eigen::Vector2d meas = tracks[trk_idx].at(cam_ext_idx);
        Eigen::Vector3d x =
            cameras[cam_ext_idx] * points3d[trk_idx].homogeneous();
        Eigen::Vector2d proj = x.hnormalized();

        // 0.残差の計算.
        double u_res = proj(0) - meas(0);
        double v_res = proj(1) - meas(1);

        // 1.勾配のカメラ外部パラメータ部分の計算
        size_t cam_int_idx = extrinsic_intrinsic_map.at(cam_ext_idx);
        {
          size_t col_idx = cam_ext_idx * cam_ext_var_num;
          grad(col_idx, 0) +=
              u_res * du_dtx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx],
                             points3d[trk_idx], x) +
              v_res * dv_dtx(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx],
                             points3d[trk_idx], x);

         // y, z, kx, ky, kz についても同様に計算 ・・・・・
        }

        // 2.勾配のカメラ内部パラメータ部分の計算
        {
          double scale = 1.0;
          size_t col_idx =
              cam_ext_num * cam_ext_var_num + cam_int_idx * cam_int_var_num;
          grad(col_idx, 0) +=
              scale * (u_res * du_dfx(T[cam_ext_idx], Rot[cam_ext_idx],
                                      K[cam_int_idx], points3d[trk_idx], x) +
                       v_res * dv_dfx(T[cam_ext_idx], Rot[cam_ext_idx],
                                      K[cam_int_idx], points3d[trk_idx], x));

         // y, z, kx, ky, kz についても同様に計算 ・・・・・
        }

        // 3.勾配の点群位置パラメータ部分の計算
        {
          size_t col_idx = cam_ext_num * cam_ext_var_num +
                           cam_int_num * cam_int_var_num +
                           trk_idx * points_var_num;

          grad(col_idx, 0) +=
              u_res * du_dX(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx],
                            points3d[trk_idx], x) +
              v_res * dv_dX(T[cam_ext_idx], Rot[cam_ext_idx], K[cam_int_idx],
                            points3d[trk_idx], x);

         // Y,Z についても同様に計算 ・・・・・
        }
      }
    }
  }

*

実験コード

github.com

目次のページ

 バンドル調整はシリーズ物として書いてまして,目次は下記のページになります.
daily-tech.hatenablog.com

3.バンドル調整 ~ Sympy を使った微分計算

 前回のエントリで回転のパラメータ化をしたことによって,透視投影の式から冗長なパラメータがなくなりました.ここからはバンドル調整を「制約のない最適化問題(Unconstrained Optimziation)」として解いていきます.

 実際には3次元復元自体に "基準座標の不定性" と "スケールの不定性" があるので,まだ冗長性が残っているのですが,これは後のエントリで出てきます.

制約条件の無い最適化(Unconstrained Optimization)

 最適化の方法をざっくりとおさらいしておきたいと思います.前提となるアイデアは,
「最急降下方向に下っていけばいつかは最小値にたどり着く.」
ですが,これをストレートに実施する方法が最急降下法で,もう少し工夫して収束を速めた方法がニュートン法ということになります.ここで,実際に最急降下法ニュートン法の更新ステップをみて,必要な微分式を洗い出しておきたいと思います.

f:id:rkoichi2001:20210505210537j:plain
最急降下法ニュートン法をざっくりと.
f:id:rkoichi2001:20210505210609j:plain
勾配ベクトルとヘッシアン.

 上記のスライドを見て頂ければわかる通り,変数の数が大きくなるとヘッシアンが巨大になります...次に,今回の問題の場合に勾配ベクトル,ヘッシアンがどうなるか見てみたいと思います.

バンドル調整の評価関数と微分

 まず,今回の評価関数を再確認します.

f:id:rkoichi2001:20210505210648j:plain
最小化したい評価関数.

 この関数を微分していくことになります.シグマの形では見づらいので,ベクトルの Inner Product の形に式を変形しています.

Gradient,Hessianに出てくる微分要素

 次に,実際に評価関数 E を任意のパラメータ x で微分したときにどうなるか見てみます.最終的に,勾配ベクトルはすべてのパラメータで E を偏微分したものをベクトルの形に並べたものになります.

f:id:rkoichi2001:20210505210805j:plain
評価関数の偏微分
f:id:rkoichi2001:20210505210836j:plain
評価関数の偏微分
f:id:rkoichi2001:20210505210905j:plain
評価関数の偏微分
f:id:rkoichi2001:20210505210921j:plain
評価関数の偏微分

Sympy を使った微分計算

 勾配ベクトルに必要な微分式は28個,ヘッシアンに必要な微分式は392個になりました.が,,,これを手計算するのはさすがにつらいので,ありがたく Sympy を使わせてもらいます.まず,Sympy による微分の方法ですが,とても簡単です.例として, f(x,y) = x*sin(x*x) + y*cos(y*y) を x と y に関して偏微分してみます.

import sympy

if __name__ == "__main__":
	x = sympy.Symbol('x')
	y = sympy.Symbol('y')
	f = x * sympy.sin(x**2) + y * sympy.cos(y**2) 
	print('df_dx : ' + str(sympy.diff(f, x)))
	print('df_dy : ' + str(sympy.diff(f, y)))

で,上記のコードを実行すると次のような結果が出ます.

df_dx : 2*x**2*cos(x**2) + sin(x**2)
df_dy : -2*y**2*sin(y**2) + cos(y**2)

 が,今回の実装では Python でなく C++ を使っているので,二乗の計算が "**2" になってしまうとつらいです....最初は置き換えしようとしてたんですがあまりの数の多さにげんなりしまして(笑)Cのフォーマットでプリントしてくれる方法を見つけました.

import sympy
from sympy import ccode

if __name__ == "__main__":
	x = sympy.Symbol('x')
	y = sympy.Symbol('y')
	f = x * sympy.sin(x**2) + y * sympy.cos(y**2) 
	print('df_dx : ' + str(ccode(sympy.diff(f, x), standard='c89')))
	print('df_dy : ' + str(ccode(sympy.diff(f, y), standard='c89')))

 上記のコードを走らせると,,,,だだーん(゚Д゚)ノ

df_dx : 2*pow(x, 2)*cos(pow(x, 2)) + sin(pow(x, 2))
df_dy : -2*pow(y, 2)*sin(pow(y, 2)) + cos(pow(y, 2))

めでたしめでたし!です.

Sympy に微分させる実際の式

 ということで,必要な微分式は上のスライドで導出したので,あとは Sympy に頑張ってもらえばよいだけです!本当に,ここまで立式した数式をそのまま突っ込んで実行すると計算してくれます.ありがたや...

 ただ,回転のパラメータ化のところで触れたと思うのですが, Angle-Axis Representation では回転角が 0 のケースが特異点になります.この特異点を避けなければいけないので,角度の値に応じて数式を2種類用意します.

Symbolの定義.

        # 1. 3D Point Coordinate.
        self.X = sympy.Symbol('p(0)')
        self.Y = sympy.Symbol('p(1)')
        self.Z = sympy.Symbol('p(2)')

        # 2. Internal Camera Parameters.
        self.fx = sympy.Symbol('K(0,0)')
        self.fy = sympy.Symbol('K(1,1)')
        self.cx = sympy.Symbol('K(0,2)')
        self.cy = sympy.Symbol('K(1,2)')
        self.s = sympy.Symbol('K(0,1)')
        self.f0 = sympy.Symbol('K(2,2)')
        self.K = sympy.Matrix([[self.fx, self.s, self.cx], [0, self.fy, self.cy], [0, 0, self.f0]])

        # 3. External Camera Parameters.
        # Translation.
        self.tx = sympy.Symbol('T(0)')
        self.ty = sympy.Symbol('T(1)')
        self.tz = sympy.Symbol('T(2)')
        self.T = sympy.Matrix([self.tx, self.ty, self.tz])

        # Rotation.
        self.vx = sympy.Symbol('rot(0)')
        self.vy = sympy.Symbol('rot(1)')
        self.vz = sympy.Symbol('rot(2)')
        self.theta = sympy.sqrt(self.vx**2 + self.vy**2 + self.vz**2)

        # Identity
        self.I = sympy.Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])

        # Outer product matrix
        self.W = sympy.Matrix([[0, -self.vz, self.vy], [self.vz, 0, -self.vx], [-self.vy, self.vx, 0]])

回転角度が一定以上あるとき.

        self.R = self.I + (sympy.sin(self.theta) / self.theta) * self.W + \
                ((1 - sympy.cos(self.theta)) / self.theta**2) * self.W * self.W
        self.P = self.K * (self.R.row_join(self.T))

        self.x = self.P * sympy.Matrix([self.X, self.Y, self.Z, 1])
        self.u = self.x[0] / self.x[2]
        self.v = self.x[1] / self.x[2]

回転角度が微小な時.

        self.R = self.I + self.W
        self.P = self.K * (self.R.row_join(self.T))

        self.x = self.P * sympy.Matrix([self.X, self.Y, self.Z, 1])
        self.u = self.x[0] / self.x[2]
        self.v = self.x[1] / self.x[2]

で,上記コードの "self.u" が xpred, "self.y" が ypred になります.あとはこれを各要素で偏微分していけば微分要素がすべて出てきます.

実験コード

 下記リポジトリの,blog_bundle_adjust/python/compute_derivatives_1st.py を実行すれば1階微分,blog_bundle_adjust/python/compute_derivatives_2nd.py を実行すれば2階微分が出力されます.

github.com

 数が多いので,全部関数の形でターミナルにプリントアウトしています.それをコピペすれば,include だけ自分で書けばコンパイルが通るようなっています.なので,実際に実行すると下記の様な形で関数が出力されます.
(すみません.ファイルが出力されて,コンパイルすればそのままOK!にはなってません(笑))

inline double du2_dXdX(const Eigen::Vector3d &T, const Eigen::Vector3d &rot, const Eigen::Matrix3d &K, const Eigen::Vector3d &p, const Eigen::Vector3d &x) {
  double theta = std::pow(
      std::pow(rot(0), 2) + std::pow(rot(1), 2) + std::pow(rot(2), 2), 0.5);
  if (std::numeric_limits<double>::epsilon() < rot.dot(rot)) {
    return "回転角度が一定以上ある場合の微分式";
  } else {
    return "回転角度が微小な場合の微分式";
  }
}

ということで,微分の計算が終わりました(*ノωノ)
次は勾配ベクトルを作成して,最急降下法をやってみます.

目次のページ

 バンドル調整はシリーズ物として書いてまして,目次は下記のページになります.
daily-tech.hatenablog.com

2.バンドル調整 ~ 回転のパラメータ化

透視投影の式

 前回エントリで扱ったモデル化で,カメラの透視投影の式を導出しました.下図のように基準座標となるカメラを決定すると,

f:id:rkoichi2001:20210504190047j:plain
各カメラの相対位置

 カメラの投影点を求めるためには下記の式を使う必要がありました.

f:id:rkoichi2001:20210504190106j:plain
基準座標で表した点の位置を使い,各カメラでの投影座標を計算.

 ここで,各変数の自由度は次のようになります.

  • 各カメラの併進ベクトル Tk : 自由度3
  • 各カメラの内部パラメータ行列 Kk : 自由度5
  • 各カメラの併進ベクトル Rk : 自由度3

 併進ベクトル,内部パラメータ行列に関しては,設定した変数の数がそのまま自由度となっています.一方で,回転行列は 3x3 行列で表現しているので,一見すると変数は9個あるように見えますが,回転の自由度は3なので冗長な表現となっていることがわかります.

回転のパラメータ化の必要性

 冗長な表現を避けるために回転行列から変換する方法としては,次の3つの方法がよく使われているみたいです.

  • Quaternion を使ったパラメータ化
  • Angle-Axis Representation を使ったパラメータ化
  • 微小回転を用いたパラメータ化

 Ceres-Solver ではデフォルトで Angle-Axis Representation が使われていたこともあり,今回は Angle-Axis Representation でやってみました.微小回転を用いたパラメータ化の説明については,金谷先生の下記の本がとても分かりやすかったです.

Angle-Axis Representation を用いた回転のパラメータ化

 Angle-Axis Representation を考えるために,ロドリゲスの式を導出します.この導出では,任意の回転軸(k)周りの任意の回転角(θ)の回転移動を回転軸ベクトル k と 回転各 θ を用いて表現します.

f:id:rkoichi2001:20210504190144j:plain
ロドリゲスの公式

 ロドリゲスの公式で,回転前の点 r を回転後の点 r' に変換させることができました.一方で,この回転と等価の回転行列 R を使っても点は変換できる(r' = Rr) となるはずなので,ロドリゲスの公式から等価な回転行列を求めます.ベクトルの外積計算を表現するために,外積計算用の行列を導入しています.

f:id:rkoichi2001:20210504190158j:plain
回転軸と回転角を使った回転行列の表現

 ということで,回転行列を回転軸と回転角を使って表現できるようになりました.が,回転軸と回転角で変数が計4つあります.もう一つ減らしたい...ということで,減らします.ここまでは回転軸ベクトルは単位ベクトルでしたが,回転軸ベクトルの大きさを回転角とすることによって変数を3つにします.

f:id:rkoichi2001:20210504190223j:plain
変数の数を4から3に

 ふ~.できました.が,このままだと回転角が小さい,もしくは0の時に式が不定になってしまいます.つまり,回転角が0の時がこの表現方法の特異点ということになりますが,このままではまずいので,次にこれを除去します.

特異点の除去

f:id:rkoichi2001:20210504190241j:plain
特異点の除去

 特異点,除去完了!ということで,実際にここまでで導出した式を使って,透視投影の方程式をワイルドに偏微分していきます(笑)

目次のページ

 バンドル調整はシリーズ物として書いてまして,目次は下記のページになります.
daily-tech.hatenablog.com

1.バンドル調整 ~ モデル化,及び問題設定

2年前に「最適化計算5 バンドル調整 理論編1」としてエントリを書いていたのですが,今回はこの記事をリライトしています.

バンドル調整とは

 三次元シーンを複数のカメラ・視点で撮影した時,全体的に「一番つじつまが合っている.」構成を求めるための手法です.ここで,「つじつまが合う」というのが何なのかを定義してやる必要があります.三次元復元の場合,「再投影誤差を小さくする.」ということがそれに該当します.(うーん.二年前に書いた表現ですが,なんだかうまく書き直せないのでそのままにします.)

バンドル調整のイメージ

 3次元点と当該点が各カメラのスクリーンに映った様子を図示します.

f:id:rkoichi2001:20210503171438j:plain
3次元点と対応する画像座標

 それぞれのカメラに関して,3次元座標とスクリーン投影座標には下記の透視投影の関係が成立します.簡単のために,レンズの歪は考えていません.

f:id:rkoichi2001:20210503171758j:plain
各カメラで成立する透視投影の関係

 ただ,ここで成立している関係はそれぞれのカメラ座標系に関してのものです.実際にバンドル調整をするときには基準座標を決めてあげないといけません.今回は簡単のためにカメラ1を基準座標として設定します.基準座標を合わせるために,それぞれの座標系であらわされている点 a の三次元座標を基準座標系(=カメラ1)で表します.それぞれのカメラの位置がわかっていれば,カメラ間の相対位置関係から次のように変換できます.ここでは,カメラ間の相対位置関係が次のようにわかっているとします.

f:id:rkoichi2001:20210503170914j:plain
カメラの相対位置関係

 次に,カメラ間の相対位置関係を用いて点 a の座標変換をします.この変換で,「カメラ k から見た点 a の座標」を「基準座標での点 a の座標」を用いて表せるようになります.

f:id:rkoichi2001:20210503211953j:plain
点の座標変換

 各カメラで成立する透視投影の関係に,上スライドの点 a の座標変換式を代入すると,結果的に下記のようにまとまります.

f:id:rkoichi2001:20210503212032j:plain
基準座標で表した点の位置を使い,各カメラでの投影座標を計算.

 上スライドの最後の式を用いれば,「カメラの3次元位置」,「カメラの内部パラメータ」,「被写体(点 a)の3次元位置」のすべてがわかっていれば,「被写体が写真のどこに写るか」がわかることになります.この式をもう少し書き出すと,,,

f:id:rkoichi2001:20210503171123j:plain
被写体のスクリーン投影点を計算するためのモデルができた.

 これで,スクリーン投影点の "モデル” ができたことになります.一方で,被写体が画像のどこに写っているかは実測できるので,ここまでの導出で被写体の画像座標に関して「実測値」と「予測値(モデル値)」がわかることになります.ここで,モデルが正確だと仮定すると,点 a の位置,各カメラの内部/外部パラメータが完全にあっていれば実測値と予測値は一致します.逆にずれが大きいと,どこかに誤差があることになります.パラメータの誤差は間接的(実測値と予測値の乖離が大きいか否か)にしかわかりませんが,次に,この「誤差の程度」を定義するために評価関数を定義します.
 

最小化したい評価関数・誤差関数

 ここで「誤差の程度」を定義するために,下記の評価関数を使います.式の定義を見て頂ければわかるように,「実測値」と「予測値(モデル値)」が完全に一致すれば評価関数の値は0になります.つまり,評価関数の値が小さくなるほど,モデルに使用されている値が正確であるということになります.

f:id:rkoichi2001:20210503171315j:plain
評価関数の定義

 ここまでの式導出の流れだと,
 1.パラメータ(「カメラの3次元位置」,「カメラの内部パラメータ」,「被写体(点 a)の3次元位置」)がわかる.
 2.モデルに値を代入して,予測値を計算する.
 3.予測値と実測値を評価関数に代入して,どの程度の誤差になるか求める.
 4.誤差が大きいので,手修正で(笑)ちょっとパラメータをいじってみる.そして2にもどる(笑)
 だと思うのですが,バンドル調整では誤差関数の最適化をすることによって「手修正によらず(笑)」最適パラメータを求めます.うーん.いつものことながら,うまいことできているなあと感心します.

今回の問題設定のイメージ

 今回の問題設定のイメージは,下記になります.

f:id:rkoichi2001:20210503174632j:plain
今回の問題設定

 3次元空間では下記の様な設定になってまして,それぞれのカメラで撮影した円環の画像も載せています.

f:id:rkoichi2001:20210503174746j:plain
3台のカメラ配置とそれぞれのカメラに写った円環

問題の前提

 問題があまり複雑になるといけない一方で,実際に使用する仮定と大きくずれると良くないので,下記の前提でバンドル調整を実施しました.
下記の3点を既知とする.
 1.画像に写っている点の画像座標.
 2.3台のカメラの位置・角度.(バンドル調整を実施する前に,この値に誤差を載せます.)
 3.円弧状の点の3次元位置.(バンドル調整を実施する前に,この値に誤差を載せます.)

カメラの内部パラメータは2種類あり,カメラ1とカメラ2が同じものを共有.カメラ3のみ異なる内部パラメータを保持.

ということで次のエントリに続きます...

目次のページ

 バンドル調整はシリーズ物として書いてまして,目次は下記のページになります.
daily-tech.hatenablog.com

0.バンドル調整 〜 目次

 ご無沙汰しております!社長です!「月に1本のブログアップ」といった新年の誓いもなんのその(笑),更新せずに半年たっちゃいましたが,せっかくの自粛GWを活かして勉強したことまとめ書きします!ということで,以下,エントリです∠(`・ω・´)


 春だ,桜だ,バンドル調整だ~.ということで,牛歩のように進めている一人SFMプロジェクト,やっとこさバンドル調整まで来ました(笑)仕事でも最適化が使いこなせるようになりたいと思っていたことも有り,少しずつ勉強していたのですが,ちょうどいい機会なのでバンドル調整の文脈で最適化のエントリを書いてみたいと思います.このページは目次として書いていますが,個々のページが書き終わった段階でここにリンクを貼りたいと思います.心が折れなければ,,,,最後まで書ききれると思います!

 ちなみに,どの文献をよんでも「ヘッシアンの計算は大変なので,,,」とあったので,力ずくで計算してみました.どろんこになりながら実装したので,少しは最適化を身近に感じるようになりました(笑)

X.モデル化,及び問題設定

 バンドル調整の簡単な説明,モデル化,最小化する数式を説明します.
daily-tech.hatenablog.com

X.回転のパラメータ化

 回転行列のエントリは9つ(3x3)ありますが,回転の自由度は3なので,回転行列以外の方法でパラメータ化しなければいけません.今回は,Angle-Axisを用いてパラメータ化しています.
daily-tech.hatenablog.com

X.Sympy を使った微分計算

 ヤコビアン,ヘッシアンともに近似をつかわず(!)計算しています(笑)さすがに手計算はむりだったので,Sympyを使っています.
daily-tech.hatenablog.com

X.勾配ベクトルとヘッシアンの作成

 バンドル調整における典型的なヘッシアンの構造を踏まえて,勾配ベクトル/ヘッシアンを作ります.
daily-tech.hatenablog.com

X.直線探索

 最急降下法ニュートン法を実行する上で必要となる直線探索のアルゴリズムを調べています.
daily-tech.hatenablog.com

X.最急降下法を用いた解法

 まずは一番シンプルに,最急降下法です.収束の遅さを実感します.

X.共役勾配法用いた解法

 次に,ヘッシアンを使わない共役勾配法がどの程度実用的なのか,やってみます.

X.ニュートン法を用いた解法

 力ずくで計算したヘッシアンを使って,最適化をやってみました.ただ,簡単には収束せず...

X.レベンバーグ・マルカート法を用いた解法

 非線形最小二乗法のデファクトスタンダードである,レベンバーグマルカート法を試しました.

X.Ceres-Solver を用いた解法

 最後に,,,同じ問題設定を Ceres-Solver を用いて解きました.ヤコビアンもヘッシアンも計算せず,あっという間ですね...

X.参考文献

Bundle Adjustment – A Modern Synthesis

SBA: A software package for generic sparse bundle adjustment

X.実験に使ったソースコード

 ソースコードはまだちょっと整理中でして,個々のエントリのアップする時には間に合わせるようにします.
github.com