Eigen Mapの使い方
動機
最近何かと話題のROS (Robot Operating System)ですが,私も常用しています.もうやめられません.
ROSではプログラムを主にC++かPythonで書きますが,私はC++ばっかり書いてます. Pythonの書き方知らないので
さて,ロボット系では色々な場面で行列演算を行いますが,C++ではEigenという便利な行列演算ライブラリがあります.便利です.わざわざ行列演算にするほどのことでもない計算を,わざわざEigenにやらせてしまいます. その際にC++の配列 (std::vectorなど) からEigenのベクトル型に変換することになります. 今回はその方法について軽く書きます.
書き方-基本-
C++の配列からEigenのベクトルへの変換はEigen::Mapを使います. 変換コードは以下のようになります. (色々飛ばしてます.)
#include <Eigen/Core> std::vector<double> a(3); Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd>(&a[0],a.size());
for文で回さなくて済むので重宝してます.
ROSでハマったところ
ROSのコールバック関数で受け取るメッセージはconst指定で受け取らないとコンパイル時にエラーが返されると思います. 受け取ったメッセージ内の配列に対して上記のコードを適用すると,型が違うとコンパイラに怒られます. 調べるのがメンドウだったので今まで関数内で宣言した通常変数にコピーしてからMapを使用していたのですが,さすがに汚いのでgoogle先生に聞いたら普通に出てきました.
Eigen::Map<> on const array • KDE Community Forums
Oops!… 蓋を開けてみれば何てことはなかった.
おまけ
Mapの条件は配列の型が同じで連続していることが条件のようなので,geometry_msgs::Wrenchのようなforce(x,y,z)とtorque(x,y,z)のような,配列の形になってなくても 一気に格納できるようです.
#include <Eigen/Core> geometry_msgs::WrenchStamped ws; Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd>(&ws.wrench.force.x,6);
配列の要素が連続であることが保証されているか,内部の仕様を見ておらず確かではないので,あまり良いことではないかもしれません.
参照
http://mikaka.org/~kana/dl/pdf/pdf-eigennote.pdf:行列ライブラリEigenのメモ