座標変換の静的チェック

ロボティクスをやっていると複雑な座標変換を扱う必要がよく出てくる.こういうときによくやられるのが,座標系Aにある点p_aを座標系Bへ動かす変換をT_a_bと記述することにして,AからCへの変換を求めたいときはT_a_b * T_b_c = T_a_cという風に左右の項がチェインするように書けば混乱なく座標系変換を扱うことができる.

これはこれで実用上うまい方法ではあるが,あくまで変数名で変換の整合性を目視確認しているだけなので,うっかり間違いが起きる可能性はある.今回はこの変換の整合性の確認をコンパイラに静的やってもらおうと思って以下のようなコードを書いてみた.基本的にはboostなどの単位系の静的チェックと同じ要領で型システムで間違った変換をはじくようにしている.

続きを読む

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を使いガウスニュートンで求めてみる.

続きを読む

姿勢最適化におけるExpmap

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

続きを読む