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

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