カメラの位置・姿勢推定０ エピポーラ幾何
3D空間上の点と投影点の対応，および透視投影行列の導入まで終わりました．ここでは，ステレオカメラやカメラの移動推定に必須のエピポーラ幾何の説明をします．
異なる位置から撮影した写真の変化
下記，首里城を異なる視点から取った写真ですが，下記の写真をパッとみるだけで，人間には「一枚目の写真を撮ったカメラ」と「二枚目の写真を撮ったカメラ」の相対位置が何となくイメージできると思います．つまり，視点が異なっていても同じ要素が写真の中に移っていれば，ある程度カメラの移動を推定できるということになります．
異なる位置から同一物体を撮影した場合に成り立つ幾何学的関係
なんだかとっても仰々しいタイトルになってしまいましたが，「視点を変えても同じものが写っていた時には満たされるべき条件があって，それが何か？」をここでは説明していきます．ちなみに，フライングして説明してしまうと，
「視点を変えても同じものが写っていた時には満たされるべき条件・数式があって，それが何か？」を表すのがエピポーラ幾何です．
下記が，「同一点Pを位置を変えた二つのカメラで撮影した」時のエピポーラ幾何のざっくりイメージです．
ここで，カメラ位置Aから撮影した点Pは投影点Aとしてスクリーンに写っていますが，点Pの位置がはっきりわかっていないと仮定すると，投影点Aに対応する点は直線CP上のどこかに存在します．逆に言うと，直線CP上の点はすべて点Aに投影されます．
上記の投影点Aの位置情報を知っていることによって，カメラ位置BでPを撮影した時の投影点Bの位置に何か情報を与えるでしょうか？答えはYESです．下記ノートを見ていただければわかるように，投影点Aの位置がわかれば，点PのスクリーンB上の投影点は緑色線（エピポーラ線）上のどこかに投影されることがわかります．ここで，三角形PCC´をエピポーラ平面といいます．
エピポーラ幾何が満たす数式
スクリーンA上の投影点Aを知ることで，投影点Bの位置がエピポーラ線上のどこかに来ることを説明しました．ここからは，この拘束条件を数式で表現したいと思います．下記のノートを見ていただければわかるように，カメラ位置C，カメラ位置C’，撮影点Pで定義される三角形CC’Pは一つの平面を構成します．よって，カメラ位置Bからカメラ位置Aへの移動ベクトルtと，カメラ位置Bの座標系から見たカメラ位置Aの視線方向ベクトルの外積を計算すると，三角形CC'Pに垂直なベクトルができます．
これを数式で表現すると，下記のようになります．
上記ノートで外積を歪対称行列で表現しましたが，ベクトル演算だと扱いにくいので，行列に置きなおしただけです．上記数式が，場所の異なるカメラから同一物体を撮影した時に満たされるべき条件です．
基本行列 (Essential Matrix) の導入
満たされるべき数式を導入しましたが，コンピュータビジョンでは上記数式のtとRの部分を一つの行列で置き換えて表現します．これをE (Essential Matrix) と言います．基本行列を用いると，二つの画像における投影点の正規化画像座標 x, x' の関係を次のように簡潔に表現することができるようになります．
基礎行列 (Fundamental Matrix) の導入
基本行列を導入しましたが，これは「正規化画像座標 x, x' の関係」でした．では，もう一歩進んで投影点の画像座標系ではどうでしょうか？正規化画像座標と画像座標の関係は下記のように以前のエントリでふれたように内部パラメータ行列Aを用いて下記のように表されます．
上記の関係を「エピポーラ幾何が満たす数式」に代入すると，下記のようになります．
上記ノートで導入された行列Fのことを基礎行列(Fundamental Matrix)といいます．
基礎行列，基本行列を使って表現された二つの式が導かれましたが，この数式は二つの画像上の投影点の満たすべき拘束条件を表しており，これを エピポーラ拘束といいます．
カメラの位置・姿勢推定０ 同次座標・斉次座標の導入
前回のエントリ「カメラの位置・姿勢推定０ 透視投影モデルと座標系の定義」で若干フライングしてしまいましたが，このエントリで斉次座標（Homogeneous Coordinate）を導入します．前回のエントリから，カメラ座標系から見た三次元の点（X, Y, Z）を正規化画像座標系に投影すると下記のように表されることがわかりました．
上式を見ればわかるように，3Dから2Dに変換する計算をするときに簡単な行列の連鎖式で表現できません．任意の世界座標（Xw, Yw, Zw）の投影点をもとめる式も計算しましたが，計算式が複雑になってくるとこれは面倒極まりないので，投影点を3Dの座標とみなします．3Dの座標とみなした場合，正規化画像座標系ではスクリーンは Z = 1 の場所にあるので，投影点は (x, y, 1) と表現されます．これを表現すると
となります．
斉次座標系の導入
本当は2Dの投影点を（x, y, 1）と表現しましたが，これを斉次座標系表現といいます．どのテキストにも，「計算に便利だから斉次座標系を導入する．」って書かれているんですが，これだとなんだか無味乾燥なので，（x, y, 1）と書かれたら，「原点から距離１の位置に置かれたスクリーンに投影されている点の座標」位に考えればいいのではないでしょうか．
（ここからちょっと自分なり解釈が入ります．間違っているかも．）
で，この斉次座標の特徴なんですが，下記の図を見ていただいてもわかるように，距離１のスクリーンに投影される3Dの点は無限にあるんですよね．要は赤の直線上に乗っている点なら，すべて同じ点に投影されるので．
なので，斉次座標系では３つの要素が実数倍されたものも等しいとみなします．つまり（1, 2, 3）と（2, 4, 6）と（2000, 4000, 6000）は同じものとして扱います．このように扱うことで，「3D空間では異なる場所にあるが，スクリーンに投影すると同じ場所に投影されるもの」を同じものとしてみなすことができます．これを数式的で表現すると．．．
ノートの最後に少し記述してありますが，例えばスクリーンの投影点 x を考えたときに，同じ点に投影される点の集合 λx は同値（equivalent）であるといいます．またこの λ を変えていくと λx はノートに書いている直線になりますが，これを同値類（equivalent class）といいます．
斉次座標系の3D空間への導入
2Dで導入した斉次座標を3Dにも投入します．射影幾何学的にはちゃんとした理由があるんだと思いますが，3Dの説明はいまいち思いつきません．ただ，同じノリで数式を展開するのみ！です．同じように，本来3つの要素で表現できる3D空間上を表現するために4つの要素を使います．
ただ，わかりやすいメリットとして，3次元空間上の点を斉次座標で表現することによって，回転移動・並進移動を行列計算で表現することがてきます．
剛体変換，特殊ユークリッド変換（SE(3)）
ちなみに，この行列で表される移動は剛体変換 (Rigid Body Motion)といいます．なぜかというと，この行列で点群を移動させても異なる二つの点の距離が変わらないからです．別名特殊ユークリッド変換 (Special Euclidean Transformation) SE(3)とも呼ばれ，次のように定義されます．
SE(3) ≡ {g=(R, T) | R ∈ SO(3), T∈ℝ3}
このSE(3)ですが，論文を読んでるとちょこちょこ出てきて，何のことかいな．と思っていたんですが，剛体変換の群のことだったんですね！
斉次座標系を用いた3D空間上の点とスクリーン上の投影点の関係
斉次座標を2D, 3Dの点に導入したので，この結果を使って3D空間上の点とスクリーン上の投影点の関係式を表現します．まず，2D, 3Dの点それぞれの斉次座標表現は下記のようになります．
カメラ座標系における投影点の表現
カメラ座標系から見たワールド座標系の回転要素・並進要素をそれぞれ R, t と置くと，「剛体変換」のところでまとめたように下記のように表現できます．
ワールド座標系からカメラ座標系への変換表現
正規画像座標から画像座標への変換は内部パラメータ行列を用いて下記のように表現できます．
正規画像座標系から画像座標系への変換表現と透視投影行列の導入
「カメラ座標系における投影点の表現」「ワールド座標系からカメラ座標系への変換表現」「正規画像座標系から画像座標系への変換表現」の三つの式を利用して世界座標で表現された点の画像座標への投影点の位置は下記のようにあらわされます．
ノートの最後で透視投影行列Pを導入しましたが，この3x4行列がわかれば，画像上のどこに点が投影されるかわかることになります．
カメラの位置・姿勢推定０ 透視投影モデルと座標系の定義
Eight Point AlgorithmやFive Point Algorithm のエントリを書くにあたり，エピポーラ幾何等の内容も書かないといけないので，まず透視投影モデルとか座標系の定義を簡単にまとめておくことにしました．
透視投影モデル（Perspective Transformation Model）
コンピュータビジョン関係でカメラをモデル化する場合には，大体この「透視投影モデル」が使われています．原理的には簡単で，中学校とか小学校の理科で実験したピンホールカメラと同じようなモデルです．ただ，ピンホールカメラの場合，ピンホールより後ろに撮像面があるので，上下が反転してしまいます．これではめんどくさいので，透視投影モデルでは撮像面をピンホールの前にもってきて実像と投影像の向きが反転しないようにしています．
ピンホールカメラのイメージ
透視投影モデルのイメージ
投影で失われる情報
この書き方が正しいかどうか微妙ですが，当然ながら写真を撮影するということは3Dの世界を2Dの世界に投影するので，情報が欠落します．下記の絵に図示しますが，
「A. 小さいおっさんが近くにいる」
「B. 大きいおっさんが遠くにいる」
上記の二つは3Dの世界ではもちろんはっきりと違いがわかりますが，2Dの世界に落としてしまうと同じように見えてしまいます．人間は「おっさん」のサイズを回りの風景等の情報から無意識に推測しているので何となくサイズ感がわかりますが，おっさん以外なにも写っていない写真を見せられると判断するすべがなくなってしまいます．
画像からは「スケールが一意に決まらない」というこの特徴は，SFMでもSLAMでもVisualOdometryでもたびたび出てきます．
種々の座標系の定義
ここでは，以降で使用する座標系を定義します．
画像座標系
OpenCVとかカメラとかで映像を取ってそれを解析する場合には，画像の左上端や左下端を原点として，そこから (u, v) [pix] の形で画像上の位置を表現すると思います．この座標系を「画像座標系」といいます．
正規化画像座標系
上記の画像座標系をもう少し解析しやすくするために調整した座標を「正規化画像座標系」といいます．この座標系では光学中心から撮像面に下した垂線と撮像面との交点が原点になります．また，簡単のために焦点距離 f は１とします．
正規化画像座標と画像座標の関係
カメラ座標系で見た点（X, Y, Z）の投影点を正規化画像座標と画像座標で考えることで，正規化画像座標と画像座標の関係を導きます．
上記ノートの最後に出てきた A はカメラの内部パラメータといって「焦点距離」「光学中心」等々カメラとレンズに固有の値です．本当は Skew とかを考慮するともう少し変数が増えますが，大体はこの簡略化した形で十分かと思います．
ワールド座標と正規化画像座標・画像座標との関係
で，ここまで準備が整うと，次に知りたくなるのは
「ワールド座標系で（Xw, Yw, Zw）と表されている点は，画像のどこに移るのか？」
という点かと思います．これを計算します．
上記ノートの最後の式ですが，正規化画像座標での投影点 (x, y) が世界座標系から見た点座標 (Xw, Yw, Zw) から表現されたので，ワールド座標系と画像座標がつながりました．実際のカメラの投影点を計算するには内部パラメータ行列が必要ですので，下記のようになります．
ここまでで，
「ワールド座標系で（Xw, Yw, Zw）と表されている点は，画像のどこに移るのか？」
がわかりました．次は計算式がすっきりするように，斉次座標系を導入します．
カメラの位置・姿勢推定２ PNP問題 理論編
引き続きカメラの位置・姿勢推定問題です．PNP問題の解き方をまとめます．
問題分類のエントリで書いたように，下記を前提とします．
前提条件１：カメラの内部パラメータがわかっている．
前提条件２：基準となる座標系（ワールド座標系）が定義されており，撮影する物体（の各点）のワールド座標系での位置が事前に分かっている．
問題設定
解こうとしている問題のイメージ
上記の前提を図示してみると，下記のようなイメージかと思います．ここで，Xiは各点のワールド座標系での位置，xiはその各点をカメラで撮影した時の画像座標系での投影点です．
家の頂点五点が赤く塗られていますが，この頂点のワールド座標系での位置が分かっているとします．この時にカメラで撮った家の写真をみて，「この写真をとったカメラの位置は？」を求めるのがこの問題です．ちなみに，上記の絵に記載されている「R, t」がカメラの位置になります．
アルゴリズムのINとOUT
アルゴリズムに対するINとOUTは下記になります．
ちなみに，ノートに赤字で書いていますが，カメラ姿勢が自由度６（並進３と回転３）なので，３点（x と y のペア）が既知ならば，不定性は残るものの原理的にはカメラの位置が求まります．これをP3P問題といいます．P3P問題を解くアルゴリズムは Three Point Algorithm と呼ばれているようです．6点あれば線形の方程式を解く問題に帰着できてこれをP6P問題といいます．
P3P問題は非線形方程式を解く必要があり複雑なので，ここではP6P問題の解き方を説明していきます．今回の例のように６点を用いてDLT法で方程式を解いた場合，変数の消去をしなくてもいいので比較的簡単に解けます．
問題解決までの流れ
実際にカメラの位置を求めるには，大きく分けて二つのステップを経る必要があります．
１．DLT法を用いて透視投影行列を求める．
２．透視投影行列を R, t に分解する．
１．DLT法を用いて透視投影行列を求める．
方程式
解くべき方程式は，斉次座標表現を用いて下記のようにあらわされます．
ここで，上式は斉次座標表現を用いて表されており，「定数倍を許して等しいとみなす」のでした．しかしながら，カメラの位置はこの「定数倍」もはっきりさせないと一意に決めることができません．そのため，方程式を解くためにもう一つの変数も導入します．
この問題設定では，上記の方程式で既知の値と未知の値は下記のようになります．
既知の値：
「ワールド座標系での点座標（Xi, Yi, Zi, 1）」
「正規化画像座標系での投影点座標（xi, yi, zi, 1）」
未知の値：
「透視投影行列（カメラ行列）の各要素12個（pij）」
「新たに導入した定数倍ファクター λi」
透視投影行列は12個の要素を持ちますが，実際には定数倍を許して等しいとみなすので，自由度は11です．また，点が一つ増えることに定数倍ファクターのλiが一つ増えていきます．つまり，未知数と方程式の関係は下記のようになります．
既知の点ペア１つがもたらす線形方程式の個数：3
既知の点ペア１つがもたらす未知数の個数：1
透視投影行列の未知数：11
上記より，下記の条件が満たされれば線形方程式が解けます．
3N ≧ 11 + N
これを解くと，N ≧ 6となって，結果的に６点の対応関係がわかればカメラの位置が求まることになります．
DLT法を適用するための方程式展開
下記，方程式を展開して，DLT法が適用できるように Mx=b の形にします．下記のノートを見ていただければわかるように，この方程式では右辺が 0 になります．このような方程式を同次連立一次方程式（Homogeneous Linear Equations）といいます．
上式が１つの点が与える方程式です．この方程式が６つあれば，問題が解けることになります．N点が与えられた時の上記方程式を表すと，ベクトル表現と用いて下記のように表現できます．
だんだん数式が複雑になってきました．．．．ここで，上記方程式の v を求めれば，透視投影行列が求まることになります．この数式は v = 0 が一つの解となることは自明ですが，明らかにこの解には興味がないので， v = 0 以外で上記の方程式を探します．つまり，v = 0 以外で，Mv が最小となる v を求めればよいことになります．また，透視投影行列には定数倍の不定性があり，要素は12個あるけれども，実際の自由度は11でした．このことより ||v|| = 1 という拘束条件をつけ足してあげると，この問題は下記のように表現できます．
係数行列 M を直行行列 U, V と対角行列 S を用いて表せました．ここで，問題は Mv = 0 (＝|| Mv || がもっとも小さくなる v) となるような v を見つけることでしたので，特異値分解した行列 S から最小固有値を探せば，それに対応する固有ベクトルが v となります．
（このコラムでは特異値分解はちょっと触れられませんが，また別の機会にエントリを書きます．）
ふー．ここまでの計算で，透視投影がやっと求まりました．ただ，ここから R, t を求めなければ行けないので，もう一仕事です．
２．透視投影行列を R, t に分解する．
以前のエントリで説明しましたたが，透視投影行列と内部パラメータ行列，外部パラメータ行列の関係は下記のように表現されます．
ここまでの手順で，透視投影行列 P は求まっているので，P を分解して R, t の行列・ベクトルを求めることができれば，カメラの位置が分かったことになります．ここで，内部パラメータ行列 K が上三角行列であること，Rが回転行列であることを利用して RQ分解を使って P を分解します．
ここで，行列 A は透視投影行列 P を構成する 3x3 の行列であり，１のステップで既知です．なので，上記ノートの最後の行列式は R3 から順番に解いていくことができます．
次にR2を解きます．
最後にR1を解きます．
ここまでで，回転行列 R が求まりました．ここで，内部パラメータ行列 K の要素 (3, 3) は定義からは 1 となるはずですが，この手順のどこでもその条件を入れていないので， K の要素すべてを (3, 3) 要素で割ってあげれば内部パラメータ行列になります．t に関しては，内部パラメータ行列 K と並進ベクトル t からなる線形の方程式を解いてあげれば求まるかと思います．ということで，PNP問題の DLT 法を用いた解法でした．
カメラの位置・姿勢推定１ 問題分類
三次元復元をすべく，カメラ・幾何学周りを勉強してますが，どうもやっぱりピンとこず．．．ひとまず，理解したことをまとめておくことにしました．
０．はじめに
「撮影した画像からカメラの位置・姿勢を推定する」というという問題を考えます．ここで，事前に分かっていることによって複数のケースに分かれます．
１．一枚の写真からカメラの位置を求める．
必要条件１：事前に撮影している物体（対応点）のワールド座標がわかっている．
必要条件２：カメラの内部パラメータがわかっている．
問題設定としては，下記のようになるかと思います．
上図の状況としては，まずカメラで撮影した家の各点（赤の点）のワールド座標位置が事前に分かっています．で，この「事前に位置の分かっている点」を「位置の分からないカメラ」で撮影したときに，「カメラの位置」を求める．という問題になります．この問題はPNP問題 (Perspective N Point Problem) と呼ばれます．
ー＞PNP問題
２．複数（ここでは二枚の画像）の画像からカメラの位置を求める．
必要条件１：複数の画像で，それぞれの対応点がわかっている．（例：一枚目の写真で写っている”鼻”の位置が，二枚目の写真のどこに移っているかわかっている．）
必要条件２：カメラの内部パラメータがわかっている．
問題設定としては，下記のようになるかと思います．
上図の状況としては，家の写真を１の場所で撮影し，次に家の写真を２の場所で撮影しています．二枚の写真に写った家の各店（赤の点）の対応関係がわかっているときに，「１から２へのカメラの姿勢変化・移動量」を求めます．なお，後述する理由から，この問題設定では「回転」「移動方向」までは決まるものの，実際に移動した距離までは求まりません．この問題は 5点問題 (Five Point Problem) ，8点問題 (Eight Point Problem) と呼ばれます．
ー＞5点問題，8点問題
次のエントリで一つずつ追いかけていきます．
難問解決
※三次元復元をするにあたって射影幾何学を勉強してるんですが，なかなかしわの少ないマイブレインでは理解に時間がかかっており，ブログを書くネタがないので今回はこんなあほな感じのエントリです（笑）
2017年度年末に，京都大学の望月教授が2012年に出していたABC予想の証明が，他の数学者の査読・チェックが終わり証明が正しいという結論になった．というニュースが出ました．（査読・チェックに５年かかるというあたり素人には驚きですが．）この日本人の活躍に触発されて，自分も難問を解いてみることにしました．
ちなみに，望月教授が証明した「ABC予想」は下記です．
「互いに素(1以外に共通の約数をもたない)である3つの自然数(正の整数)の組(a、b、c)について考える。a、bの和がcである(a+b=cを満たす)とき、積abcの互いに異なる素因数の積をdとする。任意の実数κ> 1のとき、dをκ乗(累乗)した積は、まれにcよりも小さくなる。すなわち、そのような数の組(a、b、c)は有限個だけしか存在しない」
僕が証明した「牛角食べ放題のほうがお得予想」は，下記です．
「30代前半男性が空腹状態で牛角に食事に行く場合，単品注文より食べ放題を選択したほうがコストパフォーマンスが高い．」
ということで，証明しました．
まず，前提となる食べ放題のコースは下記です．
・牛角コース＋飲み放題＝4960円
土曜日の夜19時に牛角に殴り込み，下記を食べきりました．
・カルビステーキ x 1 = 690円
・厚切りカルビタレ x 4 = 2200円
・ロースタレ x 4 = 2360円
・ロースステーキ x 1 = 890円
・いぶりがっこクリームチーズ x 2 = 780円
・キムチ x 1 = 290円
・炙りベーコン x 2 = 780円
・黒糖おさつバター x 2 = 780円
・チョレギサラダ x 1 = 590円
・ごはん中 x 1 = 260円
・ガトーショコラ x 1 = 250円
・アサヒスーパードライ x 5 = 2500円
・赤ワイングラス x 1 = 390円
写真なし．．
合計 12760円 ．．．．証明完
（但し，合コン等目的が異なる場合はこの限りではない．）
ということで，命題「30代前半男性が空腹状態で牛角に食事に行く場合，単品注文より食べ放題を選択したほうがコストパフォーマンスが高い．」を証明しました．証明に当たって，工夫したところは，，，うーんそうですね．白米の消費量を抑えたところでしょうか．
ということで，皆様も牛角に行ったときはぜひ食べ放題を選択してください．
（望月先生，しょーもないエントリに紐づけてしまいごめんなさいm(_ _)m）
沖縄一人旅 ３日目
3日目
あっというまの３泊４日でしたが，ついに最終日です．最終日の飛行機が１２時前だったので，結局観光というよりは国際通りの写真を撮ることで時間いっぱい使ってしまいました．三次元復元するには，いろんな視点の写真を集めないといけないのですが，国際通りが結構長く（約１．６キロ）朝7時過ぎから取り始めて，ほぼ三時間かかってしまいました．
ということで，三次元復元ができたらまたアップします．
ガチンコ一人旅を終えて
久々のガチンコ一人旅だったのですが，記念館とか，いろいろ見て回る分には一人旅のほうが融通も利くしいいかなあと．ただ，ご飯＆ナイトライフ（笑）は友達がいたほうがやっぱりもりあがりますね．あと，一人旅だと副産物として写真を取る量が俄然増えます（笑）．でもなんだかんだで満足度は高かったので，一年に一回くらいはまたどこかに行ければと思います．また旅行した時はブログに上げるので，見てくださいませ！ということで，３日目はなんかスカスカになってしまいましたが，以上，沖縄一人旅でした．
