魚眼カメラのキャリブレーション 〜 実装編2(再投影誤差の最小化)

相変わらず順番前後してますが,魚眼カメラのキャリブレーション最終編です.

0.前回までの流れ

下記のエントリで,全体構成が大きく分けて5つのステップからなっていることを説明しました.
daily-tech.hatenablog.com

1.InitializeParamsBasedOnFlagSetting
キャリブレーションフラグの設定値読み取り.
2.CalibrateExtrinsics
各画像撮影時のキャリブボードとカメラの位置計算.
3.MinimizeReprojectionError
すべての画像に対する再投影誤差の「和」の最小化.
4.EstimateUncertainties
キャリブの結果まとめ.
5.ConvertFormat
フォーマットの変換.

で,キャリブレーションの本質的なところは「2.CalibrateExtrinsics」「3.MinimizeReprojectionError」になります.他のステップは設定値の読み取りやフォーマットの変換的な箇所です.前回の下記エントリで,
daily-tech.hatenablog.com

「2.CalibrateExtrinsics」の内容を書いたので,このエントリでは「3.MinimizeReprojectionError」の部分を見ていきます.ちなみに,前回のエントリでもガウスニュートン法を使った最適化が出てきましたが,前回のパートでは一枚の画像に写っているキャリブボードに対して最適化してます.今回のエントリでは,「2.CalibrateExtrinsics」で求めたすべてキャリブボードの画像に対しての誤差の「和」が最小になるように最適化します.

1.今回の処理「3.MinimizeReprojectionError」の流れ

コード内にもざっくりとステップを書いたんですが,後述4ステップの繰り返しが再投影誤差最小化の大きな流れになります.ちなみに,この再投影誤差最小化のパラメータベクトルは下記になります.キャリブ対象となるカメラが1つなので,内部パラメータは9個固定ですが,外部パラメータは撮影枚数分存在するので6個 x キャリブボードの画像枚数となります.

最適化対象となるパラメータ(9個 + 6個 x 画像枚数)

内部パラメータ(合計9個)
fx, fy : 焦点距離
cx, cy : カメラの主点
k1, k2, k3, k4 : 魚眼モデルのパラメータ(詳しくは理論編を参照くださいませ.)

外部パラメータ(合計6個 x キャリブボードの画像枚数)
r1, r2, r3 : キャリブボードのロドリゲスの各要素.
t1, t2, t3 : キャリブボードの並進の各要素

再投影誤差最小化の4ステップ

Step 1 : Condition check for loop termination.
最適化計算のループ終了条件チェックです.

Step 2 : Compute jacobian and gradient in parameter space. (Gauss Newton 1)
最適化対象となっているパラメータ空間でのヤコビアンを計算.ヤコビアンからヘッシアンを近似して,特異値分解を使うことによってパラメータ空間の最急降下ベクトルを計算します.Gauss Newton法の前半部分です.

Step 3 : Update parameter by d_param * alpha_smooth2. (Gauss Newton 2)
最急降下ベクトルによる更新をパラメータベクトルに対して反映し,このターンでのGauss Newton法を終了します.Gauss Newton法の後半部分です.

Step 4 : Recomputing extrinsics based on the updated parameter sets.
パラメータベクトルが更新されたので,これを元に再度外部パラメータを更新します.

