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