\[\def\mbf#1{{\mathbf #1}}
\def\SO(#1){{\mathrm{SO}(#1)}}
\def\so(#1){{\mathfrak{so}(#1)}}
\def\D{{\delta}}
\def\Cov{{\Sigma}}
\def\VStar{{V^{*}}}
\def\Db{{\mbf{\D}_{\mbf{b}}}}
\def\Da{{\mbf{\D}_{\mbf{a}}}}
\def\DP{{\mbf{\D}_{P}}}\]
問題設定
入力
SBAは,各視点から観測されたランドマークの像の集合 \(X\) を入力とする.
\[X = \begin{bmatrix}
\mbf{x}^{\top}_{11},
\dots,
\mbf{x}^{\top}_{1m},
\mbf{x}^{\top}_{21},
\dots,
\mbf{x}^{\top}_{2m},
\dots,
\mbf{x}^{\top}_{n1},
\dots,
\mbf{x}^{\top}_{nm}
\end{bmatrix}\]
出力
3次元空間におけるランドマーク座標 \(\mbf{b}_{i},i=1,\dots,n\)
カメラ姿勢 \(\mbf{a}_{j} = [\mbf{t}_{j}, \mbf{\omega}_{j}],j=1,\dots,m\)
ただし \(\mbf{t}_{j} \in \mathbb{R}^{3}\) は並進を表すベクトルであり,\(\mbf{\omega}_{j} \in \mathfrak{so}(3)\) はカメラの回転を表す行列 \(R \in \SO(3)\) に対応するリー代数の元である.
目的
投影モデルを \(Q(\mbf{a}_{i},\mbf{b}_{j})\) とし,以下の誤差関数を最小化するような \(P = \left[\mbf{a}, \mbf{b}\right] = \left[ \mbf{a}^{\top}_{1}, \dots, \mbf{a}^{\top}_{m}, \mbf{b}^{\top}_{1}, \dots, \mbf{b}^{\top}_{n} \right]\) を求める.
(1)\[E(P) = \begin{align}
\sum_{i=1}^{n} \sum_{j=1}^{m} d_{\Cov_{\mbf{x}_{ij}}}(Q(\mbf{a}_{j}, \mbf{b}_{i}), \mbf{x}_{ij})^{2}
\end{align}\]
ここで \(d_{\Cov_{\mbf{x}}}\) は \(\mbf{x}\) に対応する分散共分散行列を \(\Cov_{\mbf{x}}\) として
\[d_{\Cov_{\mbf{x}}}(\mbf{x}_{1}, \mbf{x}_{2}) =
\sqrt{(\mbf{x}_{1} - \mbf{x}_{2})^{\top} \Cov^{-1}_{\mbf{x}} (\mbf{x}_{1} - \mbf{x}_{2})}\]
で定義される距離関数である.
(2)\[\begin{split}\hat{X}
= \begin{bmatrix}
\hat{\mbf{x}}^{\top}_{11},
\dots,
\hat{\mbf{x}}^{\top}_{1m},
\hat{\mbf{x}}^{\top}_{21},
\dots,
\hat{\mbf{x}}^{\top}_{2m},
\dots,
\hat{\mbf{x}}^{\top}_{n1},
\dots,
\hat{\mbf{x}}^{\top}_{nm}
\end{bmatrix}^{\top} \\\end{split}\]
(3)\[\hat{\mbf{x}}_{ij}
= Q(\mbf{a}_{j}, \mbf{b}_{i})\]
(4)\[\Cov_{X}
= diag(
\Cov_{\mbf{x}_{11}},
\dots,
\Cov_{\mbf{x}_{1m}},
\Cov_{\mbf{x}_{21}},
\dots,
\Cov_{\mbf{x}_{2m}},
\dots,
\Cov_{\mbf{x}_{n1}},
\dots,
\Cov_{\mbf{x}_{nm}}
)\]
とおけば,誤差を次のように表現することができる.
\[E(P)
= (X-\hat{X})^{\top} \Cov_{X}^{-1} (X-\hat{X})\]
解法の概要
SBAでは,誤差関数を最小化するような \(P\) を見つけるため, \(P^{(t)}\) を逐次的に更新し,誤差関数を探索する.すなわち,時刻 \(t\) における \(P\) の更新量を \(\D_{P}^{(t)} = \left[ \D_{\mbf{a}_{1}}^{\top}, \dots, \D_{\mbf{a}_{m}}^{\top}, \D_{\mbf{b}_{1}}^{\top}, \dots, \D_{\mbf{b}_{n}}^{\top} \right]\) として,
(5)\[P^{(t+1)} \leftarrow P^{(t)} + \D_{P}^{(t)}\]
というふうに \(P^{(t)}\) を更新することで誤差関数を最小化するような \(P\) を見つける.
更新量 \(\D_{P}^{(t)}\) の計算にはLM法を用いる.LM法の更新式は次のように表される.
(6)\[\begin{split}\left[
J^{\top} \Cov^{-1} J + \lambda I
\right]
\D_{P}^{(t)}
= J^{\top} \Cov^{-1} \left[ X - \hat{X} \right] \\\end{split}\]
\(\mbf{J}\) は \(\hat{X}\) のヤコビ行列 \(J = \frac{\partial \hat{X}}{\partial P} \rvert_{P=P^{(t)}}\) であり, \(\lambda \in \mathbb{R}, \lambda \geq 0\) は damping parameter である.
SBAでは,
\(J\) の構造に着目し,
(6) をより小さい複数の線型方程式に分解する.さらに,分解によって得られた方程式がスパースな行列によって構成されていることに着目し,計算を高速化している.
解法
線型方程式の分解
まず \(J\) を分解する. \(P\) の定義より, \(A = \frac{\partial \hat{X}}{\partial \mbf{a}},B = \frac{\partial \hat{X}}{\partial \mbf{b}}\) とおけば, \(J\) は
(7)\[J = \frac{\partial \hat{X}}{\partial P}
= \frac{\partial \hat{X}}{\partial (a, b)} = \left[ A, B \right]\]
\[\begin{split}\begin{align}
\mbf{\epsilon}_{\mbf{a}} &= A^{\top} \Cov^{-1} (X - \hat{X}) \\
\mbf{\epsilon}_{\mbf{b}} &= B^{\top} \Cov^{-1} (X - \hat{X})
\end{align}\end{split}\]
とおくことによって,
\[\begin{split}J^{\top} \Cov^{-1} (X - \hat{X})
= \begin{bmatrix} \mbf{\epsilon}_{\mbf{a}} \\ \mbf{\epsilon}_{\mbf{b}} \end{bmatrix}\end{split}\]
と書ける.
さらに
(6) の左辺を分解する.左辺の
\(J^{\top} \Cov^{-1} J\) という項は大きく4つの行列に分解することができる.
(8)\[\begin{split}\begin{align}
J^{\top} \Cov^{-1} J
&= \begin{bmatrix}
A^{\top} \\ B^{\top}
\end{bmatrix}
\Cov^{-1}
\begin{bmatrix}
A & B
\end{bmatrix} \\
&= \begin{bmatrix}
A^{\top} \Cov^{-1} A & A^{\top} \Cov^{-1} B \\
B^{\top} \Cov^{-1} A & B^{\top} \Cov^{-1} B
\end{bmatrix} \\
&= \begin{bmatrix}
U & W \\
W^{\top} & V
\end{bmatrix}
\end{align}\end{split}\]
以上の結果を用いると, (6) は
\[\begin{split}\left[
\begin{bmatrix}
U & W \\
W^{\top} & V
\end{bmatrix}
+
\begin{bmatrix}
\lambda I & 0 \\
0 & \lambda I
\end{bmatrix}
\right]
\begin{bmatrix}
\Da \\
\Db
\end{bmatrix}
=
\begin{bmatrix}
\mbf{\epsilon}_{\mbf{a}} \\
\mbf{\epsilon}_{\mbf{b}}
\end{bmatrix}\end{split}\]
という形にすることができる.さらに,
\[\begin{split}\begin{align}
U^{*} &= U + \lambda I \\
\VStar &= V + \lambda I
\end{align}\end{split}\]
とおけば,
\[\begin{split}\begin{bmatrix}
U^{*} & W \\
W^{\top} & \VStar
\end{bmatrix}
\begin{bmatrix}
\Da \\
\Db
\end{bmatrix}
=
\begin{bmatrix}
\mbf{\epsilon}_{\mbf{a}} \\
\mbf{\epsilon}_{\mbf{b}}
\end{bmatrix}\end{split}\]
となる.この両辺に
\[\begin{split}\begin{bmatrix}
I & -W{\VStar}^{-1} \\
0 & I
\end{bmatrix}\end{split}\]
という行列を左から作用させると,
(9)\[\begin{split}\begin{bmatrix}
I & -W{\VStar}^{-1} \\
0 & I
\end{bmatrix}
\begin{bmatrix}
U^{*} & W \\
W^{\top} & \VStar
\end{bmatrix}
\begin{bmatrix}
\Da \\
\Db
\end{bmatrix}
=
\begin{bmatrix}
I & -W{\VStar}^{-1} \\
0 & I
\end{bmatrix}
\begin{bmatrix}
\mbf{\epsilon}_{\mbf{a}} \\
\mbf{\epsilon}_{\mbf{b}}
\end{bmatrix} \\\end{split}\]
(10)\[\begin{split}\begin{bmatrix}
U^{*} - W{\VStar}^{-1}W^{\top} & 0 \\
W^{\top} & \VStar
\end{bmatrix}
\begin{bmatrix}
\Da \\
\Db
\end{bmatrix}
=
\begin{bmatrix}
\mbf{\epsilon}_{\mbf{a}} - W{\VStar}^{-1}\mbf{\epsilon}_{\mbf{b}} \\
\mbf{\epsilon}_{\mbf{b}}
\end{bmatrix}\end{split}\]
という形にすることができる.ここから2つの方程式を取り出す.すると, (10) において左辺の行列の右上が \(0\) になったことから, \(\Db\) を含まない \(\Da\) についての式 (11) を得ることができる.
(11)\[(U^{*} - W{\VStar}^{-1}W^{\top}) \Da
= \mbf{\epsilon}_{\mbf{a}} - W{\VStar}^{-1}\mbf{\epsilon}_{\mbf{b}}\]
(12)\[\VStar \Db
= \mbf{\epsilon}_{\mbf{b}} - W^{\top} \Da\]
したがって,(11) を先に解き,得られた \(\Da\) を (12) に代入すれば \(\Db\) を得ることができる.
具体的な計算
前節では,LM法を分解し,より少ない計算量で更新量 \(\DP\) を求める方法を述べた.ここでは,実際にヤコビ行列 \(J\) を計算し,その具体的なかたちを求める.
まず,ヤコビ行列 \(J\) はスパースな行列になる.これは,\(\forall j \neq k\) について
\[\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{a}_{k}} = \mbf{0}\]
\(\forall i \neq k\) について
\[\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{b}_{k}} = \mbf{0}\]
が成り立つためである.
例えば,\(n=4\) ,\(m=3\) のとき, \(A_{ij}=\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{a}_{j}}\) , \(B_{ij}=\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{b}_{i}}\) とおけば,\(J\) は
(13)\[\begin{split}J = \begin{bmatrix}
A_{11} & \mbf{0} & \mbf{0} & B_{11} & \mbf{0} & \mbf{0} & \mbf{0} \\
\mbf{0} & A_{12} & \mbf{0} & B_{12} & \mbf{0} & \mbf{0} & \mbf{0} \\
\mbf{0} & \mbf{0} & A_{13} & B_{13} & \mbf{0} & \mbf{0} & \mbf{0} \\
A_{21} & \mbf{0} & \mbf{0} & \mbf{0} & B_{21} & \mbf{0} & \mbf{0} \\
\mbf{0} & A_{22} & \mbf{0} & \mbf{0} & B_{22} & \mbf{0} & \mbf{0} \\
\mbf{0} & \mbf{0} & A_{23} & \mbf{0} & B_{23} & \mbf{0} & \mbf{0} \\
A_{31} & \mbf{0} & \mbf{0} & \mbf{0} & \mbf{0} & B_{31} & \mbf{0} \\
\mbf{0} & A_{32} & \mbf{0} & \mbf{0} & \mbf{0} & B_{32} & \mbf{0} \\
\mbf{0} & \mbf{0} & A_{33} & \mbf{0} & \mbf{0} & B_{33} & \mbf{0} \\
A_{41} & \mbf{0} & \mbf{0} & \mbf{0} & \mbf{0} & \mbf{0} & B_{41} \\
\mbf{0} & A_{42} & \mbf{0} & \mbf{0} & \mbf{0} & \mbf{0} & B_{42} \\
\mbf{0} & \mbf{0} & A_{43} & \mbf{0} & \mbf{0} & \mbf{0} & B_{43} \\
\end{bmatrix}\end{split}\]
となる.
では \(A_{ij}\) や \(B_{ij}\) の具体的なかたちを求めてみよう.姿勢パラメータ \(\mbf{a}_{j} = \left[ \mbf{t}_{j}, \mbf{\omega}_{j} \right]\) に関する微分 \(A_{ij}=\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{a}_{j}}\) は次のようになる.
\[\begin{split}\begin{align}
\frac{\partial \hat{\mbf{x}}_{ij}}{\partial \mbf{t}_{j}}
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\cdot
\frac{\partial (R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{v})}{\partial \mbf{v}}
\bigg\rvert_{\mbf{v}=\mbf{t}_j} \\
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\end{align}\end{split}\]
\[\begin{split}\begin{align}
\frac{\partial \hat{\mbf{x}}_{ij}}{\partial \mbf{\omega}_{j}}
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\cdot
\frac{\partial (R(\mbf{v})\mbf{b}_{i} + \mbf{t}_{j})}{\partial \mbf{v}}
\bigg\rvert_{\mbf{v}=\mbf{\omega}_{j}} \\
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\cdot
\frac{\partial (R(\mbf{v})\mbf{b}_{i})}{\partial \mbf{v}}
\bigg\rvert_{\mbf{v}=\mbf{\omega}_{j}}
\end{align}\end{split}\]
ここで, \(\frac{\partial (R(\mbf{v})\mbf{b}_{i})}{\partial \mbf{v}}\) はGallegoら による計算結果を用いることができる.
\[\frac{\partial (R(\mbf{v})\mbf{b}_{i})}{\partial \mbf{v}}
= -R(\mbf{v}) \left[ \mbf{b}_{i} \right]_{\times}
\frac{
\mbf{v}\mbf{v}^{\top} +
(R(\mbf{v})^{\top} - I) \left[ \mbf{v} \right]_{\times}
}{||\mbf{v}||^{2}}\]
3次元点の座標 \(\mbf{b}_{i}\) に関する微分 \(B_{ij}=\frac{\partial Q(\mbf{a}_{j}, \mbf{b}_{i})}{\partial \mbf{b}_{i}}\) は次のようになる.
\[\begin{split}\begin{align}
\frac{\partial \hat{\mbf{x}}_{ij}}{\partial \mbf{b}_{i}}
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\cdot
\frac{\partial (R(\mbf{\omega}_{j})\mbf{v} + \mbf{t}_{j})}{\partial \mbf{v}}
\bigg\rvert_{\mbf{v}=\mbf{b}_{i}} \\
&= \frac{\partial \pi(\mbf{p})}{\partial \mbf{p}}
\bigg\rvert_{\mbf{p}=R(\mbf{\omega}_{j})\mbf{b}_{i} + \mbf{t}_{j}}
\cdot
R(\mbf{\omega}_{j})
\end{align}\end{split}\]
以上より, \(A_{ij}\) と \(B_{ij}\) の具体的なかたちを求めることができた.あとは,
上記で得られた \(A_{ij}\) と \(B_{ij}\) (13) に代入して \(J\) を求める
(8) にしたがって \(U,V,W\) を求める
(11) と (12) によって姿勢パラメータ \(\mbf{a}\) と3次元点の座標 \(\mbf{b}\) それぞれについての更新量 \(\Da\) と \(\Db\) を求める
という3つのステップによって更新量を求めることができる.
計算量の削減
線型方程式 \(\mbf{y} = A\mbf{x},\; \mbf{x} \in \mathbb{R}^{n}, \mbf{y} \in \mathbb{R}^{m}, A \in \mathbb{R}^{n \times m}\) の解は
\[\begin{split}\begin{align}
\mbf{x}
&= (A^{\top}A)^{-1}A^{\top}\mbf{y} \\
&= K^{-1}A^{\top}\mbf{y} \\
K &= A^{\top}A,
K \in \mathbb{R}^{n \times n}
\end{align}\end{split}\]
によって得られるが,行列 \(K\) のサイズが大きくなると解を求めるための計算量が急激に増加する.これは, \(n \times n\) 行列の逆行列を計算するアルゴリズムが \(O(n^{2.3})\) 〜 \(O(n^{3})\) 程度の計算量をもつことに起因する .したがって,線型方程式を高速に解くには,問題の構造を見極め, \(K\) の逆行列を直接計算することを避けて計算量を減らす必要がある.
SBAでは,
(6) を直接解くのではなく,それを分割して得た
(11) と
(12) をそれぞれ解くことで
\(\DP\) を得ている.さらに,
\(\VStar\) がスパースであるという性質に基づいて計算量を大幅に削減している.
(13) で定義された
\(J\) を用いて
\(\VStar\) を計算すると次のようになる.
\[\begin{split}\VStar = \begin{bmatrix}
\VStar_{1} & 0 & 0 & 0 \\
0 & \VStar_{2} & 0 & 0 \\
0 & 0 & \VStar_{3} & 0 \\
0 & 0 & 0 & \VStar_{4} \\
\end{bmatrix}\end{split}\]
ただし \(\VStar_{i}\) は
\[\begin{split}\begin{align}
V_{i}
&= \sum_{j=1}^{m} B_{ij}^{\top} \Cov_{ij}^{-1} B_{ij} \\
\VStar_{i}
&= V_{i} + \lambda I.
\end{align}\end{split}\]
である.
(11) には \({\VStar}\) の逆行列が両辺に含まれている.また, (12) を解いて \(\Db\) を得る際にも両辺に左から \({\VStar}\) の逆行列をかける必要がある.\(\VStar\) のサイズが大きいとその逆行列を求めるのに多大なコストがかかってしまう.しかし, \(\VStar\) がスパースな行列であることに着目すると, \(\VStar\) の逆行列は
(14)\[\begin{split}{\VStar}^{-1} = \begin{bmatrix}
{\VStar}^{-1}_{1} & 0 & 0 & 0 \\
0 & {\VStar}^{-1}_{2} & 0 & 0 \\
0 & 0 & {\VStar}^{-1}_{3} & 0 \\
0 & 0 & 0 & {\VStar}^{-1}_{4} \\
\end{bmatrix}\end{split}\]
となるため, \(\VStar_{i},i=1,\dots,n\) のそれぞれについて逆行列を求めればよいことがわかる.結果として \(\VStar\) の逆行列の計算量はランドマーク数 \(n\) に対して線型に増加することになり, \(\VStar\) の逆行列を直接求めるのと比較すると計算量を一気に削減できる.
\(\Da\) を求める際には,
\(S = U^{*} - W{\VStar}^{-1}W^{\top}\) の逆行列を
(11) の両辺に左からかける必要がある.しかし,一般的にランドマーク数
\(n\) よりもカメラの視点数
\(m\) の方が圧倒的に小さい
\((m \ll n)\) ため,
\(S\) のサイズは
\(\VStar\) と比べると圧倒的に小さい.したがって,
\(S\) の逆行列を求める処理は全体の計算量にはほとんど影響しない.
問題のサイズ(視点数や復元対象となるランドマークの数)が大きいときは,
(6) を直接解いて
\(\DP\) を得るよりも,
(11) (12) (14) によって
\(\Da\) と
\(\Db\) をそれぞれ計算し結合することで
\(\DP\) を得るほうが圧倒的に高速である.