![見出し画像](https://assets.st-note.com/production/uploads/images/71160217/rectangle_large_type_2_405b259b23e0a1afafccd0374933cc4f.png?width=1200)
3.三角測量(3次元)2/2
はじめに
前回は、1つのカメラで見たときのカメラ座標系とセンサ座標系との関係を、行列で表現したところまで説明しました。今回はカメラが2つ(それぞれカメラ1とカメラ2と名前を付けておきます)あり、かつ同じ測定対象を見ている場合を考えて不足している式を補います。
2つのカメラで見る
2つのカメラで、同じ点を見た場合を式で表現してみましょう(2つのカメラを区別するために添え字に番号を振っておきます)。
$$
\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}
$$
式を見ると、同じ点を見ているにも関わらず、カメラ座標系の値は異なっています。これは、同じ対象を、それぞれのカメラ座標系で見ている為に起きている現象です。このままでは、式を増やしたのに、変数も増えてしまい解くことができません。
そこで、カメラ1とカメラ2のそれぞれのカメラ座標系の関係を、式で表現してみます。カメラ2のカメラ座標系に対し回転行列R(3X3)を掛け、並進行列t(3X1)を足すとカメラ1のカメラ座標系に一致するものとし、且つ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}
$$
先ほどの式と合わせて、(1)式と(2)式を変形します。
※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}
$$
と表現すると、(3)式と(4)式は下記のように表せます。
$$
\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}
$$
(5)式と(6)式を見ると
$$
Z_{c2}=b_{31}X_{c1}+b_{32}Y_{c1}+b_{33}Z_{c1}+b_{34}
$$
であることが分かります。Zc2を消して連立方程式を解くために、(5)式をZc1で割り、(6)式を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行目は1=1という、方程式を解くには不要な式なので、各1行目と2行目を整理します。
$$
\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つで解を求めるというのは、3次元空間上で2つの直線の交点を求めることと同じです。カメラのセンサ分解能は有限であり、当然誤差もあるのでほとんどの場合交わることがありません。
![](https://assets.st-note.com/img/1644755946713-2FNiQNDnHz.png)
ネットで紹介されている情報を見ると、直線同士が最も近づくときの中間値を交点とする方法や、空間をボクセルに分割して、どのボクセルに2つの直線が入ったかで交点を求めるようなものもあります。
どれも判定用の処理が追加で必要なのと、なかなか理解も難しかったのでここでは式を3つに減らして方法を紹介します。式3つで解くというのは、一方は直線で、もう一方は平面とした場合の交点を求めることと同じです。こうすると、直線と平面が平行である場合を除いて必ず交点が存在します。
![](https://assets.st-note.com/img/1644756054345-H1g6JnHpvg.png)
式で表現すると(7)式は下記のようになります。
$$
\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}
$$
御覧の通り4行目がなくなっただけです。こちらも、行列に整理しておきます。
$$
\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}}
$$
となり、3次元空間上での測定対象の座標を求めることができるようになりました。
以上が、3次元での三角測量計算式の導出過程です。2次元と比べてかなりボリュームが多く、導出過程も理解が難しいので自分は何度か挫折しかけました。世の中の解説資料とは解き方が少し違っているかと思いますが、自分なりにつまづきやすい箇所を丁寧に解説したつもりです。この資料が、皆様のお役に立てれば幸いです。次回は、少しおまけでプロジェクタのある画素から投影した画像が、カメラのどの画素に映って見えるか計算してみようと思います。式展開は簡単なので、興味のある方はもう少しお付き合いください。
この記事が気に入ったらサポートをしてみませんか?