該当部コード
bool MinimizeReprojectionError(const cv::TermCriteria& criteria,
                               const vector<vector<cv::Point3f>>& objectPoints,
                               const vector<vector<cv::Point2f>>& imagePoints,
                               const int check_cond, const double thresh_cond,
                               const int recompute_extrinsic,
                               IntrinsicParams& final_param,
                               std::vector<Vec3d>& board_rotations,
                               std::vector<Vec3d>& board_translations) {
  double updated_ratio = 1;
  cv::Vec2d err_std;
  for (int iter = 0; iter <= std::numeric_limits<int>::max(); ++iter) {
    // Step 1 : Condition check for loop termination.
    if ((criteria.type == 1 && iter >= criteria.maxCount) ||
        (criteria.type == 2 && updated_ratio <= criteria.epsilon) ||
        (criteria.type == 3 &&
         (updated_ratio <= criteria.epsilon || iter >= criteria.maxCount))) {
      break;
    }

    // Step 2 :
    // Compute jacobian and gradient in parameter space. (Gauss Newton 1)
    Mat d_param;
    {
      Mat JtJ, Jtex;
#if 1
      ComputeJacobians(objectPoints, imagePoints, final_param, board_rotations,
                       board_translations, check_cond, thresh_cond, JtJ, Jtex);
#else
      ComputeJacobiansNaively(objectPoints, imagePoints, final_param,
                              board_rotations, board_translations, check_cond,
                              thresh_cond, JtJ, Jtex);
#endif
      solve(JtJ, -Jtex, d_param);
    }

    // Step 3 :
    // Update parameter by d_param * alpha_smooth2. (Gauss Newton 2)
    {
      const double alpha_smooth = 0.4;
      double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0);
      IntrinsicParams temp_param = final_param + alpha_smooth2 * d_param;
      updated_ratio = norm(Vec4d(temp_param.f[0], temp_param.f[1],
                                 temp_param.c[0], temp_param.c[1]) -
                           Vec4d(final_param.f[0], final_param.f[1],
                                 final_param.c[0], final_param.c[1])) /
                      norm(Vec4d(temp_param.f[0], temp_param.f[1],
                                 temp_param.c[0], temp_param.c[1]));
      final_param = temp_param;
    }

    // Step 4 :
    // Recomputing extrinsics based on the updated parameter sets.
    if (recompute_extrinsic) {
      CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond,
                          thresh_cond, board_rotations, board_translations);
    }
  }

  return true;
}

2.Step 2 : ヤコビアン計算とパラメータ空間での最急降下ベクトル計算

ちょっとヤコビアンの計算部分が大きいですが,,,書き切ります.まず,全体のコード概要です.このブロックでは,問題全体に対する近似ヘッシアン「JtJ」とヤコビアンx誤差ベクトル「-Jtex」を計算します.で,「JtJ * d_param = -Jtex」というガウスニュートン法の線形方程式をときます.線形方程式を解いているのが「solve(JtJ, -Jtex, d_param)」の部分になります.

該当部コード
// Step 2 :
// Compute jacobian and gradient in parameter space. (Gauss Newton 1)
Mat d_param;
{
  Mat JtJ, Jtex;
#if 1
  ComputeJacobians(objectPoints, imagePoints, final_param, board_rotations,
                   board_translations, check_cond, thresh_cond, JtJ, Jtex);
#else
  ComputeJacobiansNaively(objectPoints, imagePoints, final_param,
                          board_rotations, board_translations, check_cond,
                          thresh_cond, JtJ, Jtex);
#endif
  solve(JtJ, -Jtex, d_param);
}

2.1 ヤコビアンの計算ロジック

では,ComputeJacobiansという関数を見ていきたいと思います.この関数はすべてのパラメータ(内部パラメータ9個+外部パラメータ6個x画像枚数)に対するヤコビアンから近似ヘッシアン(JtJ)を計算し,またすべての画像のすべての検出コーナーに対する誤差ベクトルを生成し,誤差ベクトルとヤコビアンから(Jtex)を計算します.数式,理論的なところは,ホモグラフィー計算の時にもお世話になりましたが,下記のスライドのP18にわかりやすくまとめてくれてあります.

www.slideshare.net

で,入れ子になってしまうので,とっても見難くなるんですが,この関数は下記のとおりです.このエントリでは,Step1,Step3,Step4,Step5 を説明します.

Step 1 : Create approxiomated hessian matrix and error vector.
問題全体(内部パラメータ9個+外部パラメータ6個x画像枚数)に対する近似ヘッシアンとヤコビアン x 誤差ベクトルの領域を確保します.

Step 2 : Convert format.
あとの計算で都合がいいように行列の形とかフォーマットを変換します.

Step 3 : Compute reprojection error and jacobians at projected point.
Step3, Step4, Step5 がおそらく今回のキモですが,Step3でやっていることは大きく2つあります.
①推定内/外部パラメータを用いて,キャリブの3次元点を画像に投影.実際の検知点と比較することで再投影誤差ベクトルを計算します.
②再投影点のヤコビアンを計算します.

Step 4 : Extract jacobians elements of this image.
Step3で求めたヤコビアン,再投影誤差ベクトルは特定の画像に対してのものですが,これを「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」に組み込まないと行けないので,内部パラメータのヤコビアン(J_int)と外部パラメータのヤコビアン(J_ext)に分割します.

