Triangulation (三角測量) による空間中の点の計算

引き続き SfM の構成要素となる小さなアルゴリズムをまとめておきます．

０．このエントリで扱う内容

１．Triangulation とは何か？

２．Triangulation して空間中の点の座標を計算する方法．

１．Triangulation とは何か？

１．１．Triangulation (三角測量) の概要

まず，Triangulation とは何か？って話ですが，これは三角測量のことです．
ja.wikipedia.org

上記のWikipediaのページでは下記のように定義されてました．

三角測量（さんかくそくりょう）は、ある基線の両端にある既知の点から測定したい点への角度をそれぞれ測定することによって、その点の位置を決定する三角法および幾何学を用いた測量方法である。その点までの距離を直接測る三辺測量と対比される。既知の1辺と2か所の角度から、三角形の3番目の頂点として測定点を決定することができる。

定義からだとなかなかイメージがわかないので，簡単に図にしてみます．

f:id:rkoichi2001:20190715144941j:plain — 三角測量のイメージ図

　上図，絵心がないのはいつものことですが，浜に沿って航行する船に対して大砲を射つケースを考えて見ました．大砲を船に当てるためには大砲と船の距離 d を求める必要がありますが，大砲から船まで距離を直接図るわけには行きません．ここでは代わりに２地点間の距離 ”baseline” とそれぞれの地点 (A, B) から船への直線と baseline とのなす角 (α, β) を求めることによって三角関数を使って大砲と船の距離 d が求まることがわかります．今でも工事現場周辺に行くと，三脚のついた黄色いカメラを持ったおじさんがいると思うんですが，このおじさんたちは測量士と呼ばれる人で実際に上述の三角測量をしています．「大砲と船の距離」とか「広大な工事現場の二点間の距離」とか，実際に直線的に距離を測定するのが難しいケースでは今でもガンガンつかわれているんですね．

１．２．SfM と三角測量

で，この三角測量ですが，SfM でも全く同じように使います．

f:id:rkoichi2001:20190715182507j:plain — SfMとTriangulation

上図を見ていただくとわかる通り，「大砲と船の距離を求める問題」と同じであることがわかります．SfM で Triangulation を実施する場合，基本的には下記が前提条件となります．

既知である必要があるもの

