test_pose.cpp
Go to the documentation of this file.
00001 /*
00002  * test_pose.cpp
00003  *
00004  *  Created on: Mar 15, 2012
00005  *      Author: Pablo IƱigo Blasco
00006  */
00007 
00008 #include <mrpt/poses/CPosePDFGaussian.h>
00009 #include <mrpt/poses/CPose3DPDFGaussian.h>
00010 #include <mrpt/math/CQuaternion.h>
00011 #include <geometry_msgs/PoseWithCovariance.h>
00012 #include <geometry_msgs/Pose.h>
00013 #include <geometry_msgs/Quaternion.h>
00014 #include <tf/tf.h>
00015 #include <mrpt_bridge/pose.h>
00016 #include <gtest/gtest.h>
00017 
00018 using namespace std;
00019 using namespace mrpt::utils; // DEG2RAD()
00020 
00021 void checkPoseMatrixFromRotationParameters(const double roll, const double pitch, const double yaw)
00022 {
00023   //TF-BULLET ROTATION
00024   tf::Pose original_pose;
00025   tf::Quaternion rotation;
00026   rotation.setRPY(roll, pitch, yaw);
00027   original_pose.setRotation(rotation);
00028   tf::Matrix3x3 basis = original_pose.getBasis();
00029 
00030   //MRPT-ROTATION
00031   mrpt::poses::CPose3D mrpt_original_pose;
00032   mrpt_original_pose.setYawPitchRoll(yaw, pitch, roll);
00033   mrpt::math::CMatrixDouble33 mrpt_basis = mrpt_original_pose.getRotationMatrix();
00034 
00035   for (int i = 0; i < 3; i++)
00036     for (int j = 0; j < 3; j++)
00037       EXPECT_NEAR(basis[i][j], mrpt_basis(i,j), 0.01);
00038 }
00039 
00040 TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
00041 {
00042     tf::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
00043     mrpt::math::CMatrixDouble33 des;
00044     mrpt_bridge::convert(src, des);
00045     for(int r = 0; r < 3; r++)
00046         for(int c = 0; c < 3; c++)
00047             EXPECT_FLOAT_EQ(des(r,c), src[r][c]);
00048 }
00049 TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
00050 {
00051     mrpt::math::CMatrixDouble33 src;
00052     src << 0, 1, 2, 3, 4, 5, 6, 7, 8;
00053     tf::Matrix3x3 des;
00054     mrpt_bridge::convert(src, des);
00055     for(int r = 0; r < 3; r++)
00056         for(int c = 0; c < 3; c++)
00057             EXPECT_FLOAT_EQ(des[r][c],src(r,c));
00058 }
00059 
00060 
00061 TEST(PoseConversions, checkPoseMatrixFromRotationParameters)
00062 {
00063         checkPoseMatrixFromRotationParameters(0  , 0,   0);
00064         checkPoseMatrixFromRotationParameters(0.2, 0,   0);
00065         checkPoseMatrixFromRotationParameters(0,   0.2, 0);
00066         checkPoseMatrixFromRotationParameters(0,   0,   0.2);
00067         checkPoseMatrixFromRotationParameters(0.4, 0.3, 0.2);
00068         checkPoseMatrixFromRotationParameters(M_PI,-M_PI/2.0, 0);
00069 }
00070 
00071 // Declare a test
00072 TEST(PoseConversions, reference_frame_change_with_rotations)
00073 {
00074 
00075   geometry_msgs::PoseWithCovariance ros_msg_original_pose;
00076 
00077   ros_msg_original_pose.pose.position.x = 1;
00078   ros_msg_original_pose.pose.position.y = 0;
00079   ros_msg_original_pose.pose.position.z = 0;
00080 
00081   //to mrpt
00082   mrpt::poses::CPose3DPDFGaussian mrpt_original_pose;
00083   mrpt_bridge::convert(ros_msg_original_pose, mrpt_original_pose);
00084   EXPECT_EQ(ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]);
00085 
00086   //to tf
00087   tf::Pose tf_original_pose;
00088   tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);
00089 
00090   //rotate yaw pi in MRPT
00091   mrpt::poses::CPose3D rotation_mrpt;
00092   double yaw = M_PI / 2.0;
00093   rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0);
00094   mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean;
00095   EXPECT_NEAR(mrpt_result[1], 1.0, 0.01);
00096 
00097   //rotate yaw pi in TF
00098   tf::Quaternion rotation_tf;
00099   rotation_tf.setRPY(0, 0, yaw);
00100   tf::Pose rotation_pose_tf;
00101   rotation_pose_tf.setRotation(rotation_tf);
00102   tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
00103   EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01);
00104 
00105   geometry_msgs::Pose mrpt_ros_result;
00106   mrpt_bridge::convert(mrpt_result, mrpt_ros_result);
00107 
00108   EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
00109   EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
00110   EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
00111 }
00112 
00113 void check_CPose3D_tofrom_ROS(double x,double y, double z, double yaw, double pitch, double roll)
00114 {
00115         const mrpt::poses::CPose3D p3D(x,y,z,yaw,pitch,roll);
00116 
00117         // Convert MRPT->ROS
00118         geometry_msgs::Pose ros_p3D;
00119     mrpt_bridge::convert(p3D, ros_p3D);
00120 
00121         // Compare ROS quat vs. MRPT quat:
00122         mrpt::math::CQuaternionDouble q;
00123         p3D.getAsQuaternion(q);
00124 
00125         EXPECT_NEAR( ros_p3D.position.x, p3D.x() , 1e-4 ) << "p: " << p3D << endl;
00126         EXPECT_NEAR( ros_p3D.position.y, p3D.y() , 1e-4 ) << "p: " << p3D << endl;
00127         EXPECT_NEAR( ros_p3D.position.z, p3D.z() , 1e-4 ) << "p: " << p3D << endl;
00128 
00129         EXPECT_NEAR( ros_p3D.orientation.x, q.x() , 1e-4 ) << "p: " << p3D << endl;
00130         EXPECT_NEAR( ros_p3D.orientation.y, q.y() , 1e-4 ) << "p: " << p3D << endl;
00131         EXPECT_NEAR( ros_p3D.orientation.z, q.z() , 1e-4 ) << "p: " << p3D << endl;
00132         EXPECT_NEAR( ros_p3D.orientation.w, q.r() , 1e-4 ) << "p: " << p3D << endl;
00133 
00134         // Test the other path: ROS->MRPT
00135         mrpt::poses::CPose3D p_bis;
00136     mrpt_bridge::convert(ros_p3D, p_bis);
00137 
00138         // p_bis==p3D?
00139         EXPECT_NEAR( (p_bis.getAsVectorVal() - p3D.getAsVectorVal()).array().abs().maxCoeff(),0, 1e-4 )
00140                 << "p_bis: " << p_bis<< endl
00141                 << "p3D: " << p3D << endl;
00142 }
00143 
00144 // Declare a test
00145 TEST(PoseConversions, check_CPose3D_tofrom_ROS)
00146 {
00147         check_CPose3D_tofrom_ROS(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
00148         check_CPose3D_tofrom_ROS(1,2,3, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) );
00149 
00150         check_CPose3D_tofrom_ROS(1,2,3, DEG2RAD(30),DEG2RAD(0),DEG2RAD(0) );
00151         check_CPose3D_tofrom_ROS(1,2,3, DEG2RAD(0),DEG2RAD(30),DEG2RAD(0) );
00152         check_CPose3D_tofrom_ROS(1,2,3, DEG2RAD(0),DEG2RAD(0),DEG2RAD(30) );
00153 
00154         check_CPose3D_tofrom_ROS(1,2,3, DEG2RAD(-5),DEG2RAD(15),DEG2RAD(-30) );
00155 }
00156 
00157 // Declare a test
00158 TEST(PoseConversions, check_CPose2D_to_ROS)
00159 {
00160         const mrpt::poses::CPose2D p2D(1,2, 0.56);
00161 
00162         // Convert MRPT->ROS
00163         geometry_msgs::Pose ros_p2D;
00164     mrpt_bridge::convert(p2D, ros_p2D);
00165 
00166         // Compare vs. 3D pose:
00167         const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D);
00168         mrpt::poses::CPose3D p3D_ros;
00169     mrpt_bridge::convert(ros_p2D, p3D_ros);
00170 
00171         // p3D_ros should equal p3D
00172         EXPECT_NEAR( (p3D_ros.getAsVectorVal() - p3D.getAsVectorVal()).array().abs().maxCoeff(),0, 1e-4 )
00173                 << "p3D_ros: " << p3D_ros << endl
00174                 << "p3D: " << p3D << endl;
00175 }


mrpt_bridge
Author(s):
autogenerated on Mon Aug 11 2014 11:23:21