32 #include <gtest/gtest.h> 38 int rand_num = rand()%100+1;
39 double result = min + (double)((max-min)*rand_num)/101.0;
43 TEST(TFEigenConversions, tf_eigen_vector)
53 ASSERT_NEAR(t[0],k[0],1e-6);
54 ASSERT_NEAR(t[1],k[1],1e-6);
55 ASSERT_NEAR(t[2],k[2],1e-6);
58 TEST(TFEigenConversions, tf_eigen_quaternion)
69 ASSERT_NEAR(t[0],k.coeffs()(0),1e-6);
70 ASSERT_NEAR(t[1],k.coeffs()(1),1e-6);
71 ASSERT_NEAR(t[2],k.coeffs()(2),1e-6);
72 ASSERT_NEAR(t[3],k.coeffs()(3),1e-6);
73 ASSERT_NEAR(k.norm(),1.0,1e-10);
76 TEST(TFEigenConversions, tf_eigen_transform)
88 Eigen::Affine3d affine;
89 Eigen::Isometry3d isometry;
93 for(
int i=0; i < 3; i++)
95 ASSERT_NEAR(t.
getOrigin()[i],affine.matrix()(i,3),1e-6);
96 ASSERT_NEAR(t.
getOrigin()[i],isometry.matrix()(i,3),1e-6);
97 for(
int j=0; j < 3; j++)
99 ASSERT_NEAR(t.
getBasis()[i][j],affine.matrix()(i,j),1e-6);
100 ASSERT_NEAR(t.
getBasis()[i][j],isometry.matrix()(i,j),1e-6);
103 for (
int col = 0 ; col < 3; col ++)
105 ASSERT_NEAR(affine.matrix()(3, col), 0, 1e-6);
106 ASSERT_NEAR(isometry.matrix()(3, col), 0, 1e-6);
108 ASSERT_NEAR(affine.matrix()(3,3), 1, 1e-6);
109 ASSERT_NEAR(isometry.matrix()(3,3), 1, 1e-6);
112 TEST(TFEigenConversions, eigen_tf_transform)
116 Eigen::Affine3d affine;
117 Eigen::Isometry3d isometry;
118 Eigen::Quaterniond kq;
119 kq.coeffs()(0) =
gen_rand(-1.0,1.0);
120 kq.coeffs()(1) =
gen_rand(-1.0,1.0);
121 kq.coeffs()(2) =
gen_rand(-1.0,1.0);
122 kq.coeffs()(3) =
gen_rand(-1.0,1.0);
130 for(
int i=0; i < 3; i++)
132 ASSERT_NEAR(t1.
getOrigin()[i],affine.matrix()(i,3),1e-6);
133 ASSERT_NEAR(t2.
getOrigin()[i],isometry.matrix()(i,3),1e-6);
134 for(
int j=0; j < 3; j++)
136 ASSERT_NEAR(t1.
getBasis()[i][j],affine.matrix()(i,j),1e-6);
137 ASSERT_NEAR(t2.
getBasis()[i][j],isometry.matrix()(i,j),1e-6);
143 int main(
int argc,
char **argv){
145 srand ( time(NULL) );
146 testing::InitGoogleTest(&argc, argv);
147 return RUN_ALL_TESTS();
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
Converts an Eigen Affine3d into a tf Transform.
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
Converts a tf Transform into an Eigen Affine3d.
TEST(TFEigenConversions, tf_eigen_vector)
double gen_rand(double min, double max)
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
Converts a tf Quaternion into an Eigen Quaternion.
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e)
Converts a tf Vector3 into an Eigen Vector3d.
int main(int argc, char **argv)