VO, VSLAM, SfMの違い

SfM

時間的に連続していない画像セットからカメラposeと対象物の3D形状を推定。
オフラインで動く

VO

時間的に連続した画像からlocally consistentなトラジェクトリを生成。
時間的に連続した画像には同じ静止物が継続して写っていることを想定。
オンラインで動く

VSLAM

時間的に連続した画像からglobally consistentなトラジェクトリを生成。
オンラインで動く
特徴点座標をずっとメモリに保存しておき、loop closureやグラフ最適化を行う。

VOとVSLAMを比較すると、VSLAMはglobally consistentな特徴点が得られるがその分計算量は多い。

VOの詳細

VOの目的は2つの連続する時刻 $t=t^{k}$ と $t=t^{k+1}$ の間でのカメラ姿勢の変換 $T_{k}^{k+1}$ を求めることである。求め方は主に2つある。

Feature-based method

$t=t^{k}$ と $t=t^{k+1}$ の画像で観測された同じ点 $\vec{u}^{i}$ と $\vec{u}^{i+1}$ を比較し、その辻褄が合うように $T_{k}^{k+1}$ を求める方法である。このとき、特徴点の3次元座標 $\vec{p}^{i}$ は既知であるとする。projection関数を $\pi$ とすると、

$\begin{eqnarray} T_{k}^{k+1} = \mathrm{argmin}_{T} \sum_{i}|| \vec{u}^{i+1} - \pi(T\vec{p}^{i}) || ^{2} \end{eqnarray}$

ただし深さ方向の自由度は決定できないので別手法で測定する必要がある。これはどういうことかというと、 $t=T^{k}$ の観測された2点を使ってmatchingする場合、Aにおいて[tex: P{A1}]と[tex: P{A2}]の2点があったというケースと、Bにおいて[tex: P{B1}]と[tex: P{B2}]の2点があったというケースでどちらも画像における特徴点位置は同じであり、両者が区別できないからである。

f:id:salpik:20211018234841p:plain — 深さ方向の自由度が決定できない理由

メリット

2つの画像間のTransformが大きくても同じ点が観測される限り使える

Direct method

$I$ をピクセル強度（0から255までの値）として、

