31 #include <gtest/gtest.h> 34 TEST(TfEigen, ConvertVector3dStamped)
39 geometry_msgs::PointStamped p1;
46 TEST(TfEigen, ConvertVector3d)
48 const Eigen::Vector3d v(1,2,3);
51 geometry_msgs::Point p1;
58 TEST(TfEigen, ConvertQuaterniondStamped)
63 geometry_msgs::QuaternionStamped p1;
69 EXPECT_EQ(v.w(), v1.w());
70 EXPECT_EQ(v.x(), v1.x());
71 EXPECT_EQ(v.y(), v1.y());
72 EXPECT_EQ(v.z(), v1.z());
75 TEST(TfEigen, ConvertQuaterniond)
77 const Eigen::Quaterniond v(1,2,3,4);
79 Eigen::Quaterniond v1;
80 geometry_msgs::Quaternion p1;
84 EXPECT_EQ(v.w(), v1.w());
85 EXPECT_EQ(v.x(), v1.x());
86 EXPECT_EQ(v.y(), v1.y());
87 EXPECT_EQ(v.z(), v1.z());
90 TEST(TfEigen, TransformQuaterion) {
92 const Eigen::Isometry3d iso(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()));
93 const Eigen::Affine3d affine(iso);
98 trafo.header.frame_id =
"expected";
103 EXPECT_TRUE(out.isApprox(expected));
110 trafo.header.frame_id =
"expected";
113 EXPECT_TRUE(out.isApprox(expected));
118 TEST(TfEigen, ConvertAffine3dStamped)
120 const Eigen::Affine3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
124 geometry_msgs::PoseStamped p1;
128 EXPECT_EQ(v.translation(), v1.translation());
129 EXPECT_EQ(v.rotation(), v1.rotation());
134 TEST(TfEigen, ConvertIsometry3dStamped)
136 const Eigen::Isometry3d v_nonstamped(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
140 geometry_msgs::PoseStamped p1;
144 EXPECT_EQ(v.translation(), v1.translation());
145 EXPECT_EQ(v.rotation(), v1.rotation());
152 const Eigen::Affine3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
155 geometry_msgs::Pose p1;
159 EXPECT_EQ(v.translation(), v1.translation());
160 EXPECT_EQ(v.rotation(), v1.rotation());
163 TEST(TfEigen, ConvertIsometry3d)
165 const Eigen::Isometry3d v(Eigen::Translation3d(1,2,3) * Eigen::AngleAxis<double>(1, Eigen::Vector3d::UnitX()));
167 Eigen::Isometry3d v1;
168 geometry_msgs::Pose p1;
172 EXPECT_EQ(v.translation(), v1.translation());
173 EXPECT_EQ(v.rotation(), v1.rotation());
176 TEST(TfEigen, ConvertTransform)
180 double alpha = M_PI/4.0;
181 double theta = M_PI/6.0;
182 double gamma = M_PI/12.0;
184 tm << cos(theta)*cos(gamma),-cos(theta)*sin(gamma),sin(theta), 1,
185 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,
186 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,
189 Eigen::Affine3d T(tm);
194 EXPECT_TRUE(T.isApprox(Tback));
195 EXPECT_TRUE(tm.isApprox(Tback.matrix()));
198 Eigen::Isometry3d I(tm);
203 EXPECT_TRUE(I.isApprox(Iback));
204 EXPECT_TRUE(tm.isApprox(Iback.matrix()));
209 int main(
int argc,
char **argv){
210 testing::InitGoogleTest(&argc, argv);
212 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.