ロボティクスで姿勢系の最適化を行うとき最近はExpmapを使うのが定跡になっているが，なぜExpmapを使うのか・どんな良いことがあるかの記述は少ないように思う．今回は姿勢最適化のためのExpmapの利点について書いてみようと思う．

そもそも数学的な背景は置いておいてExpmapとは何かというと，最適化用途的には逐次最適化で得られた微小変換を正確に積分する方法と言っていいと思う．例えば，現在の姿勢(回転のみ：SO3)がだとして，これに対してガウス・ニュートンなどで得られたXYZ軸それぞれに対する回転量を加えたいとする (って書いたりする)．

安直には各軸に対して順番に回転をかけていくことが考えられる（）．しかし，実際にはでは各軸に対して同時に回転する変換を考えている．各軸まわりに順に回転していく方法は回転量が十分に小さいときには良い近似になるが，回転量が大きくなってくると誤差が大きくなるし，回転軸をどの順番で選んでいくかという問題もある．

もう少し良い方法としては，数値積分の要領で回転を細かく区切って適用していくことが考えられる．つまり，各軸周りの回転と表記するとして，をくらいに分割してと微小回転を積算していく．この方法なら各軸に対して細かく回転していくため全ての軸に対して同時に回転を考えたときに近い結果になるし，軸の回転順をどのようにとっても(XYZでもYXZでも)同じ結果になる．しかし，精度を上げるにはNを大きくする必要があり，その分処理も重たくなる．

Expmapはこの微小変換の積分を正確かつ効率的に行ってくれる．回転量に対してExpmapを適用したはを無限に細かく区切って累積して得られた回転変換と一致する．

上の動画は，1) 各軸を順に回転する方法，2) 各軸を細かく回転していく方法，3) Expmap，をそれぞれ使ったときに得られる回転変換を表している．順に回転する方法は回転量が大きくなるにつれて，他の方法に対して誤差が大きくなっていることがわかる．一方で，Expmapを使った結果は細かく回転していった結果とほぼ一致しており，正確に各軸周りの回転を積算できていることが確認できる．

これは回転だけでなく並進を含む場合(SE3)にも同じことが言える．SE3において微小変換が与えられたとして，これをもとに回転と同時に並進移動が行われた結果の姿勢変換を求めたい．動画の左では変換を求める際に回転と並進が独立した状態になっているため，変換量が大きくなったときに一定の方向(Y軸)にしか移動できていないことがわかる．一方で細かく変換する方法(中央)とExpmap(右)では，その瞬間における回転方向に基づいて並進量が正確に積算されていることがわかる．

このようにExpmapを利用すると，特に変分量が大きいとき(最適化の初期ステップなど)の姿勢の更新がより正確になり収束が良くなったりする．その他にもExpmapの逆変換を行うLogmap()は，ある回転に対して一意なベクトル表現へのマッピングを与えるため，同じ回転を表す表現が複数あるクォータニオンよりも取扱が良かったり(周期性の問題はあるが)，ゼロベクトルが単位変換を表すため誤差関数を単純にのノルムにしたりできるというのも便利な点である．さらにヤコビアンの計算も簡単になったりするということもあり，Expmapは非常に姿勢の最適化に適したものになっている．

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