32 #include <gtest/gtest.h> 35 TEST(TfEigen, ConvertVector3dStamped)
40 geometry_msgs::PointStamped p1;
47 TEST(TfEigen, ConvertVector3d)
49 const Eigen::Vector3d v(1,2,3);
52 geometry_msgs::Point p1;
59 TEST(TfEigen, ConvertQuaterniondStamped)
64 geometry_msgs::QuaternionStamped p1;
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());
76 TEST(TfEigen, ConvertQuaterniond)
78 const Eigen::Quaterniond v(1,2,3,4);
80 Eigen::Quaterniond v1;
81 geometry_msgs::Quaternion p1;
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());
91 TEST(TfEigen, TransformQuaterion) {
93 const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()));
94 const Eigen::Affine3d affine(iso);
99 trafo.header.frame_id =
"expected";
104 EXPECT_TRUE(out.isApprox(expected));
111 trafo.header.frame_id =
"expected";
114 EXPECT_TRUE(out.isApprox(expected));
119 TEST(TfEigen, ConvertAffine3dStamped)
121 const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
125 geometry_msgs::PoseStamped p1;
129 EXPECT_EQ(v.translation(), v1.translation());
130 EXPECT_EQ(v.rotation(), v1.rotation());
135 TEST(TfEigen, ConvertIsometry3dStamped)
137 const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
141 geometry_msgs::PoseStamped p1;
145 EXPECT_EQ(v.translation(), v1.translation());
146 EXPECT_EQ(v.rotation(), v1.rotation());
153 const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
156 geometry_msgs::Pose p1;
160 EXPECT_EQ(v.translation(), v1.translation());
161 EXPECT_EQ(v.rotation(), v1.rotation());
164 TEST(TfEigen, ConvertIsometry3d)
166 const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
168 Eigen::Isometry3d v1;
169 geometry_msgs::Pose p1;
173 EXPECT_EQ(v.translation(), v1.translation());
174 EXPECT_EQ(v.rotation(), v1.rotation());
177 TEST(TfEigen, ConvertTransform)
181 double alpha = M_PI/4.0;
182 double theta = M_PI/6.0;
183 double gamma = M_PI/12.0;
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,
190 Eigen::Affine3d T(tm);
195 EXPECT_TRUE(T.isApprox(Tback));
196 EXPECT_TRUE(tm.isApprox(Tback.matrix()));
199 Eigen::Isometry3d I(tm);
204 EXPECT_TRUE(I.isApprox(Iback));
205 EXPECT_TRUE(tm.isApprox(Iback.matrix()));
210 int main(
int argc,
char **argv){
211 testing::InitGoogleTest(&argc, argv);
213 return RUN_ALL_TESTS();
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
Convert an Eigen Affine3d transform to the equivalent geometry_msgs message type. ...
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
TEST(TfEigen, ConvertVector3dStamped)
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.