「se3」タグアーカイブ

Expmapを使った最適化（ポーズグラフ編）

前回はexpmapを使った点に作用する姿勢の最適化を行ったが，今回は姿勢同士の作用に対する最適化について書いてみる．まずは簡単に現在の推定姿勢を $P_0$ として，目標の姿勢 $P_1$ に一致するように推定姿勢を更新してみる．

Expmapを使った姿勢最適化（ICP編）

前回Expmapがなぜ最適化に使われるのかを書いたが，今回は実際にどのようにExpmapを使って姿勢の最適化ができるのか書いてみようと思う．Expmapの微分の導出などに関しては[1] http://ethaneade.com/lie.pdf を参照してほしい．

単純な例としてまずICPを考えよう．ICPでは点 $x_i$ とその最近傍点 $x_i'$ の距離を最小化する相対姿勢 $T$ を求める．つまり，ある点 $x_i$ に対する誤差関数は $e_i = T x_i - x_i'$ となり，点群全体における誤差その二乗和で与えられる．この誤差関数を最小化する相対姿勢 $T$ は実際にはクローズドフォームで求めることができるが，ここではあえてExpmapを使いガウスニュートンで求めてみる．