Step 5 : Update jacobians and error vector for whole problem.
Step 4で分割した内部パラメータのヤコビアン(J_int)と外部パラメータのヤコビアン(J_ext)を「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」に組み込みます.

Step 6 : Shrink matrix if some parameters are not to be estimated.
最適化対象外のパラメータを行列から削除しないといけないので,設定パラメータに応じて不要な部分を「問題全体に対する近似ヘッシアン」と「問題全体に対するヤコビアン x 誤差ベクトル」から削除します.

該当部コード
void ComputeJacobians(const vector<vector<cv::Point3f>>& objectPoints,
                      const vector<vector<cv::Point2f>>& imagePoints,
                      const IntrinsicParams& param,
                      vector<cv::Vec3d>& board_rotations,
                      vector<cv::Vec3d>& board_translations,
                      const int& check_cond, const double& thresh_cond,
                      Mat& JtJ, Mat& Jtex) {
  int image_num = (int)objectPoints.size();

  // Step 1 : Create approxiomated hessian matrix and error vector.
  // 9 Intrinsic Params + 6 Extrinsic Param * Board Number.
  JtJ = Mat::zeros(9 + 6 * image_num, 9 + 6 * image_num, CV_64FC1);
  Jtex = Mat::zeros(9 + 6 * image_num, 1, CV_64FC1);

  for (int image_idx = 0; image_idx < image_num; ++image_idx) {
    // Step 2 : Convert format.
    cv::Mat object_points_mat, image_points_mat;
    ConvertVector2dToMat(objectPoints[image_idx], imagePoints[image_idx],
                         object_points_mat, image_points_mat);

    // Step 3 : Compute reprojection error and jacobians at projected point.
    Mat jacobians, reproj_err;
    {
      Mat projected_point_mat(cv::Size(2, object_points_mat.rows), CV_64FC1);
      ProjectPoints(object_points_mat, projected_point_mat,
                    Mat(board_rotations[image_idx]),
                    Mat(board_translations[image_idx]), param, jacobians);
      reproj_err = projected_point_mat - image_points_mat;
    }

    // Step 4 : Extract jacobians elements of this image.
    Mat J_int, J_ext;
    {
      J_int = Mat(jacobians.rows, 9, CV_64FC1);
      // fx, fy, cx, cy
      jacobians.colRange(0, 4).copyTo(J_int.colRange(0, 4));
      // alpha
      jacobians.col(14).copyTo(J_int.col(4));
      // k1, k2, k3, k4
      jacobians.colRange(4, 8).copyTo(J_int.colRange(5, 9));
      // r1, r2, r3, t1, t2, t3
      J_ext = jacobians.colRange(8, 14).clone();
    }

    // Step 5 : Update error vector.
    {
      JtJ(Rect(0, 0, 9, 9)) += J_int.t() * J_int;
      JtJ(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = J_ext.t() * J_ext;

      JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)) = J_int.t() * J_ext;
      JtJ(Rect(0, 9 + 6 * image_idx, 9, 6)) =
          JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)).t();

      Jtex.rowRange(0, 9) +=
          J_int.t() * reproj_err.reshape(1, 2 * reproj_err.rows);
      Jtex.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) =
          J_ext.t() * reproj_err.reshape(1, 2 * reproj_err.rows);
    }

    if (check_cond) {
      Mat JJ_kk = J_ext;
      SVD svd(JJ_kk, SVD::NO_UV);
      CV_Assert(svd.w.at<double>(0) / svd.w.at<double>(svd.w.rows - 1) <
                thresh_cond);
    }
  }

  // Step 6 : Shrink matrix if some parameters are not to be estimated.
  std::vector<uchar> idxs(param.isEstimate);
  idxs.insert(idxs.end(), 6 * image_num, 1);
  subMatrix(JtJ, JtJ, idxs, idxs);
  subMatrix(Jtex, Jtex, std::vector<uchar>(1, 1), idxs);
}

2.2 Step 1:問題全体(内部パラメータ9個+外部パラメータ6個x画像枚数)に対する近似ヘッシアンとヤコビアン x 誤差ベクトルの領域を確保

だいぶ見づらいなあ...というのは承知のうえで(笑),説明を続けます.このステップでは必要な行列の領域を確保していますが,まずは再投影誤差の定義です.

f:id:rkoichi2001:20190625120802j:plain
この問題の再投影誤差の定義

