tf2_eigen-test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) Koji Terada
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  *
14  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
18  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24  * POSSIBILITY OF SUCH DAMAGE.
25  */
26 
30 #include <tf2_eigen/tf2_eigen.h>
31 #include <ros/ros.h>
32 #include <gtest/gtest.h>
33 #include <tf2/convert.h>
34 
35 TEST(TfEigen, ConvertVector3dStamped)
36 {
37  const tf2::Stamped<Eigen::Vector3d> v(Eigen::Vector3d(1,2,3), ros::Time(5), "test");
38 
40  geometry_msgs::PointStamped p1;
41  tf2::convert(v, p1);
42  tf2::convert(p1, v1);
43 
44  EXPECT_EQ(v, v1);
45 }
46 
47 TEST(TfEigen, ConvertVector3d)
48 {
49  const Eigen::Vector3d v(1,2,3);
50 
51  Eigen::Vector3d v1;
52  geometry_msgs::Point p1;
53  tf2::convert(v, p1);
54  tf2::convert(p1, v1);
55 
56  EXPECT_EQ(v, v1);
57 }
58 
59 TEST(TfEigen, ConvertQuaterniondStamped)
60 {
61  const tf2::Stamped<Eigen::Quaterniond> v(Eigen::Quaterniond(1,2,3,4), ros::Time(5), "test");
62 
64  geometry_msgs::QuaternionStamped p1;
65  tf2::convert(v, p1);
66  tf2::convert(p1, v1);
67 
68  EXPECT_EQ(v.frame_id_, v1.frame_id_);
69  EXPECT_EQ(v.stamp_, v1.stamp_);
70  EXPECT_EQ(v.w(), v1.w());
71  EXPECT_EQ(v.x(), v1.x());
72  EXPECT_EQ(v.y(), v1.y());
73  EXPECT_EQ(v.z(), v1.z());
74 }
75 
76 TEST(TfEigen, ConvertQuaterniond)
77 {
78  const Eigen::Quaterniond v(1,2,3,4);
79 
80  Eigen::Quaterniond v1;
81  geometry_msgs::Quaternion p1;
82  tf2::convert(v, p1);
83  tf2::convert(p1, v1);
84 
85  EXPECT_EQ(v.w(), v1.w());
86  EXPECT_EQ(v.x(), v1.x());
87  EXPECT_EQ(v.y(), v1.y());
88  EXPECT_EQ(v.z(), v1.z());
89 }
90 
91 TEST(TfEigen, TransformQuaterion) {
92  const tf2::Stamped<Eigen::Quaterniond> in(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitX())), ros::Time(5), "test");
93  const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()));
94  const Eigen::Affine3d affine(iso);
95  const tf2::Stamped<Eigen::Quaterniond> expected(Eigen::Quaterniond(Eigen::AngleAxisd(1, Eigen::Vector3d::UnitZ())), ros::Time(10), "expected");
96 
97  geometry_msgs::TransformStamped trafo = tf2::eigenToTransform(affine);
98  trafo.header.stamp = ros::Time(10);
99  trafo.header.frame_id = "expected";
100 
102  tf2::doTransform(in, out, trafo);
103 
104  EXPECT_TRUE(out.isApprox(expected));
105  EXPECT_EQ(expected.stamp_, out.stamp_);
106  EXPECT_EQ(expected.frame_id_, out.frame_id_);
107 
108  // the same using Isometry
109  trafo = tf2::eigenToTransform(iso);
110  trafo.header.stamp = ros::Time(10);
111  trafo.header.frame_id = "expected";
112  tf2::doTransform(in, out, trafo);
113 
114  EXPECT_TRUE(out.isApprox(expected));
115  EXPECT_EQ(expected.stamp_, out.stamp_);
116  EXPECT_EQ(expected.frame_id_, out.frame_id_);
117 }
118 
119 TEST(TfEigen, ConvertAffine3dStamped)
120 {
121  const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
122  const tf2::Stamped<Eigen::Affine3d> v(v_nonstamped, ros::Time(42), "test_frame");
123 
125  geometry_msgs::PoseStamped p1;
126  tf2::convert(v, p1);
127  tf2::convert(p1, v1);
128 
129  EXPECT_EQ(v.translation(), v1.translation());
130  EXPECT_EQ(v.rotation(), v1.rotation());
131  EXPECT_EQ(v.frame_id_, v1.frame_id_);
132  EXPECT_EQ(v.stamp_, v1.stamp_);
133 }
134 
135 TEST(TfEigen, ConvertIsometry3dStamped)
136 {
137  const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
138  const tf2::Stamped<Eigen::Isometry3d> v(v_nonstamped, ros::Time(42), "test_frame");
139 
141  geometry_msgs::PoseStamped p1;
142  tf2::convert(v, p1);
143  tf2::convert(p1, v1);
144 
145  EXPECT_EQ(v.translation(), v1.translation());
146  EXPECT_EQ(v.rotation(), v1.rotation());
147  EXPECT_EQ(v.frame_id_, v1.frame_id_);
148  EXPECT_EQ(v.stamp_, v1.stamp_);
149 }
150 
151 TEST(TfEigen, ConvertAffine3d)
152 {
153  const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
154 
155  Eigen::Affine3d v1;
156  geometry_msgs::Pose p1;
157  tf2::convert(v, p1);
158  tf2::convert(p1, v1);
159 
160  EXPECT_EQ(v.translation(), v1.translation());
161  EXPECT_EQ(v.rotation(), v1.rotation());
162 }
163 
164 TEST(TfEigen, ConvertIsometry3d)
165 {
166  const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
167 
168  Eigen::Isometry3d v1;
169  geometry_msgs::Pose p1;
170  tf2::convert(v, p1);
171  tf2::convert(p1, v1);
172 
173  EXPECT_EQ(v.translation(), v1.translation());
174  EXPECT_EQ(v.rotation(), v1.rotation());
175 }
176 
177 TEST(TfEigen, ConvertTransform)
178 {
179  Eigen::Matrix4d tm;
180 
181  double alpha = M_PI/4.0;
182  double theta = M_PI/6.0;
183  double gamma = M_PI/12.0;
184 
185  tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1, //
186  cos(alpha)*sin(gamma)+sin(alpha)*sin(theta)*cos(gamma),cos(alpha)*cos(gamma)-sin(alpha)*sin(theta)*sin(gamma),-sin(alpha)*cos(theta), 2, //
187  sin(alpha)*sin(gamma)-cos(alpha)*sin(theta)*cos(gamma),cos(alpha)*sin(theta)*sin(gamma)+sin(alpha)*cos(gamma),cos(alpha)*cos(theta), 3, //
188  0, 0, 0, 1;
189 
190  Eigen::Affine3d T(tm);
191 
192  geometry_msgs::TransformStamped msg = tf2::eigenToTransform(T);
193  Eigen::Affine3d Tback = tf2::transformToEigen(msg);
194 
195  EXPECT_TRUE(T.isApprox(Tback));
196  EXPECT_TRUE(tm.isApprox(Tback.matrix()));
197 
198  // same for Isometry
199  Eigen::Isometry3d I(tm);
200 
201  msg = tf2::eigenToTransform(T);
202  Eigen::Isometry3d Iback = tf2::transformToEigen(msg);
203 
204  EXPECT_TRUE(I.isApprox(Iback));
205  EXPECT_TRUE(tm.isApprox(Iback.matrix()));
206 }
207 
208 
209 
210 int main(int argc, char **argv){
211  testing::InitGoogleTest(&argc, argv);
212 
213  return RUN_ALL_TESTS();
214 }
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. ...
Definition: tf2_eigen.h:67
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
TEST(TfEigen, ConvertVector3dStamped)
ros::Time stamp_
std::string frame_id_
void convert(const A &a, B &b)
int main(int argc, char **argv)
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
Convert a timestamped transform to the equivalent Eigen data type.
Definition: tf2_eigen.h:48


tf2_eigen
Author(s): Koji Terada
autogenerated on Fri Oct 16 2020 19:08:52