$\begin{eqnarray} T_{k}^{k+1} &=& \mathrm{argmin}_{T} \sum_{i}|| I^{k+1}(\vec{u}^{i'}) - I^{k}(\vec{u}^{i}) || ^{2} \\ &=& \mathrm{argmin}_{T} \sum_{i}|| I^{k+1}(\pi(T\vec{p}^{i})) - I^{k}(\vec{u}^{i}) || ^{2} \\ &=& \mathrm{argmin}_{T} \sum_{i}|| I^{k+1}(\pi(T \pi^{-1}(\vec{u}^{i})d)) - I^{k}(\vec{u}^{i}) || ^{2} \\ \end{eqnarray}$

ここでは物体までの距離 $d$ は別のセンサーなどで観測済みであるとした。

メリット

フレームレートが大きいと前フレームからのTransform変位があまりないので前回値がほぼそのまま解として使える。

Matchingによるpose estimation

2D to 2D

最初の段階ではまだ特徴点の3次元座標が決まっていないので、異なるタイムスタンプの画像平面上で特徴点座標を比較する。

3D to 2D

ある程度時間が経って特徴点の3次元座標が決まってくると、その3次元座標と画像上の特徴点位置を比較する。

3D to 3D

ステレオカメラなどで三角測量により1タイムスタンプから3次元位置がわかるときは、異なるタイムスタンプ同士の3次元座標同士を比較する。

Adjustment

Pose adjustment

異なるフレーム間の相対Transformを拘束条件として全体のtrajectoryを調整すること

Bundle adjustment

異なるフレーム間の相対Transformだけでなく、feature pointsの3次元位置も合わせて調整（最適化）すること。Bundle adjustmentの方が精度が高いが計算コストが高い。自由度が多いので最適化に使う初期解の精度が重要。

Bundle adjustmentの定式化

問題設定

2つの点を3つの異なる位置から撮影した画像があるとし、その3次元点座標とカメラ位置姿勢を推定する問題とする。 world座標系に $\vec{p}_{A}^{w}, \vec{p}_{B}^{w}$ があり、3つの位置 $a, b, c$ から観測した画像位置をそれぞれ $\vec{u}_{A}^{a}, \vec{u}_{B}^{a}$ 、 $\vec{u}_{A}^{b}, \vec{u}_{B}^{b}$ 、 $\vec{u}_{A}^{c}, \vec{u}_{B}^{c}$ とする。\ このとき $\vec{p}_{A}^{w}, \vec{p}_{B}^{w}$ およびカメラ位置姿勢 $T_{a}^{w}, T_{b}^{w}, T_{c}^{w}$ を求めたい。

f:id:salpik:20211107171325p:plain — 問題設定

コスト関数

画像平面上で実際に観測されたピクセル座標と、3次元座標の位置をprojectionしたピクセル座標の距離をコスト関数 $C$ としこれを最小化する。例えば特徴点 $A$ の位置 $a$ におけるコスト関数は

$\begin{eqnarray} C_{A}^{a} \equiv || \vec{e}_{A}^{a} ||^{2} \equiv || \vec{u}_{A}^{a} - \pi (T_{w}^{a} \vec{p}_{A}^{w}) ||^{2} \end{eqnarray}$

と表される。ここで

$\begin{eqnarray} \pi(x, y, z) \equiv \frac{1}{z} \left( \begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \end{array}\right) \left( \begin{array}{c} x \\ y \\ z \end{array}\right) \end{eqnarray}$

はカメラ座標の点をピンホールカメラを使って画像上の点に射影する関数である。\ 全体としてのコスト関数は

$\begin{eqnarray} C &\equiv& \sum_{i=a, b, c. I=A, B} C_{I}^{i} \\ &=& (\vec{e}_{A}^{a \mathrm{T}}, \vec{e}_{B}^{a \mathrm{T}}, \vec{e}_{A}^{b \mathrm{T}}, \vec{e}_{B}^{b \mathrm{T}}, \vec{e}_{A}^{c \mathrm{T}}, \vec{e}_{B}^{c \mathrm{T}}) \left( \begin{array}{c} \vec{e}_{A}^{a} \\ \vec{e}_{B}^{a} \\ \vec{e}_{A}^{b} \\ \vec{e}_{B}^{b} \\ \vec{e}_{A}^{c} \\ \vec{e}_{B}^{c} \end{array}\right) \end{eqnarray}$

一般的に誤差の重みが異なる場合も考慮して

$\begin{eqnarray} C &=& (\vec{e}_{A}^{a \mathrm{T}}, \vec{e}_{B}^{a \mathrm{T}}, \vec{e}_{A}^{b \mathrm{T}}, \vec{e}_{B}^{b \mathrm{T}}, \vec{e}_{A}^{c \mathrm{T}}, \vec{e}_{B}^{c \mathrm{T}}) \left( \begin{array}{ccc} \lambda_{1}^{2} & 0 & \dots \\ 0 & \lambda_{2}^{2} & 0 \\ \vdots & & \end{array}\right) \left( \begin{array}{c} \vec{e}_{A}^{a} \\ \vec{e}_{B}^{a} \\ \vec{e}_{A}^{b} \\ \vec{e}_{B}^{b} \\ \vec{e}_{A}^{c} \\ \vec{e}_{B}^{c} \end{array} \right) \\ &\equiv & \vec{e}^{\mathrm{T}} H^{2} \vec{e} \end{eqnarray}$

ガウス ニュートンによる最適化

最適化変数は $A, B$ の座標と $a, b, c$ のカメラ位置姿勢であるが、これをまとめて $X$ と記述することにする。 $X$ の現在の値を $X_{0}$ としこの値を反復的に更新していきたいので、その更新量 $\Delta X$ を求めたい。 $\vec{e}$ を $X_{0}$ まわりでテーラー展開すると

$\begin{eqnarray} \vec{e} = \vec{e}(X_{0}) + \frac{\partial \vec{e}}{\partial X} (X-X_{0}) \end{eqnarray}$

これをコスト関数に代入して

$\begin{eqnarray} C &=& \left[ \vec{e}(X_{0}) + \frac{\partial \vec{e}}{\partial X} (X-X_{0}) \right] ^\mathrm{T} H^{2} \left[ \vec{e}(X_{0}) + \frac{\partial \vec{e}}{\partial X} (X-X_{0})\right] \\ &=& \vec{e}(X_{0})^\mathrm{T} H^{2} \vec{e}(X_{0}) + 2\vec{e}(X_{0})^\mathrm{T}H^{2} \frac{\partial \vec{e}}{\partial X} (X-X_{0}) + (X-X_{0})^\mathrm{T} \frac{\partial \vec{e}}{\partial X}^\mathrm{T} H^{2} \frac{\partial \vec{e}}{\partial X} (X-X_{0}) \\ \end{eqnarray}$

$C$ は $X$ に対して極値となっている必要があるから、

$\begin{eqnarray} \vec{0}^{\mathrm{T}} &=& \frac{\partial C}{\partial X} \\ &=& 2\vec{e}(X_{0})^\mathrm{T}H^{2} \frac{\partial \vec{e}}{\partial X} + 2(X-X_{0})^\mathrm{T} \frac{\partial \vec{e}}{\partial X}^\mathrm{T} H^{2} \frac{\partial \vec{e}}{\partial X} \\ \vec{0} &=& \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \vec{e}(X_{0}) + \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \frac{\partial \vec{e}}{\partial X} (X-X_{0}) \\ (X-X_{0}) &=& -\left[ \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \frac{\partial \vec{e}}{\partial X} \right]^{-1} \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \vec{e}(X_{0}) \\ X &=& X_{0} - \left[ \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \frac{\partial \vec{e}}{\partial X} \right]^{-1} \frac{\partial \vec{e}}{\partial X}^\mathrm{T}H^{2} \vec{e}(X_{0}) \\ &=& X_{0} - (J^{\mathrm{T}} J)^{-1} J^{\mathrm{T}} H \vec{e}(X_{0}) \ \ \ \ \ \ (J \equiv H\frac{\partial \vec{e}}{\partial X}) \end{eqnarray}$

となり、このように $X$ を更新していけば良いことがわかった。

微分のチェーンルール定義

$\vec{a}^{\mathrm{T}}=(a_{1}, a_{2}, a_{3})$ 、 $\vec{b}^{\mathrm{T}}=(b_{1}, b_{2})$ に対して

$\begin{eqnarray} \frac{\partial \vec{a}}{\partial \vec{b}} \equiv \left( \begin{array}{cc} \frac{\partial a_{1}}{\partial b_{1}} & \frac{\partial a_{1}}{\partial b_{2}} \\ \frac{\partial a_{2}}{\partial b_{1}} & \frac{\partial a_{2}}{\partial b_{2}} \\ \frac{\partial a_{3}}{\partial b_{1}} & \frac{\partial a_{3}}{\partial b_{2}} \end{array}\right) \end{eqnarray}$

と定義する。こうすると

$\begin{eqnarray} \frac{\partial \vec{a}}{\partial \vec{b}} \frac{\partial \vec{b}}{\partial \vec{c}} &=& \left( \begin{array}{cc} \frac{\partial a_{1}}{\partial b_{1}} & \frac{\partial a_{1}}{\partial b_{2}} \\ \frac{\partial a_{2}}{\partial b_{1}} & \frac{\partial a_{2}}{\partial b_{2}} \\ \frac{\partial a_{3}}{\partial b_{1}} & \frac{\partial a_{3}}{\partial b_{2}} \end{array}\right) \left( \begin{array}{ccc} \frac{\partial b_{1}}{\partial c_{1}} & \frac{\partial b_{1}}{\partial c_{2}} & \frac{\partial b_{1}}{\partial c_{3}} \\ \frac{\partial b_{2}}{\partial c_{1}} & \frac{\partial b_{2}}{\partial c_{2}} & \frac{\partial b_{2}}{\partial c_{3}} \end{array}\right) \\ &=& \left( \begin{array}{ccc} \frac{\partial a_{1}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{1}} + \frac{\partial a_{1}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{1}} & \frac{\partial a_{1}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{2}} + \frac{\partial a_{1}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{2}} & \frac{\partial a_{1}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{3}} + \frac{\partial a_{1}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{3}} \\ \frac{\partial a_{2}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{1}} + \frac{\partial a_{2}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{1}} & \frac{\partial a_{2}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{2}} + \frac{\partial a_{2}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{2}} & \frac{\partial a_{2}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{3}} + \frac{\partial a_{2}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{3}} \\ \frac{\partial a_{3}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{1}} + \frac{\partial a_{3}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{1}} & \frac{\partial a_{3}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{2}} + \frac{\partial a_{3}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{2}} & \frac{\partial a_{3}}{\partial b_{1}}\frac{\partial b_{1}}{\partial c_{3}} + \frac{\partial a_{3}}{\partial b_{2}}\frac{\partial b_{2}}{\partial c_{3}} \\ \end{array}\right) \\ &=& \left( \begin{array}{ccc} \frac{\partial a_{1}}{\partial c_{1}} & \frac{\partial a_{1}}{\partial c_{2}} & \frac{\partial a_{1}}{\partial c_{3}} \\ \frac{\partial a_{2}}{\partial c_{1}} & \frac{\partial a_{2}}{\partial c_{2}} & \frac{\partial a_{2}}{\partial c_{3}} \\ \frac{\partial a_{3}}{\partial c_{1}} & \frac{\partial a_{3}}{\partial c_{2}} & \frac{\partial a_{3}}{\partial c_{3}} \end{array}\right) \\ &=& \frac{\partial \vec{a}}{\partial \vec{c}} \end{eqnarray}$

となり、チェーンルールが使えることがわかる。

$J$ の求め方

ここからはどのように $J$ を求めていけば良いかを考えていく。まず、

$\begin{eqnarray} \vec{e} &=& \vec{u} - \vec{\hat{u}} \\ &=& \vec{u} - K\vec{\hat{p}} \ \ \ \ \ (\vec{\hat{u}} \equiv K\vec{\hat{p}}) \\ &=& \vec{u} - K\frac{\vec{p}}{z} \ \ \ \ \ (\vec{\hat{p}} \equiv \frac{\vec{p}}{z}) \\ &=& \vec{u} - K\frac{R_{w}^{a}\vec{p}_{A}^{w}+\vec{t}_{w}^{a}}{z} \ \ \ \ \ (\vec{p} \equiv R_{w}^{a}\vec{p}_{a}^{w} + \vec{t}_{w}^{a}) \\ \end{eqnarray}$

となる。ここで、関係式を整理しておくと、

$\begin{eqnarray} \vec{\hat{u}} &=& K\vec{\hat{p}} \\ &=& \left( \begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \end{array}\right) \\ &=& \left( \begin{array}{c} f_{x}\hat{x} + c_{x} \\ f_{y}\hat{y} + c_{y} \end{array}\right) \\ \vec{\hat{p}} &=& \left( \begin{array}{c} \frac{x}{z} \\ \frac{y}{z} \end{array}\right) \end{eqnarray}$

これを用いると、特徴点座標に関するJは

$\begin{eqnarray} \frac{\partial \vec{e}}{\partial \vec{p}_{A}^{w}} &=& \frac{\partial \vec{e}}{\partial \vec{\hat{u}}} \frac{\partial \vec{\hat{u}}}{\partial \vec{\hat{p}}} \frac{\partial \vec{\hat{p}}}{\partial \vec{p}} \frac{\partial \vec{p}}{\partial \vec{p}_{A}^{w}} \\ &=& \left( \begin{array}{cc} -1 & 0 \\ 0 & -1 \end{array}\right) \left( \begin{array}{cc} f_{x} & 0 \\ 0 & f_{y} \end{array}\right) \left( \begin{array}{ccc} \frac{1}{z} & 0 & -\frac{x}{z^{2}} \\ 0 & \frac{1}{z} & -\frac{y}{z^{2}} \end{array}\right) R_{w}^{a} \end{eqnarray}$

であり、カメラの並進部分に関するJは

$\begin{eqnarray} \frac{\partial \vec{e}}{\partial \vec{t}_{w}^{a}} &=& \frac{\partial \vec{e}}{\partial \vec{\hat{u}}} \frac{\partial \vec{\hat{u}}}{\partial \vec{\hat{p}}} \frac{\partial \vec{\hat{p}}}{\partial \vec{p}} \frac{\partial \vec{p}}{\partial \vec{t}_{w}^{a}} \\ &=& \left( \begin{array}{cc} -1 & 0 \\ 0 & -1 \end{array}\right) \left( \begin{array}{cc} f_{x} & 0 \\ 0 & f_{y} \end{array}\right) \left( \begin{array}{ccc} \frac{1}{z} & 0 & -\frac{x}{z^{2}} \\ 0 & \frac{1}{z} & -\frac{y}{z^{2}} \end{array}\right) I \end{eqnarray}$

であり、カメラの回転部分に関するJは

$\begin{eqnarray} \frac{\partial \vec{e}}{\partial R_{w}^{a}} &=& \frac{\partial \vec{e}}{\partial \vec{\hat{u}}} \frac{\partial \vec{\hat{u}}}{\partial \vec{\hat{p}}} \frac{\partial \vec{\hat{p}}}{\partial \vec{p}} \frac{\partial \vec{p}}{\partial R_{w}^{a}} \\ &=& \left( \begin{array}{cc} -1 & 0 \\ 0 & -1 \end{array}\right) \left( \begin{array}{cc} f_{x} & 0 \\ 0 & f_{y} \end{array}\right) \left( \begin{array}{ccc} \frac{1}{z} & 0 & -\frac{x}{z^{2}} \\ 0 & \frac{1}{z} & -\frac{y}{z^{2}} \end{array}\right) \frac{\partial \vec{p}}{\partial R_{w}^{a}} \end{eqnarray}$

となり、最後の項をどう計算すればよいかがわからない。ここで、 $R_{w}^{a}$ が直行行列であることから、 $R_{w}^{a}$ をあるパラメータ $t$ によって連続的に変化する行列であるとすると、

$\begin{eqnarray} R_{w}^{a}(t) (R_{w}^{a}(t))^\mathrm{T} &=& I \\ \dot{R}_{w}^{a}(t) (R_{w}^{a}(t))^\mathrm{T} + R_{w}^{a}(t) (\dot{R}_{w}^{a}(t))^\mathrm{T} &=& 0 \\ \dot{R}_{w}^{a}(t) (R_{w}^{a}(t))^\mathrm{T} &=& -R_{w}^{a}(t) (\dot{R}_{w}^{a}(t))^\mathrm{T} \\ &=& - \left[ \dot{R}_{w}^{a}(t) R_{w}^{a}(t) \right]^\mathrm{T} \\ \end{eqnarray}$

となるので、 $\dot{R}_{w}^{a}(t) (R_{w}^{a}(t))^\mathrm{T}$ は交代行列であることがわかる。したがって

$\begin{eqnarray} \lfloor \vec{\theta} \rfloor \equiv \left( \begin{array}{ccc} 0 & -\theta_{3} & \theta_{2} \\ \theta_{3} & 0 & -\theta_{1} \\ -\theta_{2} & \theta_{1} & 0 \end{array}\right) \end{eqnarray}$

と定義される $\lfloor \vec{\theta} \rfloor$ を用いて

$\begin{eqnarray} \dot{R}_{w}^{a}(t) (R_{w}^{a}(t))^\mathrm{T} &=& \lfloor \vec{\theta} \rfloor \\ \dot{R}_{w}^{a}(t) &=& \lfloor \vec{\theta} \rfloor R_{w}^{a}(t) \\ R_{w}^{a}(t) &=& R_{w}^{a}(0) \exp(\lfloor \vec{\theta} \rfloor) \\ &\sim& R_{w}^{a}(0)(I + \lfloor \vec{\theta} \rfloor) \end{eqnarray}$

これを用いて、先ほどの $J$ の計算において $R_{w}^{a}$ ではなく $\vec{\theta}_{w}^{a}$ で微分することを考えると、

$\begin{eqnarray} \frac{\partial \vec{p}}{\partial \vec{\theta}_{w}^{a}} &=& \frac{R_{w}^{a}(I + \lfloor \Delta \vec{\theta}_{w}^{a} \rfloor)\vec{p}_{a}^{w} - R_{w}^{a}\vec{p}_{a}^{w}}{\Delta \vec{\theta}_{w}^{a}} \\ &=& \frac{R_{w}^{a} \lfloor \Delta \vec{\theta}_{w}^{a} \rfloor \vec{p}_{a}^{w}}{\Delta \vec{\theta}_{w}^{a}} \\ &=& - \frac{R_{w}^{a} \lfloor \vec{p}_{a}^{w} \rfloor \Delta \vec{\theta}_{w}^{a}}{\Delta \vec{\theta}_{w}^{a}} \ \ \ \ \ \ (「公式まとめ」のブログを参照) \\ &=& - R_{w}^{a} \lfloor \vec{p}_{a}^{w} \rfloor \end{eqnarray}$

と微分が計算できることがわかった。

Reference

https://www.rsj.or.jp/databox/international/iros16tutorial_2.pdf

salpikのブログ

VIO, VSLAM, SfM