Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef TF_HELPER_H
00039 #define TF_HELPER_H
00040
00041 #include <Eigen/Geometry>
00042 #include <tf/transform_datatypes.h>
00043 #include <imu_filter/observation.h>
00044
00045 namespace imu_filter {
00046
00047
00048 namespace tf_helper
00049 {
00056 static inline void toTfTransform( const Eigen::Quaterniond & q, const Eigen::Vector3d & trans, tf::Transform & transform )
00057 {
00058 transform.setRotation( tf::Quaternion( q.x(), q.y(), q.z(), q.w() ) );
00059 transform.setOrigin( tf::Vector3( trans.x(), trans.y(), trans.z() ) );
00060 }
00061
00067 static inline void toTfTransform( const Eigen::Affine3d & pose, tf::Transform & transform )
00068 {
00069 Eigen::Quaterniond q;
00070 q = pose.rotation();
00071 transform.setRotation( tf::Quaternion( q.x(), q.y(), q.z(), q.w() ) );
00072 transform.setOrigin( tf::Vector3( pose.translation().x(), pose.translation().y(), pose.translation().z() ) );
00073 }
00074
00080 static inline void toTfTransform( const Observation::Vector & vec, tf::Transform & transform )
00081 {
00082 transform.setRotation( tf::Quaternion( vec[ 0 ], vec[ 1 ], vec[ 2 ], vec[ 3 ] ) );
00083 transform.setOrigin( tf::Vector3( vec[ 4 ], vec[ 5 ], vec[ 6 ] ) );
00084 }
00085
00092 static inline void toEigen( const tf::Transform & transform, Eigen::Quaterniond & q, Eigen::Vector3d & trans )
00093 {
00094 const tf::Quaternion & rot = transform.getRotation();
00095 const tf::Vector3 & t = transform.getOrigin();
00096
00097 q.x() = rot.x();
00098 q.y() = rot.y();
00099 q.z() = rot.z();
00100 q.w() = rot.w();
00101
00102 trans.x() = t.x();
00103 trans.y() = t.y();
00104 trans.z() = t.z();
00105 }
00106
00107 }
00108
00109 }
00110
00111 #endif