「GraphSLAM」の編集履歴（バックアップ）一覧はこちら

GraphSLAM - (2013/11/09 (土) 19:17:02) の最新版との変更点

追加された行は緑色になります。

削除された行は赤色になります。

* GraphSLAM

各時刻において取得されたデータから相対姿勢を計算して得られるポーズグラフを最適化する手法．


** 初期姿勢列

まず各時系列での姿勢を推定しておく．ICPなどでよい．
この時の姿勢列を次のように定める．

$$ x_{1:T} = \{x_1 , x_2 , ... , x_T \} $$

三次元で考えると並進ベクトル$$ t _i ^ T $$とクォータニオン$$ q _i ^T $$をおいて，

$$ x_i ^T = ( t _i ^ T , q _i ^T )$$


** ポーズグラフ生成

時刻$$ i,j $$間の相対姿勢が推定できるとき，次のように表す．

$$ z_{i,j}^T = ( t _{i,j} ^ T , q _{i,j} ^T )$$

この相対姿勢は例えばSIFTなどで直接計測値から求めることができる．
一方で最初に作った姿勢列をたどれば同様に求めることができ，
この２つの間には計測誤差や累積誤差などにより差が生じる．
この差を次のように表すことができる．

$$ e_{i,j} (x_i, x_j) = z _{i,j} ^{-1} \oplus (x _i ^{-1} \oplus x_j) $$

ここで$$ \oplus $$は姿勢変換を表す．つまり$$ i,j $$間の相対姿勢を２つの方法で求めたものの差を表している．

$$ x _i ^{-1} \oplus x_j = ( q_i(t_j)^T, (q_i \cdot q_j)^T )^T $$







** 情報源

チュートリアル．基本的にこの論文を参考に記事を書いている．
http://ais.informatik.uni-freiburg.de/publications/papers/grisetti10titsmag.pdf

* GraphSLAM

各時刻において取得されたデータから相対姿勢を計算して得られるポーズグラフを最適化する手法．
ここでは例によって使うために必要な情報だけまとめる．


** 初期姿勢列

まず各時系列での姿勢を推定しておく．ICPなどでよい．
この時の姿勢列を次のように定める．

$$ x_{1:T} = \{x_1 , x_2 , ... , x_T \} $$

三次元で考えると並進ベクトル$$ t _i ^ T $$とクォータニオン$$ q _i ^T $$をおいて，

$$ x_i ^T = ( t _i ^ T , q _i ^T )$$
$$ x = (t_x, t_y, t_z, q_x, q_y, q_z, q_w)^T $$

** ポーズグラフ生成

時刻$$ i,j $$間の相対姿勢が推定できるとき，次のように表す．

$$ z_{ij}^T = ( t _{ij} ^ T , q _{ij} ^T )$$

この相対姿勢は例えばSIFTなどで直接計測値から求めることができる．
一方で最初に作った姿勢列をたどれば同様に求めることができ，
この２つの間には計測誤差や累積誤差などにより差が生じる．
この差を次のように表すことができる．

$$ e_{ij} (x_i, x_j) = e_{ij}(x) = z _{ij} ^{-1} \oplus (x _i ^{-1} \oplus x_j) $$

ここで$$ \oplus $$は姿勢変換を表す．つまり$$ i,j $$間の相対姿勢を２つの方法で求めたものの誤差を表している．

$$ x _i \oplus x_j = ( q_i(t_j)^T, (q_i \cdot q_j)^T )^T $$


** 情報行列

誤差共分散行列$$ \Omega _{ij} $$と先ほど求めた誤差$$ e_{ij} $$を用いると，
やりたいことは次式を最小化する$$ x $$を求めることである．

$$ F(x) = \sum _{(ij) \in C} e_{ij} ^T \Omega _{i,j} e_{ij} $$

で，最小化のために$$ e_{ij} $$のヤコビアン$$ J_{ij} $$を求めるが，
このヤコビアンは$$i,j$$に関係する以外の成分はすべて零である．

