00001 #ifndef _TRANSFORM_GRAPH_ORIENTATION_H_ 00002 #define _TRANSFORM_GRAPH_ORIENTATION_H_ 00003 00004 #include "Eigen/Dense" 00005 #include "Eigen/Geometry" 00006 #include "geometry_msgs/Quaternion.h" 00007 #include "tf/transform_datatypes.h" 00008 00009 namespace transform_graph { 00014 class Orientation { 00015 public: 00017 Orientation(); 00019 Orientation(double w, double x, double y, double z); 00021 Orientation(const Eigen::Matrix3d& m); 00023 Orientation(const Eigen::Quaterniond& q); 00025 Orientation(const geometry_msgs::Quaternion& q); 00027 Orientation(const tf::Quaternion& q); 00029 Orientation(const tf::Matrix3x3& m); 00030 00033 Eigen::Matrix3d matrix() const; 00034 00037 Eigen::Quaterniond quaternion() const; 00038 00041 geometry_msgs::Quaternion quaternion_msg() const; 00042 00045 tf::Quaternion tf_quaternion() const; 00046 00049 tf::Matrix3x3 tf_matrix() const; 00050 00051 private: 00052 Eigen::Matrix3d matrix_; 00053 }; 00054 } // namespace transform_graph 00055 00056 #endif // _TRANSFORM_GRAPH_ORIENTATION_H_