2012/01/19

三次元・直線・最小自乗法(vs. ぷで)

ぷでさんが面白い記事を書いていたので、僕も同じテーマで書いてみる事にしたよ。

「3次元における直線 \(L_{i}\: \left( i = 0,1,\cdots, n-1 \right) \) の尤もらしい交点 \( \vect{p} \) を最小自乗法で求める」

これは、位置と姿勢が既知である多視点画像における対応点3次元位置を推定する際にしばしば用いられる議論だ(下図参照)。3次元空間中では一般的に複数の直線の交点は一意には定まらず、いわゆる “ねじれ” の関係になる。その際、できるだけ誤差を抑えるために、各直線が最接近する点を採用する事が望ましい。

複数カメラの視線を基に被写体の3次元座標を得るイメージ.
カメラ光学中心から各対応点を貫く直線の交点が3次元位置となる.


直線 \(L_{i}\) が通る点を \( \vect{b}_{i} \) 、傾き(方向)を \( \vect{d}_{i} \) とすると、\(L_{i}\) 上の任意の点はパラメータ \(t_{i}\) を用いて
\begin{align}
L_{i}(t_{i}) = t_{i}\vect{d}_{i} + \vect{b}_{i}
\end{align}と表す事ができる。

また、直線上の座標 \(L_{i}(t_{i})\) と点 \(\vect{p}\) の距離の自乗 \(f_{i}(t_{i},\,\vect{p})\) は以下のようになる。
\begin{align}
f_{i}(t_{i},\,\vect{p}) &= \left| L_{i}(t_{i}) - \vect{p} \right|^{2} \\
&= \left| t_{i}\vect{d}_{i} + \vect{b}_{i} - \vect{p} \right|^{2}
\end{align}

ここで、すべての直線上の点 \(L_{i}(t_{i})\) から \(\vect{p}\) までの距離自乗の総和を
\begin{align}
& F(t_{0},\, \cdots,\, t_{i-1},\, t_{i},\, t_{i+1},\, \cdots, \, t_{n-1}, \, \vect{p}) \\
=& \frac{1}{2} \sum_{i=0}^{n-1} \left| t_{i} \vect{d}_{i} + \vect{b}_{i} - \vect{p} \right|^{2} \\
=& \frac{1}{2} \sum_{i=0}^{n-1} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} - \vect{p} \right)^{t} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} - \vect{p} \right)
\end{align}と置き、関数 \(F(t_{0},\, \cdots,\, t_{i-1},\, t_{i},\, t_{i+1},\, \cdots, \, t_{n-1}, \, \vect{p})\) の最小化問題を解く。

まず、上式を各 \( t_{i} \) で偏微分して \( 0 \) と置き、
\begin{align}
& \frac{\partial }{\partial t_{i}} F(t_{0},\, \cdots,\, t_{i-1},\, t_{i},\, t_{i+1},\, \cdots, \, t_{n-1}, \, \vect{p}) \\
=& \frac{\partial}{\partial t_{i}} \left[ \frac{1}{2} \sum_{i=0}^{n-1} \left( t_{i} \vect{d}_{i} + \left( \vect{b}_{i} - \vect{p} \right) \right)^{t} \left( t_{i} \vect{d}_{i} + \left( \vect{b}_{i} - \vect{p} \right) \right) \right] \\
=& \frac{\partial}{\partial t_{i}} \left[ \frac{1}{2} \sum_{i=0}^{n-1} t_{i}^{2}\vect{d}_{i}^{t}\vect{d}_{i} + \sum_{i=0}^{n-1} t_{i} \vect{d}_{i}^{t} \left( \vect{b}_{i} - \vect{p} \right) + \frac{1}{2} \sum_{i=0}^{n-1} \left( \vect{b}_{i} - \vect{p} \right)^{t} \left( \vect{b}_{i} - \vect{p} \right) \right] \\
=& t_{i}\vect{d}_{i}^{t}\vect{d}_{i} + \vect{d}_{i}^{t} \left( \vect{b}_{i} - \vect{p} \right) = 0
\end{align}
次に \( \vect{p} \) で偏微分して同様に \( 0 \) と置くと
\begin{align}
&\frac{\partial }{\partial \vect{p}} F(t_{0},\, \cdots,\, t_{i-1},\, t_{i},\, t_{i+1},\, \cdots, \, t_{n-1}, \, \vect{p}) \\
=& \frac{\partial}{\partial \vect{p}} \left[ \frac{1}{2} \sum_{i=0}^{n-1} \left( \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right) - \vect{p} \right)^{t} \left( \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right) - \vect{p} \right) \right] \\
=& \frac{\partial}{\partial \vect{p}} \left[ \frac{1}{2} \sum_{i=0}^{n-1} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right)^{t} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right) - \sum_{i=0}^{n-1} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right)^{t} \vect{p} + \frac{1}{2}\sum_{i=0}^{n-1} \vect{p}^{t}\vect{p} \right] \\
=& -\sum_{i=0}^{n-1} \left( t_{i} \vect{d}_{i} + \vect{b}_{i} \right) + n\vect{p} = \vect{0}
\end{align}が得られる。

これらをまとめると、未知数 \( t_{0},\, \cdots,\, t_{n-1},\, \vect{p} \) に関する次の方程式になる。
\begin{align}
\begin{pmatrix}
\vect{d}_{0}^{t}\vect{d}_{0} & 0 & \cdots & 0 & -\vect{d}_{0}^{t} \\
0 & \vect{d}_{1}^{t}\vect{d}_{1} & \ddots & \vdots & -\vect{d}_{1}^{t} \\
\vdots & \ddots & \ddots & 0 & \vdots \\
0 & \cdots & 0 & \vect{d}_{n-1}^{t}\vect{d}_{n-1} & -\vect{d}_{n-1}^{t} \\
-\vect{d}_{0} & -\vect{d}_{1} & \cdots & -\vect{d}_{n-1} & \vect{E}_{3,\,3}
\end{pmatrix}
\begin{pmatrix}
t_{0} \\
t_{1} \\
\vdots \\
t_{n-1} \\
\vect{p}
\end{pmatrix} =
\begin{pmatrix}
-\vect{d}_{0}^{t} \vect{b}_{0} \\
-\vect{d}_{1}^{t} \vect{b}_{1} \\
\vdots \\
-\vect{d}_{n-1}^{t} \vect{b}_{n-1} \\
\sum_{i=0}^{n-1}\vect{b}_{i}
\end{pmatrix}
\end{align}

ここで、\( \vect{E}_{3,\,3} \) は 3 階の単位行列である。上記の方程式を LU 分解か何かを使って解けばたぶん最急降下法を用いなくても \( \vect{p} \) を得る事ができる(…んじゃないかな)。

ちなみに boost を用いた LU 分解の実装例はこのへん。OpenCV による実装例はこのへん

1 件のコメント:

ひとことどうぞφ(・ω・,,)