・カメラ位置Aの内部パラメータ K, 回転 R，位置 T
・カメラ位置Bの内部パラメータ K', 回転 R'，位置 T'
・三次元点Pの２つの画像中での位置 (x, x')

求めたいもの

・三次元点Pの位置座標 X

ということで，これを次の項で理論展開していきます．

２．Triangulation して空間中の点の座標を計算する方法．

２．０．参考文献

R. I. Hartley and P. Sturm, Triangulation, Comput. Vision Image Understand., 68-2 (1997-11), 146–157.

２．１．満たすべき方程式

まず，Pの位置座標を X，それぞれの正規化画像中の座標を x, x' とすると次の式が成り立ちます．

f:id:rkoichi2001:20190715182822j:plain — 射影変換によるスクリーンへの投影

上記射影変換の式をもう少し展開すると，下記のようになります．

f:id:rkoichi2001:20190715183129j:plain — 射影変換式からの線形方程式作成１

で，Pの中身を書き出してもう少し突っ込んで式展開すると，最終的に２つの画像点対応点から求まる４つの線形方程式ができます．

f:id:rkoichi2001:20190715164436j:plain — 射影変換式からの線形方程式作成２

最終的に求まった４つの方程式ですが，「１．固有ベクトルを使って解く」か，「２．最小二乗法を使って解く」かで２つに別れます．まず，固有ベクトルを使って解く方法ですが，下記のような流れになります．

f:id:rkoichi2001:20190715170732j:plain — 固有ベクトルを使った Triangulation の解法

このやり方ではいつものように Ax = 0 となるように行列式を整理し，この行列に対して最小固有ベクトルを計算してこれを解とします．

もうひとつの「２．最小二乗法を使って解く」では，解の形を X = [X, Y, Z, 1]^T とし，変数 < 方程式数とすることによって最小二乗法を使ってときます．

f:id:rkoichi2001:20190715171834j:plain — 最小二乗法を用いた Triangulation の解法

２．２．反復計算による解の高精度化

上記２．１の解法で Triangulation ができるのですが，文献とコードを読んでみるともう少し精度を上げる方法があるみたいだったので，これも書き留めておきます．ちなみに，このエントリで書いている方法はベストな方法ではないです．ベストな方法は上記 Triangulation に乗っている６次式を使う方法とか，最適補正という手法がベストな方法みたいです．が，ここではあんまり深くに入って行くと戻ってこれないので（笑），ある程度精度が出て，理解もできる方法でとどめておきます．

重み係数の適用

射影変換式を変形することによって，１つの画像点に対して下記の通り２つの線形方程式ができることがわかりました．
up3^TX = p1^TX
vp3^TX = p2^TX

この式を引き算の形に直すと，以下のようになることがわかります．
up3^TX - p1^TX = 0
vp3^TX - p2^TX = 0

で，理論上は上の等式が成り立つわけですが，実際には計測誤差等が含まれるので右辺は完全に 0 にはなりません．
up3^TX - p1^TX = ε1
vp3^TX - p2^TX = ε2

ここで，常識の ε1, ε2 が最小二乗法や固有ベクトルの計算を通して最小化しようとしている値ですが，実際ここで最小化したいのは ε ではなく，「求まった３次元点を再投影した値と，画像上で検出した対応点の差（再投影誤差）」が最小化したいものです．もう一度，式展開をしてみます．

f:id:rkoichi2001:20190715175314j:plain — 線形方程式への重み係数導入

上図の最後に「本当に最小化したい誤差」を表現した４つの方程式を書きましたが，分母に求めるべき変数の X が来てしまっているので線形方程式として解くことができません．こうなるとお手上げなので，ここでは前回に求まった X からp3^TXを計算し，この値で方程式全体を割ります．

アルゴリズムの流れ

下記にアルゴリズムの流れを書きましたが，「１．重みで方程式全体を割る．」，「２．割った方程式で Triangulation をする．」「３．求まったXで重み更新」という流れを繰り返します．文献によると，多く見積もっても１０回程度で反復計算は収束するとのことでした．

f:id:rkoichi2001:20190715181139j:plain — 重み係数を用いた Triangulation の流れ

該当部コード

ということで，該当部分のコードです．最小二乗法で計算しています．

cv::Matx41d IterativeLinearLSTriangulation(const cv::Point3d& norm_pnt1,
                                           const cv::Matx34d& P1,
                                           const cv::Point3d& norm_pnt2,
                                           const cv::Matx34d& P2) {
  // Do once for initial value.
  double w1(1.0), w2(1.0);
  cv::Matx43d A;
  cv::Matx41d B, X;

  BuildInhomogeneousEqnSystemForTriangulation(norm_pnt1, P1, norm_pnt2, P2, w1,
                                              w2, A, B);

  SolveLinearEqn(A, B, X);

  // Iteratively refine triangulation.
  for (int i = 0; i < 10; i++) {
    // Calculate weight.
    double p2x1 = (P1.row(2) * X)(0);
    double p2x2 = (P2.row(2) * X)(0);

    if (std::abs(w1 - p2x1) < TRI_ITERATIVE_TERM &&
        std::abs(w2 - p2x2) < TRI_ITERATIVE_TERM) {
      break;
    }

    w1 = p2x1;
    w2 = p2x2;

    BuildInhomogeneousEqnSystemForTriangulation(norm_pnt1, P1, norm_pnt2, P2,
                                                w1, w2, A, B);

    SolveLinearEqn(A, B, X);
  }

  return X;
}


void BuildInhomogeneousEqnSystemForTriangulation(
    const cv::Point3d& norm_p1, const cv::Matx34d& P1,
    const cv::Point3d& norm_p2, const cv::Matx34d& P2, double w1, double w2,
    cv::Matx43d& A, cv::Matx41d& B) {
  cv::Matx43d A_((norm_p1.x * P1(2, 0) - P1(0, 0)) / w1,
                 (norm_p1.x * P1(2, 1) - P1(0, 1)) / w1,
                 (norm_p1.x * P1(2, 2) - P1(0, 2)) / w1,
                 (norm_p1.y * P1(2, 0) - P1(1, 0)) / w1,
                 (norm_p1.y * P1(2, 1) - P1(1, 1)) / w1,
                 (norm_p1.y * P1(2, 2) - P1(1, 2)) / w1,
                 (norm_p2.x * P2(2, 0) - P2(0, 0)) / w2,
                 (norm_p2.x * P2(2, 1) - P2(0, 1)) / w2,
                 (norm_p2.x * P2(2, 2) - P2(0, 2)) / w2,
                 (norm_p2.y * P2(2, 0) - P2(1, 0)) / w2,
                 (norm_p2.y * P2(2, 1) - P2(1, 1)) / w2,
                 (norm_p2.y * P2(2, 2) - P2(1, 2)) / w2);

  cv::Matx41d B_(-(norm_p1.x * P1(2, 3) - P1(0, 3)) / w1,
                 -(norm_p1.y * P1(2, 3) - P1(1, 3)) / w1,
                 -(norm_p2.x * P2(2, 3) - P2(0, 3)) / w2,
                 -(norm_p2.y * P2(2, 3) - P2(1, 3)) / w2);

  A = A_;
  B = B_;
}


void SolveLinearEqn(const cv::Matx43d& A, const cv::Matx41d& B,
                    cv::Matx41d& X) {
  cv::Matx31d tmp_X;
  tmp_X(0) = X(0);
  tmp_X(1) = X(1);
  tmp_X(2) = X(2);
  cv::solve(A, B, tmp_X, cv::DECOMP_SVD);
  X(0) = tmp_X(0);
  X(1) = tmp_X(1);
  X(2) = tmp_X(2);
  X(3) = 1.0;
}