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 #include <time.h>
00031 #include <tf_conversions/tf_eigen.h>
00032 #include <gtest/gtest.h>
00033
00034 using namespace tf;
00035
00036 double gen_rand(double min, double max)
00037 {
00038 int rand_num = rand()%100+1;
00039 double result = min + (double)((max-min)*rand_num)/101.0;
00040 return result;
00041 }
00042
00043 TEST(TFEigenConversions, tf_eigen_vector)
00044 {
00045 tf::Vector3 t;
00046 t[0] = gen_rand(-10,10);
00047 t[1] = gen_rand(-10,10);
00048 t[2] = gen_rand(-10,10);
00049
00050 Eigen::Vector3d k;
00051 vectorTFToEigen(t,k);
00052
00053 ASSERT_NEAR(t[0],k[0],1e-6);
00054 ASSERT_NEAR(t[1],k[1],1e-6);
00055 ASSERT_NEAR(t[2],k[2],1e-6);
00056 }
00057
00058 TEST(TFEigenConversions, tf_eigen_quaternion)
00059 {
00060 tf::Quaternion t;
00061 t[0] = gen_rand(-1.0,1.0);
00062 t[1] = gen_rand(-1.0,1.0);
00063 t[2] = gen_rand(-1.0,1.0);
00064 t[3] = gen_rand(-1.0,1.0);
00065 t.normalize();
00066 Eigen::Quaterniond k;
00067 quaternionTFToEigen(t,k);
00068
00069 ASSERT_NEAR(t[0],k.coeffs()(0),1e-6);
00070 ASSERT_NEAR(t[1],k.coeffs()(1),1e-6);
00071 ASSERT_NEAR(t[2],k.coeffs()(2),1e-6);
00072 ASSERT_NEAR(t[3],k.coeffs()(3),1e-6);
00073 ASSERT_NEAR(k.norm(),1.0,1e-10);
00074 }
00075
00076 TEST(TFEigenConversions, tf_eigen_transform)
00077 {
00078 tf::Transform t;
00079 tf::Quaternion tq;
00080 tq[0] = gen_rand(-1.0,1.0);
00081 tq[1] = gen_rand(-1.0,1.0);
00082 tq[2] = gen_rand(-1.0,1.0);
00083 tq[3] = gen_rand(-1.0,1.0);
00084 tq.normalize();
00085 t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
00086 t.setRotation(tq);
00087
00088 Eigen::Affine3d k;
00089 transformTFToEigen(t,k);
00090
00091 for(int i=0; i < 3; i++)
00092 {
00093 ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
00094 for(int j=0; j < 3; j++)
00095 {
00096 ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
00097 }
00098 }
00099 for (int col = 0 ; col < 3; col ++)
00100 ASSERT_NEAR(k.matrix()(3, col), 0, 1e-6);
00101 ASSERT_NEAR(k.matrix()(3,3), 1, 1e-6);
00102
00103 }
00104
00105 TEST(TFEigenConversions, eigen_tf_transform)
00106 {
00107 tf::Transform t;
00108 Eigen::Affine3d k;
00109 Eigen::Quaterniond kq;
00110 kq.coeffs()(0) = gen_rand(-1.0,1.0);
00111 kq.coeffs()(1) = gen_rand(-1.0,1.0);
00112 kq.coeffs()(2) = gen_rand(-1.0,1.0);
00113 kq.coeffs()(3) = gen_rand(-1.0,1.0);
00114 kq.normalize();
00115 k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
00116 k.rotate(kq);
00117
00118 transformEigenToTF(k,t);
00119 for(int i=0; i < 3; i++)
00120 {
00121 ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6);
00122 for(int j=0; j < 3; j++)
00123 {
00124 ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6);
00125 }
00126 }
00127 }
00128
00129
00130 int main(int argc, char **argv){
00131
00132 srand ( time(NULL) );
00133 testing::InitGoogleTest(&argc, argv);
00134 return RUN_ALL_TESTS();
00135 }