上記ノートに書いてあるように,再投影誤差の最小要素は画像点のx座標(もしくはy座標)です.上記のように再投影誤差を設定すると,ヤコビアンは下記のようになります.

f:id:rkoichi2001:20190625123804j:plain
この問題のヤコビアン

ちょっとメモがいっぱいいっぱいになってしまいましたが,この問題のヤコビアンは [2xnxN] X [9+6xN] の巨大行列になります.ここで,nは画像内の検知コーナー点の数,Nは画像の枚数です.で,次に誤差ベクトルは下記のようになります.

f:id:rkoichi2001:20190625124449j:plain
誤差ベクトル

で,ガウスニュートン法で必要なのは「近似ヘッシアン」と「ヤコビアン x 誤差ベクトル」なので,この行列のサイズは下記のようになります.

f:id:rkoichi2001:20190625125256j:plain
近似ヘッシアンとヤコビアンx誤差ベクトルのサイズ

ということで,JtJ のサイズは(9 + 6 x N)x(9 + 6 x N)の正方行列,Jtex のサイズが(9 + 6 x N)x 1 となることがわかりました.このサイズ確保をしているのがこのステップです.

該当コード
// Step 1 : Create approxiomated hessian matrix and error vector.
// 9 Intrinsic Params + 6 Extrinsic Param * Board Number.
JtJ = Mat::zeros(9 + 6 * image_num, 9 + 6 * image_num, CV_64FC1);
Jtex = Mat::zeros(9 + 6 * image_num, 1, CV_64FC1);

2.3 Step 3:次に一枚一枚の画像に対して再投影誤差とヤコビアンを計算します.

で,ここの関数がとっても大きく,ちょっと別エントリにしないと書ききれないので,あとでリンクを貼ります.

2.4 Step 4:Step 3で求めた個々の画像に対するヤコビアンを内部パラメータのヤコビアン,外部パラメータのヤコビアンに分けます.

OpenCVのコードを追っていってわかったんですが,ヤコビアンの行に関する構造体(下記参照)が定義されてまして,

struct JacobianRow {
  Vec2d df, dc;
  Vec4d dk;
  Vec3d dom, dT;
  double dalpha;
};

Step3の関数で得られるヤコビアン行列の並びは下記のようになってます.

f:id:rkoichi2001:20190625151428j:plain
Step3の計算で出てくるヤコビアンの並び

で,上のノートにも書いたんですが,このヤコビアン,α(アスペクト比)がヤコビアンの最後の列に来てしまってます.ここに注意して,これを内部パラメータのヤコビアンと外部パラメータのヤコビアンの2つに分割します.分割している該当のコードが下記です.

// Step 4 : Extract jacobians elements of this image.
Mat J_int, J_ext;
{
  J_int = Mat(jacobians.rows, 9, CV_64FC1);
  // fx, fy, cx, cy
  jacobians.colRange(0, 4).copyTo(J_int.colRange(0, 4));
  // alpha
  jacobians.col(14).copyTo(J_int.col(4));
  // k1, k2, k3, k4
  jacobians.colRange(4, 8).copyTo(J_int.colRange(5, 9));
  // r1, r2, r3, t1, t2, t3
  J_ext = jacobians.colRange(8, 14).clone();
}

で,上記の分割によって1つの画像から下記の2つのヤコビアンが求まりました.

f:id:rkoichi2001:20190625153949j:plain
内部パラメータのヤコビアンと外部パラメータのヤコビアン

2.5 Step 5 : Step4で求めた部分問題に対する2つのヤコビアンを全体のヤコビアンに組み込みます.

ヤコビアンの分割とかせずに,Step3で出てくるヤコビアンを積み上げていけばいいやん?」と最初は思ったんですが,ちゃんと理由がありました.

内部パラメータはすべての画像の再投影誤差に影響を与えるが,外部パラメータはその画像の再投影誤差にしか影響をあたえない!

というのがミソでした.外部パラメータが自分の画像の再投影誤差にしか影響を与えないという事実によって,他の画像の要素に対する微分がすべて0になります.この特徴を活かして事前に行列の形を把握しておくと,Jt * J という巨大な行列の掛け算を避けることができます.このトリックを使わずに問題を解いてみたんですが,同じ解にはなりましたが明らかに実行速度が遅くなりました.で,行列の形を予め把握しておくために,Jt * J の行列構造を書き下して見ます.

