test_eigen_tf.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <time.h>
32 #include <gtest/gtest.h>
33 
34 using namespace tf;
35 
36 double gen_rand(double min, double max)
37 {
38  int rand_num = rand()%100+1;
39  double result = min + (double)((max-min)*rand_num)/101.0;
40  return result;
41 }
42 
43 TEST(TFEigenConversions, tf_eigen_vector)
44 {
45  tf::Vector3 t;
46  t[0] = gen_rand(-10,10);
47  t[1] = gen_rand(-10,10);
48  t[2] = gen_rand(-10,10);
49 
50  Eigen::Vector3d k;
51  vectorTFToEigen(t,k);
52 
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);
56 }
57 
58 TEST(TFEigenConversions, tf_eigen_quaternion)
59 {
61  t[0] = gen_rand(-1.0,1.0);
62  t[1] = gen_rand(-1.0,1.0);
63  t[2] = gen_rand(-1.0,1.0);
64  t[3] = gen_rand(-1.0,1.0);
65  t.normalize();
66  Eigen::Quaterniond k;
68 
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);
74 }
75 
76 TEST(TFEigenConversions, tf_eigen_transform)
77 {
78  tf::Transform t;
79  tf::Quaternion tq;
80  tq[0] = gen_rand(-1.0,1.0);
81  tq[1] = gen_rand(-1.0,1.0);
82  tq[2] = gen_rand(-1.0,1.0);
83  tq[3] = gen_rand(-1.0,1.0);
84  tq.normalize();
85  t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
86  t.setRotation(tq);
87 
88  Eigen::Affine3d affine;
89  Eigen::Isometry3d isometry;
90  transformTFToEigen(t, affine);
91  transformTFToEigen(t, isometry);
92 
93  for(int i=0; i < 3; i++)
94  {
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++)
98  {
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);
101  }
102  }
103  for (int col = 0 ; col < 3; col ++)
104  {
105  ASSERT_NEAR(affine.matrix()(3, col), 0, 1e-6);
106  ASSERT_NEAR(isometry.matrix()(3, col), 0, 1e-6);
107  }
108  ASSERT_NEAR(affine.matrix()(3,3), 1, 1e-6);
109  ASSERT_NEAR(isometry.matrix()(3,3), 1, 1e-6);
110 }
111 
112 TEST(TFEigenConversions, eigen_tf_transform)
113 {
114  tf::Transform t1;
115  tf::Transform t2;
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);
123  kq.normalize();
124  isometry.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
125  isometry.rotate(kq);
126  affine = isometry;
127 
128  transformEigenToTF(affine,t1);
129  transformEigenToTF(isometry,t2);
130  for(int i=0; i < 3; i++)
131  {
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++)
135  {
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);
138  }
139  }
140 }
141 
142 
143 int main(int argc, char **argv){
144 /* initialize random seed: */
145  srand ( time(NULL) );
146  testing::InitGoogleTest(&argc, argv);
147  return RUN_ALL_TESTS();
148 }
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
Converts an Eigen Affine3d into a tf Transform.
Definition: tf_eigen.cpp:121
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
Converts a tf Transform into an Eigen Affine3d.
Definition: tf_eigen.cpp:111
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Quaternion & normalize()
TEST(TFEigenConversions, tf_eigen_vector)
double gen_rand(double min, double max)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e)
Converts a tf Quaternion into an Eigen Quaternion.
Definition: tf_eigen.cpp:70
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e)
Converts a tf Vector3 into an Eigen Vector3d.
Definition: tf_eigen.cpp:131
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
int main(int argc, char **argv)


tf_conversions
Author(s): Tully Foote
autogenerated on Fri Jun 7 2019 22:00:41