魚眼カメラのキャリブレーション 〜 実装編０（全体構成）

前回までの理論編に引き続き，実装編に入っていきます．

ちなみに，「沖縄三次元復元プロジェクト！」をやっていく中で，オープンソースのSFMソフトをいろいろと試して感じてるんですが，やっぱり人のコード読まないとダメですね．．．「今更なにいってんの？」という話ではあると思うんですが，どうしても論文とか本とかを読んで「なんとなく雰囲気がわかった状態」で終えてしまうことがいままで多かったんですが，これだと本質的なとこが理解できてないですね．．．ということもあって，OpenCVのコードとかも今後読んでいかないといけないなと思い，まずは手始めにキャリブレーションのコードに突撃してみました．やったこととしては，

１．実装の元になった論文を読む．
２．結果が大体同じことを確認しながら，OpenCVのコードをリファクタ．
３．実装でわからないとこにぶつかったら，ググる．論文読む．

というステップでやりました．一通りやっていく中で，

・最適化（最急降下法）
・ホモグラフィー行列の計算
・ホモグラフィー行列最適化（ガウスニュートン法）
・主成分分析を使った点群の正規化
・再投影誤差の最小化（ガウスニュートン法）
・ヤコビアンの計算方法
．．．

と無数のアイテムに出会いました．特に最適化なんかはいまいちしっくり来てなくて困ってたんですが，生きたコンテキストで勉強したことでちょっとは理解が深まったと思います．今後もこのやり方を続けて行きたいんですが，これ，恐ろしく時間がかかるんですよね．．．というのと，大きな関数に対してこれをするのはなかなか大変です．．．まあ，あせらずちょっとづつですかね．

ということで，ここから本題です．

０．全体構成

今回は，OpenCVのcv::fisheye::calibrateの関数をそのままパクってリファクタしたんですが，論文の構成にできるだけ近づくようにリファクタしました．大きな関数のフローとしては，下記のようになります．（ちなみに，OpenCVはvectorでもMatでもできるだけ動くようにIFが抽象化されてますが，このコードがなかなか理解を妨げたので，IFを簡素化してます．）

１．InitializeParamsBasedOnFlagSetting

ユーザから指定されたフラグを元に，キャリブの初期値を決定します．ここはそんなに大したことはやってないかと思います．

２．CalibrateExtrinsics

入力された画像上の点とキャリブボード上の点の対応関係から，下記を実施します．
①DLTを使ってホモグラフィー行列を求める．
②①で得た初期値を元に，ガウスニュートン法でホモグラフィー行列の精度を向上させる．
③キャリブボード上の点のZ座標が０であることを利用して，ホモグラフィー行列から外部パラメータを計算．

３．MinimizeReprojectionError

①ヤコビアンを計算し，ガウスニュートン法を使ってパラメータの更新ステップを計算．
②パラメータの更新ステップをパラメータに反映
③パラメータの更新率がしきい値を下回ったら終了．

４．EstimateUncertainties

①最終決定した内部・外部パラメータを使ってボード上の点を画像に投影．
②①の投影点と実際に撮影されている点を比較して，再投影誤差を計算．

５．ConvertFormat

出力のフォーマットに合わせるための関数です．ここはそんなに大したことはやってないかと思います．

該当部コード

double CalibrateFisheye(
    const std::vector<std::vector<cv::Point3f>>& objectPoints,
    const std::vector<std::vector<cv::Point2f>>& imagePoints,
    const cv::Size& image_size, const int flags,
    const cv::TermCriteria criteria, cv::Matx33d& K, std::vector<double>& D,
    std::vector<cv::Vec3d>& board_rotations,
    std::vector<cv::Vec3d>& board_translations) {
  IntrinsicParams final_param;

  // Step 1. Initailization of the parameter
  int check_cond, recompute_extrinsic;
  InitializeParamsBasedOnFlagSetting(flags, K, D, image_size, final_param,
                                     check_cond, recompute_extrinsic);

  // Step 2. Calculate Homography and Extrinsics
  const double thresh_cond = 1e6;
  CalibrateExtrinsics(objectPoints, imagePoints, final_param, check_cond,
                      thresh_cond, board_rotations, board_translations);

  // Step 3. Minimize Reprojection Error.
  cv::Vec2d err_std;
  MinimizeReprojectionError(criteria, objectPoints, imagePoints, check_cond,
                            thresh_cond, recompute_extrinsic, final_param,
                            board_rotations, board_translations);

  // Step 4. Calib Result Summary.
  double rms;
  EstimateUncertainties(objectPoints, imagePoints, final_param, board_rotations,
                        board_translations, err_std, rms);

  // Step 5. Format Conversion.
  ConvertFormat(final_param, K, D);

  return rms;
}

ということで，次のエントリでは "CalibrateExtrinsics" の中身を見ていきます．

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