transform.cpp
Go to the documentation of this file.
00001 #include "pointmatcher_ros/transform.h"
00002 #include "tf/transform_listener.h"
00003 #include "tf/transform_datatypes.h"
00004 #include "tf_conversions/tf_eigen.h"
00005 #include "eigen_conversions/eigen_msg.h"
00006 #include "ros/ros.h"
00007 #include "ros/common.h"
00008 
00009 // ugly test depending on roscpp because tf_conversions is not properly versionized
00010 #if !ROS_VERSION_MINIMUM(1, 9, 30)
00011         #define transformTFToEigen TransformTFToEigen
00012         #define transformEigenToTF TransformEigenToTF
00013 #endif // !ROS_VERSION_MINIMUM(1, 9, 30)
00014 
00015 namespace PointMatcher_ros
00016 {
00017         template<typename T>
00018         typename PointMatcher<T>::TransformationParameters transformListenerToEigenMatrix(const tf::TransformListener &listener, const std::string& target, const std::string& source, const ros::Time& stamp)
00019         {
00020                 typedef typename PointMatcher<T>::TransformationParameters TransformationParameters;
00021                 
00022                 tf::StampedTransform stampedTr;
00023                 listener.waitForTransform(target, source, stamp, ros::Duration(0.1));
00024                 listener.lookupTransform(target, source, stamp, stampedTr);
00025                                                 
00026                 Eigen::Affine3d eigenTr;
00027                 tf::transformTFToEigen(stampedTr, eigenTr);
00028                 return eigenTr.matrix().cast<T>();
00029         }
00030         
00031         template
00032         PointMatcher<float>::TransformationParameters transformListenerToEigenMatrix<float>(const tf::TransformListener &listener, const std::string& target, const std::string& source, const ros::Time& stamp);
00033         template
00034         PointMatcher<double>::TransformationParameters transformListenerToEigenMatrix<double>(const tf::TransformListener &listener, const std::string& target, const std::string& source, const ros::Time& stamp);
00035 
00036         
00037         template<typename T>
00038         typename PointMatcher<T>::TransformationParameters odomMsgToEigenMatrix(const nav_msgs::Odometry& odom)
00039         {
00040                 Eigen::Affine3d eigenTr;
00041                 tf::poseMsgToEigen(odom.pose.pose, eigenTr);
00042                 return eigenTr.matrix().cast<T>();
00043         }
00044         
00045         template
00046         PointMatcher<float>::TransformationParameters odomMsgToEigenMatrix<float>(const nav_msgs::Odometry& odom);
00047         template
00048         PointMatcher<double>::TransformationParameters odomMsgToEigenMatrix<double>(const nav_msgs::Odometry& odom);
00049         
00050         
00051         template<typename T>
00052         nav_msgs::Odometry eigenMatrixToOdomMsg(const typename PointMatcher<T>::TransformationParameters& inTr, const std::string& frame_id, const ros::Time& stamp)
00053         {
00054                 nav_msgs::Odometry odom;
00055                 odom.header.stamp = stamp;
00056                 odom.header.frame_id = frame_id;
00057                 
00058                 // Fill pose
00059                 const Eigen::Affine3d eigenTr(
00060                         Eigen::Matrix4d(
00061                                 eigenMatrixToDim<double>(
00062                                         inTr.template cast<double>(), 4
00063                                 )
00064                         )
00065                 );
00066                 tf::poseEigenToMsg(eigenTr, odom.pose.pose);
00067 
00068                 // Fill velocity, TODO: find proper computation from delta poses to twist
00069                 //odom.child_frame_id = cloudMsgIn.header.frame_id;
00070                 odom.twist.covariance[0+0*6] = -1;
00071                 odom.twist.covariance[1+1*6] = -1;
00072                 odom.twist.covariance[2+2*6] = -1;
00073                 odom.twist.covariance[3+3*6] = -1;
00074                 odom.twist.covariance[4+4*6] = -1;
00075                 odom.twist.covariance[5+5*6] = -1;
00076 
00077                 return odom;
00078         }
00079         
00080         template
00081         nav_msgs::Odometry eigenMatrixToOdomMsg<float>(const PointMatcher<float>::TransformationParameters& inTr, const std::string& frame_id, const ros::Time& stamp);
00082         template
00083         nav_msgs::Odometry eigenMatrixToOdomMsg<double>(const PointMatcher<double>::TransformationParameters& inTr, const std::string& frame_id, const ros::Time& stamp);
00084         
00085         
00086         template<typename T>
00087         tf::Transform eigenMatrixToTransform(const typename PointMatcher<T>::TransformationParameters& inTr)
00088         {
00089                 tf::Transform tfTr;
00090                 const Eigen::Affine3d eigenTr(
00091                         Eigen::Matrix4d(
00092                                 eigenMatrixToDim<double>(
00093                                         inTr.template cast<double>(), 4
00094                                 )
00095                         )
00096                 );
00097                 tf::transformEigenToTF(eigenTr, tfTr);
00098                 return tfTr;
00099         }
00100         
00101         template
00102         tf::Transform eigenMatrixToTransform<float>(const PointMatcher<float>::TransformationParameters& inTr);
00103         template
00104         tf::Transform eigenMatrixToTransform<double>(const PointMatcher<double>::TransformationParameters& inTr);
00105         
00106         
00107         template<typename T>
00108         tf::StampedTransform eigenMatrixToStampedTransform(const typename PointMatcher<T>::TransformationParameters& inTr, const std::string& target, const std::string& source, const ros::Time& stamp)
00109         {
00110                 return tf::StampedTransform(eigenMatrixToTransform<T>(inTr), stamp, target, source);
00111         }
00112         
00113         template
00114         tf::StampedTransform eigenMatrixToStampedTransform<float>(const PointMatcher<float>::TransformationParameters& inTr, const std::string& target, const std::string& source, const ros::Time& stamp);
00115         template
00116         tf::StampedTransform eigenMatrixToStampedTransform<double>(const PointMatcher<double>::TransformationParameters& inTr, const std::string& target, const std::string& source, const ros::Time& stamp);
00117         
00118         
00119         template<typename T>
00120         typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
00121         {
00122                 typedef typename PointMatcher<T>::TransformationParameters M;
00123                 assert(matrix.rows() == matrix.cols());
00124                 assert((matrix.rows() == 3) || (matrix.rows() == 4));
00125                 assert((dimp1 == 3) || (dimp1 == 4));
00126                 
00127                 if (matrix.rows() == dimp1)
00128                         return matrix;
00129                 
00130                 M out(M::Identity(dimp1,dimp1));
00131                 out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
00132                 out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
00133                 return out;
00134         }
00135         
00136         template
00137         PointMatcher<float>::TransformationParameters eigenMatrixToDim<float>(const PointMatcher<float>::TransformationParameters& matrix, int dimp1);
00138         template
00139         PointMatcher<double>::TransformationParameters eigenMatrixToDim<double>(const PointMatcher<double>::TransformationParameters& matrix, int dimp1);
00140 }; // PointMatcher_ros


libpointmatcher_ros
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:13