About Triangulation
同じシーンを2台のカメラで撮影する場合、それぞれのカメラの位置、向き、内部パラメータがわかっていれば、対応する点の3次元位置を復元することができます。これは、それぞれのカメラのレンズ中心を始点とする、その点の視線を求めることができ、2台のカメラのレンズ中心とシーン内のその点を結ぶ三角形を計算することができるからです。これは三角測量と呼ばれています。
Camera Matrix and Triangulation
画像の(x,y)に投影されるシーン内の点(X,Y,Z)は、透視投影のモデルから次のような式で表されます。
\begin{align*}
x=f_0\frac{P_{11}X+P_{12}Y+P_{13}Z+P_{14}}{P_{31}X+P_{32}Y+P_{33}Z+P_{34}}, y=f_0\frac{P_{21}X+P_{22}Y+P_{23}Z+P_{24}}{P_{31}X+P_{32}Y+P_{33}Z+P_{34}} \tag{1}
\end{align*}
f0はスケール定数で、Pij(i=1,2,3,j=1,...,4)はカメラの内部パラメータと運動パラメータから定まる係数です。式(1)は次のように書き換えることができます。
\begin{align*}
\begin{pmatrix}
x/f_0 \\
y/f_0 \\
1 \\
\end{pmatrix}\simeq
\begin{pmatrix}
P_{11} & P_{12} & P_{13} & P_{14} \\
P_{21} & P_{22} & P_{23} & P_{24} \\
P_{31} & P_{32} & P_{33} & P_{34} \\
\end{pmatrix}
\begin{pmatrix}
X \\
Y \\
Z \\
1 \\
\end{pmatrix} \tag{2}
\end{align*}
行列P=(Pij)はカメラ行列と呼ばれます。三角測量を行うには、あらかじめカメラ行列Pを計算しておく必要があります。いわゆるカメラのキャリブレーションです。2つのカメラ行列をそれぞれ P=(Pij) と P′=(P′ij) とすると、シーン中の点(X,Y,Z)を各画像上の点(x,y),(x',y')で観測したとき、誤差がなければ、(x,y),(x',y')から次のように(X,Y,Z)を計算することができます。
Triangulation by Camera Matrix
1. Calculate the following 4x3 matrix T and 4-dimensional vector p
\begin{align*}
T=\begin{pmatrix}
f_0P_{11}-xP_{31} & f_0P_{12}-xP_{32} & f_0P_{13}-xP_{33} \\
f_0P_{21}-yP_{31} & f_0P_{22}-yP_{32} & f_0P_{23}-yP_{33} \\
f_0P\prime_{11}-x\prime P\prime_{31} & f_0P\prime_{12}-x\prime P\prime_{32} & f_0P\prime_{13}-x\prime P\prime_{33} \\
f_0P\prime_{21}-y\prime P\prime_{31} & f_0P\prime_{22}-y\prime P\prime_{32} & f_0P\prime_{23}-y\prime P\prime_{33} \\
\end{pmatrix},
p=\begin{pmatrix}
f_0P_{14}-xP_{34} \\
f_0P_{24}-yP_{34} \\
f_0P\prime_{14}-x\prime P\prime_{34} \\
f_0P\prime_{24}-y\prime P\prime_{34} \\
\end{pmatrix} \tag{3}
\end{align*}
2. Solve the following simultaneous linear equations to obtain X, Y, and Z
T^\intercal T\begin{pmatrix}
X \\
Y \\
Z \\
\end{pmatrix}=-T^\intercal p \tag{4}
式(1)の分母を払って整理し、2台目のカメラについても同様にすると、X,Y,Zについて以下の連立一次方程式が得られます。
\begin{align*}
(f_0P_{11}-xP_{31})X + (f_0P_{12}-xP_{32})Y + (f_0P_{13}-xP_{33})Z + f_0P_{14} - xP_{34} = 0 \\
(f_0P_{21}-yP_{31})X + (f_0P_{22}-yP_{32})Y + (f_0P_{23}-yP_{33})Z + f_0P_{24} - yP_{34} = 0 \\
(f_0P\prime_{11}-x\prime P\prime_{31})X + (f_0P\prime_{12}-x\prime P\prime_{32})Y + (f_0P\prime_{13}-x\prime P\prime_{33})Z + f_0P\prime_{14} - x\prime_P\prime_{34} = 0 \\
(f_0P\prime_{21}-y\prime P\prime_{31})X + (f_0P\prime_{22}-y\prime P\prime_{32})Y + (f_0P\prime_{23}-y\prime P\prime_{33})Z + f_0P\prime_{24} - y\prime P\prime_{34} = 0 \tag{5}
\end{align*}
これは3つの未知数X,Y,Zに対する4つの方程式であり、それらは互いに線形従属で、独立なものは3個です。したがって、これら4つのうちどれか3つを抽出して解を計算すれば、残りの方程式は自動的に満たされます。しかし、これらを同時に解いても同じ解が得られます。具体的には、式(3)の行列Tとベクトルpを用いて、式(5)を次のように書いて、
T\begin{pmatrix}
X \\
Y \\
Z \\
\end{pmatrix}=p \tag{6}
両辺にTの転置を掛けることで、式(4)を解くことができます。これは式(6)に最小二乗法を適用したことと同義です。
\begin{align*}
\lVert Ax-b \rVert^2&=(Ax-b,Ax-b) \\
&=(Ax,Ax)-2(Ax-b)+(b,b) \\
&=(x,A^\intercal Ax)-2(x,A^\intercal b)+\lVert b \rVert^2
\end{align*}
xで微分すると、以下の式が得られます。
A^\intercal Ax=A^\intercal b
Triangulation from corresponding points with errors
観測点(x,y),(x',y')に誤差がある場合、2台のカメラで定義される視線は必ずしも1点で交わるとは限りません。この場合、補正する方法があります。
合理的な方法としては、観測された対応点(x,y), (x',y')を視線が交わる位置まで画像上の座標を補正して3次元位置(X,Y,Z)を計算することです。視線が交わるための必要十分条件はエピ極線方程式であり、基礎行列Fはカメラ行列P,P′から求められます。
\begin{pmatrix}
\begin{pmatrix}
x/f_0 \\
y/f_0 \\
1 \\
\end{pmatrix},
F\begin{pmatrix}
x\prime/f_0 \\
y\prime/f_0 \\
1 \\
\end{pmatrix}
\end{pmatrix}=0 \tag{7}
観測された対応点(x,y),(x′,y′)から式(7)を満たす位置(xˉ,yˉ),(xˉ′,yˉ′)への最短の補正を考えます。以下の移動距離の二乗和を最小化します。
S=(x-\bar{x})^2+(y-\bar{y})^2+(x\prime-\bar{x}\prime)^2+(y\prime-\bar{y}\prime)^2 \tag{8}
このSは再投影誤差と呼ばれます。
ここまでの処理のコードはこちらです。以下のコマンドで三角測量が試せます。
python triangulation.py
テストデータには、以下の2つの視点のカメラ画像を使用しました。
