ロボティクスをやっていると複雑な座標変換を扱う必要がよく出てくる．こういうときによくやられるのが，座標系Aにある点p_aを座標系Bへ動かす変換をT_a_bと記述することにして，AからCへの変換を求めたいときはT_a_b * T_b_c = T_a_cという風に左右の項がチェインするように書けば混乱なく座標系変換を扱うことができる．
これはこれで実用上うまい方法ではあるが，あくまで変数名で変換の整合性を目視確認しているだけなので，うっかり間違いが起きる可能性はある．今回はこの変換の整合性の確認をコンパイラに静的やってもらおうと思って以下のようなコードを書いてみた．基本的にはboostなどの単位系の静的チェックと同じ要領で型システムで間違った変換をはじくようにしている．
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace symbols {
struct A {};
struct B {};
struct C {};
}
template<typename T1, typename T2>
struct Transform {
template<typename T3, typename T4>
Transform<T1, T4> operator* (const Transform<T3, T4>& rhs) const {
static_assert(std::is_same_v<T2, T3>, "invalid transform multiplication");
return Transform<T1, T4> { transform* rhs.transform };
}
Transform<T2, T1> inverse() const {
return Transform<T2, T1>{ transform.inverse() };
}
Eigen::Isometry3d transform;
};
int main(int argc, char** argv) {
using namespace symbols;
Transform<A, B> T_a_b = { Eigen::Isometry3d::Identity() };
Transform<B, C> T_b_c = { Eigen::Isometry3d::Identity() };
Transform<B, A> T_b_a = T_a_b.inverse();
auto T_a_c = T_a_b * T_b_c; // Transform<A, C>
auto T_c_a = T_b_c.inverse() * T_b_a; // Transform<C, A>
auto T_invalid1 = T_b_c * T_b_a; // compile error
Transform<A, C> T_invalid2 = T_a_b * T_b_c; // compile error
return 0;
}
本当はさらに任意の数の変換を与えると，自動的に目的の変換をコンパイルタイムで見つける以下のような関数を書いてみたいと思っていた．
template<typename T1, typename T2, template... Args>
Transform<T1, T2> compose(Args... args) { ... }
Transform<C, A> T_c_a = compose<C, A>(T_a_b, T_b_c, T_b_a);
// automatically finds T_c_a = T_b_c.inverse() * T_a_b
が，テンプレートメタプログラミングの知識が足りずにやめた．アルゴリズム的にはシンボルを頂点として変換を辺としたグラフ上で目的の二つの座標をつなぐ経路を見つければよいのでそんなに難しくはないはずなのだが，テンプレート上でうまくグラフを構築する方法が思いつかず断念した．そのうち時間があるときにまたやってみようと思う．