おべんきょうwiki
GraphSLAM
最終更新:
yahirohumpty
-
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