Essential Matrix (基本行列) と R, T の復元

SfM のエントリ作成にあたって，せっかくなので構成要素となる小さなアルゴリズムをまとめています．このエントリでは，２つの項目を書き留めておきます．

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

１．Essential Matrix (基本行列) とは何か？
２．Essential Matrix がわかった時に，それからカメラモーション (R, T) を取り出す方法．

１．Essential Matrix (基本行列) とは何か？

それでは，まず基本行列の説明です．かなり前にエピポーラ幾何を説明する下記のエントリを書いたんですが，その中で Essential Matrix (基本行列) に触れていました．

daily-tech.hatenablog.com

で，このエントリの中で，

「視点を変えても同じものが写っていた時には満たされるべき条件・数式があって，それが何か？」を表すのがエピポーラ幾何です．

という記述をしています．で，この表現から一歩進んで数式に落とすべく下記の図を考えてみます．これ以降，このエントリでは正規化画像座標系で考えています．

f:id:rkoichi2001:20190714120711j:plain
エピポーラ幾何からの関係式の生成

図中に記入しましたが，

１．２つのカメラの視線ベクトルをどちらかの座標系で表現する．
２．一方の視線ベクトルとカメラの並進ベクトルが作る平面の法線ベクトルを計算する．
３．他方の視線ベクトルと２で計算した法線ベクトルが直行する．

という３つの条件から，最後の数式が出てきます．ただ，この数式のままだとベクトルの外積とかが入ってきててややこしいです．なので，" t X R " の部分をもう少し扱いやすくするために行列演算で置き換えます．下記がその過程と結果です．

f:id:rkoichi2001:20190714122629j:plain
ベクトル外積演算の行列演算への置き換え

上記の結果から，ベクトルの外積演算が行列の積演算に置き換わることがわかりました．行列積への置き換えを使って，エピポーラ幾何から生成した数式を書き換えると，下記のようになります．

f:id:rkoichi2001:20190714123715j:plain
エピポーラ幾何関係式の行列積を用いた置き換え

で，上図の最後に置き換え結果を記しましたが，この E が Essential Matrix (基本行列) になります．置き換えの過程を見てもわかる通り， E は２つのカメラの位置関係 R, T にのみ依存しています．

２．Essential Matrix がわかった時に，それからカメラモーション (R, T) を取り出す方法．

で，散々回りくどくEssential Matrix (基本行列) の説明とか式導出をしてきましたが，「で，Essential Matrix が求まってなにが嬉しいの？」 という質問にはまだ答えられてませんでした．ミソとなるのは，１の項の最後に書いた「E は２つのカメラの位置関係 R, T にのみ依存しています．」という部分で，画像の対応点を見つけて計算しE がわかれば，

「２つのカメラの位置関係（R, T）がわかる！」

ということになります．２の項では，Eがわかった時に，

「Eからどうやって R と T を計算するか」

という点を見ていきます．

２．１．Essential Matrix の特徴・特性

でEssential Matrixの特徴ですが，全部で３つの特徴があります．うち２つは先ほどの導出からわかります．

f:id:rkoichi2001:20190714133913j:plain
Essential Matrix の特徴・特性

で，３つめがちょっとわかりにくいのですが，「０でない２つの特異値の値が同じ」というものがあります．

f:id:rkoichi2001:20190714141320j:plain
Essential Matrixの 0 でない２つの特異値が同じになる

上記の証明では，歪対称行列を特異値分解すると，下記の形になることを使っています．（が，ここをなんでやねん？するのは諦めました（笑））

f:id:rkoichi2001:20190714141958j:plain
３ｘ３の歪対称行列の特異値分解

で，歪対称行列が上記のように特異値分解できることとがわかったので，あとはここから「０でない２つの特異値が同じで，残りの特異値は０である」ことを示すために，直行行列Wを使って使ってZを変換してあげています．この変換を経て E = U diag(1, 1, 0) W UTRと表すことができて，「０でない２つの特異値の値が同じ」であることがわかりました．

２．２．Essential Matrix からの R, T の計算

２．１ではEssential Matrix を特異値分解すると，E = U diag(1, 1, 0) VT となることがわかりました．

今度は
E が与えられた時に，Eを特異値分解してE = U diag(1, 1, 0) VTが求まったとした時に，そこからどうやって R, T を計算するか？
が問題になります．

ただ，先ほどの証明の過程で出てきた数式を使えば，
E = U diag(1, 1, 0) VT = U diag(1, 1, 0) W UTR
[t]x = UZUT

となることがわかっているので，結果的に求まる解は

１．[t]x = UZUT, -UZUT（もしくは，Uの３つめの列ベクトルでも同じ．）

２．R = UWTVT, UWVT

となります．ここで，Tに関してもRに関しても２つ解がでてきます．これは，特異値分解の箇所で符号が定まらないことが原因で（Rの場合はWとWTの場合で符号が反転します．），どの組み合わせを用いてもここまでの議論が成立します．そのため，最終的に求まるカメラ行列は４つになります．「この４候補の中から正しい解を選択する」というロジックが最後に必要になります．

f:id:rkoichi2001:20190714150602j:plain
E行列を分解してできる４つのカメラ行列

２．３．Essential Matrix を分解するコードサンプル

下記，別のエントリで出てくる SfM のコードの一部ですが，実際に基本行列を分解して R, T を求めています．分解した結果として，２つづつの並進ベクトル，回転行列を呼び出し元に返しています．

bool decompose_E_to_R_T_internal(
      const cv::Matx33d& E,
      cv::Matx33d& R1,
      cv::Matx33d& R2,
      cv::Matx31d& T1,
      cv::Matx31d& T2) {

  // Use OpenCV SVD.
  cv::SVD svd(E, cv::SVD::MODIFY_A);

  // Result check.
  double singular_value_ratio = 
      std::abs(svd.w.at<double>(0) / svd.w.at<double>(1));
  // If two singular values are too far apart, this caluclation fails.
  if (singular_value_ratio < 0.7 || 1.5 < singular_value_ratio) { 
    return false;
  }

  cv::Matx33d  W(0,-1,0,1,0,0,0,0,1);
  R1 = cv::Mat(svd.u * cv::Mat(W) * svd.vt);
  T1 = cv::Mat(svd.u.col(2));

  cv::Matx33d Wt(0,1,0,-1,0,0,0,0,1);
  R2 = cv::Mat(svd.u * cv::Mat(Wt) * svd.vt);
  T2 = cv::Mat(-svd.u.col(2));
  return true;
}