おべんきょうwiki
GraphSLAM
最終更新:
Bot(ページ名リンク)
-
view
GraphSLAM
各時刻において取得されたデータから相対姿勢を計算して得られるポーズグラフを最適化する手法.
ここでは例によって使うために必要な情報だけまとめる.
ここでは例によって使うために必要な情報だけまとめる.
初期姿勢列
まず各時系列での姿勢を推定しておく.ICPなどでよい.
この時の姿勢列を次のように定める.
この時の姿勢列を次のように定める.
ポーズグラフ生成
時刻
間の相対姿勢が推定できるとき,次のように表す.
この相対姿勢は例えばSIFTなどで直接計測値から求めることができる.
一方で最初に作った姿勢列をたどれば同様に求めることができ,
この2つの間には計測誤差や累積誤差などにより差が生じる.
この差を次のように表すことができる.
一方で最初に作った姿勢列をたどれば同様に求めることができ,
この2つの間には計測誤差や累積誤差などにより差が生じる.
この差を次のように表すことができる.
ここで
は姿勢変換を表す.つまり
間の相対姿勢を2つの方法で求めたものの誤差を表している.
情報行列
誤差共分散行列
と先ほど求めた誤差
を用いると,
やりたいことは次式を最小化する
を求めることである.
やりたいことは次式を最小化する
で,最小化のために
のヤコビアン
を求めるが,
このヤコビアンは
に関係する以外の成分はすべて零である.
このヤコビアンは
つまり
はそれぞれ
ノードのヤコビアン.
で,なんやかんやで情報行列
を用いて以下のような形式に落ち着くわけだが
(なんやかんやはなんやかんやなので元の論文を見てください)
(なんやかんやはなんやかんやなので元の論文を見てください)
これを解いて
を加えて現在の状態を更新すれば良い.
なお情報行列はさっきのヤコビアンを使うとやはり
ノードのところ以外すべて零とおいたものの総和になる.
マニフォールド
要するに非ユークリッド空間で考えてみるという事で,
の代わりに
をつかう.
なおこの式の
つまり実部は(この論文ではxyzwの並びになっているので)最後にくっつけ直す形になっている.
で,これをつかってヤコビアンを作りなおしてもう一回同じ事を考える.
マニフォールド版のヤコビアン
はユークリッド空間版のヤコビアンを使って次のように表される.
マニフォールド版のヤコビアン
なお,
あとは情報行列の組立やらなんやらは全く同じ.
情報源
チュートリアル.基本的にこの論文を参考に記事を書いている.
http://ais.informatik.uni-freiburg.de/publications/papers/grisetti10titsmag.pdf
http://ais.informatik.uni-freiburg.de/publications/papers/grisetti10titsmag.pdf