mrpt::poses::CPose2Dのメモ

MRPTには2次元/3次元空間での姿勢を扱うのに便利なCPose2D/CPose3Dクラスがあるのですが、これの取り扱いをずっと勘違いしていて嵌ってしまったのでメモ。

相対の姿勢移動と移動後の姿勢から元の姿勢を求める

姿勢Aの状態から相対的な姿勢Bだけ移動した姿勢Cは、MRPTでは次のように書きます。

C = A + B

逆にCとBから元々の姿勢Aを求めたい場合、次のようにします。

A = C + (-B)

A = C - Bではダメです。計算結果が変わってしまいます。

実装の実態

-Bと+(-B)で結果が変わるのはかなり気持ち悪いです。なぜこのようになるか調べてみました。チュートリアルを読んでみると、これはA+Bが同時変換行列の掛け算で実装されていることに起因するようです。

それぞれの姿勢を同時変換行列A,B,Cで表した場合

C = A + B ⇔C = A・B

となります。また、この式とB = C - Aという実装の定義から

B = inv(A)・C ⇔ B = C - A

となります。A = C・inv(B)ですから、A = C - Bではダメだということがわかります。そこでAを求めるために-B⇔inv(B)を利用して、

A = C + (-B)

と記述する必要が出てきてしまうのです。

同次変換だと知ってしまえば理解できるのですが、記述一つで結果が変わってしまうのはバグを生みやすいので注意が必要だと感じました。

もっと綺麗な方法を知ってる人がいたら教えてください。