２つの画像の対応点からから３次元上の点を求める

理論

ある３次元上の点 $\boldsymbol{X}=(X,Y,Z,1)$ をカメラ内部パラメータ行列 $A$ とカメラ外部パラメータ行列 $[R|t]$ を用いて画像上に投影した点を $\boldsymbol{x}=(x,y,1)$ とすると、以下の関係が成り立ちます[1]。
${ s\boldsymbol{x}= A[R|t] \boldsymbol{X} \tag{1} }$
または、
${ \displaystyle s \begin{pmatrix} x_{1} \\ y_{1} \\ 1 \\ \end{pmatrix} = \begin{pmatrix} f_{x} && 0 && c_{x} \\ 0 && f_{y} && c_{y} \\ 0 && 0 && 1 \\ \end{pmatrix} \begin{pmatrix} r_{11} && r_{12 }&& r_{13} && t_{1}\\ r_{21} && r_{22 }&& r_{23} && t_{2}\\ r_{31} && r_{32 }&& r_{33} && t_{3}\\ \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ 1 \\ \end{pmatrix} \tag{2} }$
$s$ はスケール調整の定数です。カメラ内部行列の $f_{x}$ , $f_{y}$ はそれぞれピクセル単位の焦点距離、 $c_{x}$ , $c_{y}$ は画像中心を表します。カメラの固有パラメータという感じです。カメラ外部行列はカメラ姿勢を表し、３次元物体の座標をカメラ座標に変換する役割を持ちます。

同じカメラで別の視点から $\boldsymbol{X}$ を投影した画像が２枚（画像１、画像２）あり、それぞれのカメラ内部・外部行列を合わせたものを $M_{1}=A[R|t]_{1}$ , $M_{2}=A[R|t] _{2}$ 、画像上の投影点を $x_{1}$ , $x_{2}$ とすると、それぞれに対して(1)を適応して、

${ s\boldsymbol{x}_{1}= M_{1} \boldsymbol{X} \tag{3} }$

${ s\boldsymbol{x}_{2}= M_{2} \boldsymbol{X} \tag{4} }$

と表せます。
これを変形すると、

${ \begin{pmatrix} M_{1} && -\boldsymbol{x}_{1}&& 0\\ M_{1} && 0 && -\boldsymbol{x}_{2}\\ \end{pmatrix} \begin{pmatrix} \boldsymbol{X} \\ s_{1} \\ s_{2} \\ \end{pmatrix} = \boldsymbol{0} \tag{5} }$