三次元・直線・最小自乗法（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 による実装例はこのへん。