f:id:rkoichi2001:20190625162039j:plain
個々画像の内・外部パラメータに関するヤコビアンを使って表した問題全体のヤコビアン

次に,実際に Jt * J を計算したくないので,上述の問題全体のヤコビアンを使って近似ヘッシアン(Jt * J)の構造を書き出してみます.

f:id:rkoichi2001:20190625164333j:plain
近似ヘッシアンの構造

近似ヘッシアンの構造に規則性があることがわかりました.ということで,巨大ヤコビアンの掛け算を避けて,上部ノートの構造に従って近似ヘッシアンを計算しているのが下記の部分です.ちなみに,普通に巨大ヤコビアンの掛け算をしたコードは”ComputeJacobiansNaively”という関数になります.

下記が該当部コードになりますが,近似ヘッシアン以外にも,ヤコビアン * 誤差ベクトルも計算しています.reproj_err が 2 x n の行列なので,これを(2 x n)の 列ベクトルに変換してから掛け算してます.

該当部コード
// Step 5 : Update error vector.
{
  JtJ(Rect(0, 0, 9, 9)) += J_int.t() * J_int;
  JtJ(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = J_ext.t() * J_ext;

  JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)) = J_int.t() * J_ext;
  JtJ(Rect(0, 9 + 6 * image_idx, 9, 6)) =
      JtJ(Rect(9 + 6 * image_idx, 0, 6, 9)).t();

  Jtex.rowRange(0, 9) +=
      J_int.t() * reproj_err.reshape(1, 2 * reproj_err.rows);
  Jtex.rowRange(9 + 6 * image_idx, 9 + 6 * (image_idx + 1)) =
      J_ext.t() * reproj_err.reshape(1, 2 * reproj_err.rows);
}

最後に,上記計算で求まった「近似ヘッシアン」と「ヤコビアン * 誤差ベクトル」は,内部パラメータ,外部パラメータ両方に対する行列になってますが,この問題では内部パラメータのみが最適化の対象なので,行列から不要な箇所を削除します.これを実施しているのが下記の部分です.
(だったら最初からそう計算しとくべきなのでは,,,という気もしないでもないですが,ヤコビアンの全体構造がわかったので良しとしましょう.)

// Step 6 : Shrink matrix if some parameters are not to be estimated.
std::vector<uchar> idxs(param.isEstimate);
idxs.insert(idxs.end(), 6 * image_num, 1);
subMatrix(JtJ, JtJ, idxs, idxs);
subMatrix(Jtex, Jtex, std::vector<uchar>(1, 1), idxs);

2.6 ガウスニュートン法を使った最急降下ベクトルの計算

ということで,やっとComputeJacobiansの外に出ました...近似ヘッシアンが求まったので,ガウスニュートン法を使って最適化対象となるパラメータ空間中での最急降下ベクトルを求めます.

該当部コード

solve(JtJ, -Jtex, d_param);

3.Step3 & Step4 : 最急降下ベクトルを使ったパラメータベクトルの更新と,外部パラメータの再計算.

ここまでくれば,あとは最急降下ベクトルをパラメータベクトルに反映させ,パラメータベクトルの更新前後で変化率が一定以下になった場合に計算を終了します.

該当部コード
// Step 3 :
// Update parameter by d_param * alpha_smooth2. (Gauss Newton 2)
{
  const double alpha_smooth = 0.4;
  double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0);
  IntrinsicParams temp_param = final_param + alpha_smooth2 * d_param;
  updated_ratio = norm(Vec4d(temp_param.f[0], temp_param.f[1],
                             temp_param.c[0], temp_param.c[1]) -
                       Vec4d(final_param.f[0], final_param.f[1],
                             final_param.c[0], final_param.c[1])) /
                  norm(Vec4d(temp_param.f[0], temp_param.f[1],
                             temp_param.c[0], temp_param.c[1]));
  final_param = temp_param;
}

// Step 4 :
// Recomputing extrinsics based on the updated parameter sets.
if (recompute_extrinsic) {
  CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond,
                     thresh_cond, board_rotations, board_translations);
}

ということで,最後はなんかちょっと走り走りになってしまったですが,カメラキャリブレーション時の再投影誤差の最小化でした.

リファクタしたコードは下記のGithubにあげておきます.もちろん本家はOpenCVなので,あくまでも流れをつかむ用途でしか使えないですが...
github.com