３．三角測量（３次元）２／２
はじめに
前回は、１つのカメラで見たときのカメラ座標系とセンサ座標系との関係を、行列で表現したところまで説明しました。今回はカメラが２つ（それぞれカメラ１とカメラ２と名前を付けておきます）あり、かつ同じ測定対象を見ている場合を考えて不足している式を補います。
２つのカメラで見る
２つのカメラで、同じ点を見た場合を式で表現してみましょう（２つのカメラを区別するために添え字に番号を振っておきます）。
$$
\begin{cases}
Z_{c1}
\begin{pmatrix}
u_{c1}\\
v_{c1}\\
1
\end{pmatrix}
=
\begin{pmatrix}
f_{x1} & 0 & c_{x1}\\
0 & f_{y1} & c_{y1}\\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}\\
\end{pmatrix} (1)\\
\\
Z_{c2}
\begin{pmatrix}
u_{c2}\\
v_{c2}\\
1
\end{pmatrix}
=
\begin{pmatrix}
f_{x2} & 0 & c_{x2}\\
0 & f_{y2} & c_{y2}\\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
X_{c2}\\
Y_{c2}\\
Z_{c2}\\
\end{pmatrix} (2)
\end{cases}
$$
式を見ると、同じ点を見ているにも関わらず、カメラ座標系の値は異なっています。これは、同じ対象を、それぞれのカメラ座標系で見ている為に起きている現象です。このままでは、式を増やしたのに、変数も増えてしまい解くことができません。
そこで、カメラ１とカメラ２のそれぞれのカメラ座標系の関係を、式で表現してみます。カメラ２のカメラ座標系に対し回転行列R（３X３）を掛け、並進行列ｔ（３X１）を足すとカメラ１のカメラ座標系に一致するものとし、且つRとtはキャリブレーションにより既知とします（※ここでは座標変換やキャリブレーションについては解説しませんのでご了承ください）。この回転行列と並進行列を使ってカメラ座標系で見た座標を次のように表現することができます。なお、ここから実数と行列が入り混じるので、行列は太字で表現するようにします。
$$
\begin{pmatrix}
X_{c2}\\
Y_{c2}\\
Z_{c2}
\end{pmatrix}
=
\bm{R}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
+\bm{t}
$$
この式をもう少し整理します。
$$
\begin{pmatrix}
X_{c2}\\
Y_{c2}\\
Z_{c2}
\end{pmatrix}
=
\begin{pmatrix}
R_{11} & R_{12} & R_{13}\\
R_{21} & R_{22} & R_{23}\\
R_{31} & R_{32} & R_{33}
\end{pmatrix}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
+
\begin{pmatrix}
t_x\\
t_y\\
t_z
\end{pmatrix}\\
\\
=
\begin{pmatrix}
R_{11} & R_{12} & R_{13} & t_x\\
R_{21} & R_{22} & R_{23} & t_y\\
R_{31} & R_{32} & R_{33} & t_z
\end{pmatrix}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}\\
1
\end{pmatrix}
\\
ここで、\left(\bm{R|t}\right) = \begin{pmatrix}
R_{11} & R_{12} & R_{13} & t_x\\
R_{21} & R_{22} & R_{23} & t_y\\
R_{31} & R_{32} & R_{33} & t_z
\end{pmatrix} ,
\bm{X_{c1}} =
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}\\
1
\end{pmatrix} と置くと\\
\\
\begin{pmatrix}
X_{c2}\\
Y_{c2}\\
Z_{c2}
\end{pmatrix}
=
\left(\bm{R|t}\right)\bm{X_{c1}} と表せる
$$
この式を(2)に代入するのですが、連立方程式を解きやすくするためにもう一つ行列を定義しておきます。
$$
\bm{I} = \begin{pmatrix}
1 & 0 & 0 & 0\\
0 & 1 & 0 & 0\\
0 & 0 & 1 & 0
\end{pmatrix}
$$
先ほどの式と合わせて、（１）式と（２）式を変形します。
※Kcamはカメラ内部行列です
$$
\begin{cases}
Z_{c1}
\begin{pmatrix}
u_{c1}\\
v_{c1}\\
1
\end{pmatrix}
=
\bm{K_{cam1}IX_{cam}} (3)\\
Z_{c2}
\begin{pmatrix}
u_{c2}\\
v_{c2}\\
1
\end{pmatrix}
=
\bm{K_{cam2}(R|t)X_{cam}} (4)
\end{cases}
$$
ここで、
$$
\bm{K_{cam1}I}=
\begin{pmatrix}
a_{11} & a_{12} & a_{13} & a_{14}\\
a_{21} & a_{22} & a_{23} & a_{24}\\
a_{31} & a_{32} & a_{33} & a_{34}
\end{pmatrix},
\bm{K_{cam2}(R|t)}=
\begin{pmatrix}
b_{11} & b_{12} & b_{13} & b_{14}\\
b_{21} & b_{22} & b_{23} & b_{24}\\
b_{31} & b_{32} & b_{33} & b_{34}
\end{pmatrix}
$$
と表現すると、（３）式と（４）式は下記のように表せます。
$$
\begin{cases}
Z_{c1}
\begin{pmatrix}
u_{c1}\\
v_{c1}\\
1
\end{pmatrix}
=
\begin{pmatrix}
a_{11}X_{c1}+a_{12}Y_{c1}+a_{13}Z_{c1}+a_{14}\\
a_{21}X_{c1}+a_{22}Y_{c1}+a_{23}Z_{c1}+a_{24}\\
a_{31}X_{c1}+a_{32}Y_{c1}+a_{33}Z_{c1}+a_{34}\\
\end{pmatrix} (5)
\\
Z_{c2}
\begin{pmatrix}
u_{c2}\\
v_{c2}\\
1
\end{pmatrix}
=
\begin{pmatrix}
b_{11}X_{c1}+b_{12}Y_{c1}+b_{13}Z_{c1}+b_{14}\\
b_{21}X_{c1}+b_{22}Y_{c1}+b_{23}Z_{c1}+b_{24}\\
b_{31}X_{c1}+b_{32}Y_{c1}+b_{33}Z_{c1}+b_{34}\\
\end{pmatrix} (6)
\end{cases}
$$
（５）式と（６）式を見ると
$$
Z_{c2}=b_{31}X_{c1}+b_{32}Y_{c1}+b_{33}Z_{c1}+b_{34}
$$
であることが分かります。Zc2を消して連立方程式を解くために、（５）式をZc1で割り、（６）式をZc2で割ります。
$$
\begin{cases}
\begin{pmatrix}
u_{c1}\\
v_{c1}\\
1
\end{pmatrix}
=
\begin{pmatrix}
\cfrac{a_{11}X_{c1}+a_{12}Y_{c1}+a_{13}Z_{c1}+a_{14}}{Z_{c1}}\\
\cfrac{a_{21}X_{c1}+a_{22}Y_{c1}+a_{23}Z_{c1}+a_{24}}{Z_{c1}}\\
1\\
\end{pmatrix}
\\
\begin{pmatrix}
u_{c2}\\
v_{c2}\\
1
\end{pmatrix}
=
\begin{pmatrix}
\cfrac{b_{11}X_{c1}+b_{12}Y_{c1}+b_{13}Z_{c1}+b_{14}}{b_{31}X_{c1}+b_{32}Y_{c1}+b_{33}Z_{c1}+b_{34}}\\
\cfrac{b_{21}X_{c1}+b_{22}Y_{c1}+b_{23}Z_{c1}+b_{24}}{b_{31}X_{c1}+b_{32}Y_{c1}+b_{33}Z_{c1}+b_{34}}\\
1\\
\end{pmatrix}
\end{cases}
$$
3行目は１＝１という、方程式を解くには不要な式なので、各１行目と２行目を整理します。
$$
\begin{cases}
a_{11}X_{c1}+a_{12}Y_{c1}+(a_{13}-u_{c1})Z_{c1}=-a_{14}\\
a_{21}X_{c1}+a_{22}Y_{c1}+(a_{23}-v_{c1})Z_{c1}=-a_{24}\\
(b_{11}-u_{c2}b_{31})X_{c1}+(b_{12}-u_{c2}b_{32})Y_{c1}+(b_{13}-u_{c2}b_{33})Z_{c1}=u_{c2}b_{34}-b_{14}\\
(b_{21}-v_{c2}b_{31})X_{c1}+(b_{22}-v_{c2}b_{32})Y_{c1}+(b_{23}-v_{c2}b_{33})Z_{c1}=v_{c2}b_{34}-b_{24}
\end{cases} (7)
$$
X,Y,Zでまとめてみます。
$$
\begin{pmatrix}
a_{11} & a_{12} & a_{13}-u_{c1}\\
a_{21} & a_{22} & a_{23}-v_{c1}\\
b_{11}-u_{c2}b_{31} & b_{12}-u_{c2}b_{32} & b_{13}-u_{c2}b_{33}\\
b_{21}-v_{c2}b_{31} & b_{22} - v_{c2}b_{32} & b_{23}-v_{c2}b_{33}
\end{pmatrix}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
=
\begin{pmatrix}
-a_{14}\\
-a_{24}\\
u_{c2}b_{34}-b_{14}\\
v_{c2}b_{34}-b_{24}
\end{pmatrix}
$$
ここで、左端と右辺の行列を下記のように置きます。
$$
\bm{A}=
\begin{pmatrix}
a_{11} & a_{12} & a_{13}-u_{c1}\\
a_{21} & a_{22} & a_{23}-v_{c1}\\
b_{11}-u_{c2}b_{31} & b_{12}-u_{c2}b_{32} & b_{13}-u_{c2}b_{33}\\
b_{21}-v_{c2}b_{31} & b_{22} - v_{c2}b_{32} & b_{23}-v_{c2}b_{33}
\end{pmatrix}
,
\bm{b}=
\begin{pmatrix}
-a_{14}\\
-a_{24}\\
u_{c2}b_{34}-b_{14}\\
v_{c2}b_{34}-b_{24}
\end{pmatrix}
$$
先ほどの式を上の変数で置き換えると、
$$
\bm{A}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
=
\bm{b}
$$
と書くことができます。この式は連立方程式の形になっており、Aに逆行列が存在するならば下記のようにしてX,Y,Zを求めることができます。
$$
もし、\bm{A^{-1}}が存在するなら、\\
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
=
\bm{A^{-1}b}
$$
となり測定対象の座標が求められます。
ただ、この逆行列が曲者で、普通は逆行列が求まりません。数学的にはランクを求めて逆行列が存在するか確かめる方が良いのかもしれませんが、ここでは幾何学的に説明したいと思います。理由は単純で、式4つで解を求めるというのは、３次元空間上で２つの直線の交点を求めることと同じです。カメラのセンサ分解能は有限であり、当然誤差もあるのでほとんどの場合交わることがありません。
ネットで紹介されている情報を見ると、直線同士が最も近づくときの中間値を交点とする方法や、空間をボクセルに分割して、どのボクセルに２つの直線が入ったかで交点を求めるようなものもあります。
どれも判定用の処理が追加で必要なのと、なかなか理解も難しかったのでここでは式を３つに減らして方法を紹介します。式３つで解くというのは、一方は直線で、もう一方は平面とした場合の交点を求めることと同じです。こうすると、直線と平面が平行である場合を除いて必ず交点が存在します。
式で表現すると（７）式は下記のようになります。
$$
\begin{cases}
a_{11}X_{c1}+a_{12}Y_{c1}+(a_{13}-u_{c1})Z_{c1}=-a_{14}\\
a_{21}X_{c1}+a_{22}Y_{c1}+(a_{23}-v_{c1})Z_{c1}=-a_{24}\\
(b_{11}-u_{c2}b_{31})X_{c1}+(b_{12}-u_{c2}b_{32})Y_{c1}+(b_{13}-u_{c2}b_{33})Z_{c1}=u_{c2}b_{34}-b_{14}
\end{cases}
$$
御覧の通り４行目がなくなっただけです。こちらも、行列に整理しておきます。
$$
\begin{pmatrix}
a_{11} & a_{12} & a_{13}-u_{c1}\\
a_{21} & a_{22} & a_{23}-v_{c1}\\
b_{11}-u_{c2}b_{31} & b_{12}-u_{c2}b_{32} & b_{13}-u_{c2}b_{33}
\end{pmatrix}
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
=
\begin{pmatrix}
-a_{14}\\
-a_{24}\\
u_{c2}b_{34}-b_{14}
\end{pmatrix}
$$
先ほどと同様に
$$
\bm{A_3}=
\begin{pmatrix}
a_{11} & a_{12} & a_{13}-u_{c1}\\
a_{21} & a_{22} & a_{23}-v_{c1}\\
b_{11}-u_{c2}b_{31} & b_{12}-u_{c2}b_{32} & b_{13}-u_{c2}b_{33}
\end{pmatrix}
,
\bm{b_3}=
\begin{pmatrix}
-a_{14}\\
-a_{24}\\
u_{c2}b_{34}-b_{14}
\end{pmatrix}
$$
とおき。Aに逆行列が存在する場合、
$$
\begin{pmatrix}
X_{c1}\\
Y_{c1}\\
Z_{c1}
\end{pmatrix}
=
\bm{A_{3}^{-1}b_{3}}
$$
となり、３次元空間上での測定対象の座標を求めることができるようになりました。
以上が、３次元での三角測量計算式の導出過程です。２次元と比べてかなりボリュームが多く、導出過程も理解が難しいので自分は何度か挫折しかけました。世の中の解説資料とは解き方が少し違っているかと思いますが、自分なりにつまづきやすい箇所を丁寧に解説したつもりです。この資料が、皆様のお役に立てれば幸いです。次回は、少しおまけでプロジェクタのある画素から投影した画像が、カメラのどの画素に映って見えるか計算してみようと思います。式展開は簡単なので、興味のある方はもう少しお付き合いください。