$$ J_{ij} = (0 \dots 0 \ A_{ij} \  0 \dots 0  \ B_{ij} \  0 \dots 0)  $$

つまり$$ A_{ij}, B_{ij} $$はそれぞれ$$i, j$$ノードのヤコビアン．

$$ A_{ij} = \frac{\partial e_{ij}(x)}{\partial x_i} $$
$$ B_{ij} = \frac{\partial e_{ij}(x)}{\partial x_j} $$


で，なんやかんやで情報行列$$ H = \sum H_{ij}$$を用いて以下のような形式に落ち着くわけだが
（なんやかんやはなんやかんやなので元の論文を見てください）

$$ J_{ij}^T \Omega _{ij} J_{ij} \Delta x = - e_{ij}^T \Omega _{ij} J_{ij} $$
$$ \sum H _{ij} \Delta x = - \sum b_{ij} $$

これを解いて$$ \Delta x $$を加えて現在の状態を更新すれば良い．

$$ x \leftarrow x + \Delta x $$

なお情報行列はさっきのヤコビアンを使うとやはり$$i,j$$ノードのところ以外すべて零とおいたものの総和になる．

$$ H_{ij} = \left( \begin{array}{ccccc}0 & 0 & 0 & 0 & 0 \\ 0 & A_{ij}^T \Omega _{ij} A_{ij} & 0 & A_{ij}^T \Omega _{ij} B_{ij} & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & B_{ij}^T \Omega _{ij} A_{ij} & 0 &  B_{ij}^T \Omega _{ij} B_{ij} & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \end{array} \right) $$

$$ b_{ij} = \left( \begin{array}{c} 0 \\ A_{ij}^T \Omega _{ij} e_{ij} \\ 0 \\ B_{ij}^T \Omega _{ij} e_{ij} \\ 0 \\ \end{array} \right) $$


** マニフォールド

要するに非ユークリッド空間で考えてみるという事で，
$$ x + \Delta x $$ の代わりに $$ x \boxplus \Delta \tilde{x} $$をつかう．

$$ x_i \boxplus \Delta \tilde{x}_i = x_i \oplus \left( \begin{array}{c} \Delta \tilde{t}_i \\ \Delta \tilde{q}_i \\ \sqrt{1- || \Delta \tilde{q}_i || ^2} \end{array} \right) $$
なおこの式の$$\tilde{q}$$は虚部だけを使っている．
つまり実部は（この論文ではxyzwの並びになっているので）最後にくっつけ直す形になっている．

$$ \tilde{x} = (t_x, t_y, t_z, q_x, q_y, q_z)^T $$

で，これをつかってヤコビアンを作りなおしてもう一回同じ事を考える．
マニフォールド版のヤコビアン$$ \tilde{J} _{ij} $$はユークリッド空間版のヤコビアンを使って次のように表される．

$$ \tilde{J}_{ij} = (0 \dots 0 \ \tilde{A}_{ij} \  0 \dots 0  \ \tilde{B}_{ij} \  0 \dots 0)  $$
$$ \tilde{A}_{ij} = A_{ij} M_i $$
$$ \tilde{B}_{ij} = B_{ij} M_j $$

なお，

$$ M_i = \left. \frac{x_i \boxplus \Delta \tilde{x_i}}{\partial \Delta \tilde{x_i}} \right| _{\Delta \tilde{x} = 0} $$
$$ M_j = \left. \frac{x_j \boxplus \Delta \tilde{x_j}}{\partial \Delta \tilde{x_j}} \right| _{\Delta \tilde{x} = 0} $$


あとは情報行列の組立やらなんやらは全く同じ．


** 情報源

チュートリアル．基本的にこの論文を参考に記事を書いている．
http://ais.informatik.uni-freiburg.de/publications/papers/grisetti10titsmag.pdf