14 #define ECL_USE_EIGEN3 20 #include <gtest/gtest.h> 23 #include "../../include/ecl/geometry/angle.hpp" 24 #include "../../include/ecl/geometry/legacy_pose2d.hpp" 33 using ecl::linear_algebra::Matrix2d;
34 using ecl::linear_algebra::Vector2d;
35 using ecl::linear_algebra::Vector3d;
41 TEST(LegacyPose2DTests,RotationMatrixConstructors) {
43 EXPECT_EQ(0.0,pose1.x());
44 EXPECT_EQ(0.0,pose1.y());
45 EXPECT_GT(1.01,pose1.rotation()(0,0));
46 EXPECT_LT(0.99,pose1.rotation()(0,0));
47 EXPECT_GT(0.01,pose1.rotation()(0,1));
48 EXPECT_LT(-0.01,pose1.rotation()(0,1));
49 EXPECT_GT(0.01,pose1.rotation()(1,0));
50 EXPECT_LT(-0.01,pose1.rotation()(1,0));
51 EXPECT_GT(1.01,pose1.rotation()(1,1));
52 EXPECT_LT(0.99,pose1.rotation()(1,1));
54 EXPECT_EQ(1.0,pose2.x());
55 EXPECT_EQ(2.0,pose2.y());
56 EXPECT_EQ(3.14,pose2.heading());
57 Vector2d trans; trans << 1.0, 2.0;
60 EXPECT_EQ(1.0,pose3.x());
61 EXPECT_EQ(2.0,pose3.y());
62 EXPECT_GT(3.15, pose3.heading());
63 EXPECT_LT(3.13, pose3.heading());
65 EXPECT_EQ(1.0,pose4.x());
66 EXPECT_EQ(2.0,pose4.y());
67 EXPECT_GT(3.15, pose4.heading());
68 EXPECT_LT(3.13, pose4.heading());
70 EXPECT_EQ(1.0,pose5.x());
71 EXPECT_EQ(2.0,pose5.y());
72 EXPECT_GT(3.15, pose5.heading());
73 EXPECT_LT(3.13, pose5.heading());
76 EXPECT_EQ(1.0,pose6.x());
77 EXPECT_EQ(2.0,pose6.y());
78 EXPECT_GT(3.15, pose6.heading());
79 EXPECT_LT(3.13, pose6.heading());
82 TEST(LegacyPose2DTests,RotationAngleConstructors) {
84 EXPECT_EQ(0.0,pose1.x());
85 EXPECT_EQ(0.0,pose1.y());
86 EXPECT_GT(0.01,pose1.rotation());
87 EXPECT_LT(-0.01,pose1.rotation());
89 EXPECT_EQ(1.0,pose2.x());
90 EXPECT_EQ(2.0,pose2.y());
91 EXPECT_EQ(3.14,pose2.heading());
92 Vector2d trans; trans << 1.0, 2.0;
95 EXPECT_EQ(1.0,pose3.x());
96 EXPECT_EQ(2.0,pose3.y());
97 EXPECT_GT(3.15, pose3.heading());
98 EXPECT_LT(3.13, pose3.heading());
100 EXPECT_EQ(1.0,pose4.x());
101 EXPECT_EQ(2.0,pose4.y());
102 EXPECT_GT(3.15, pose4.heading());
103 EXPECT_LT(3.13, pose4.heading());
105 EXPECT_EQ(1.0,pose5.x());
106 EXPECT_EQ(2.0,pose5.y());
107 EXPECT_GT(3.15, pose5.heading());
108 EXPECT_LT(3.13, pose5.heading());
111 EXPECT_EQ(1.0,pose6.x());
112 EXPECT_EQ(2.0,pose6.y());
113 EXPECT_GT(3.15, pose6.heading());
114 EXPECT_LT(3.13, pose6.heading());
117 TEST(LegacyPose2DTests,assignment) {
123 EXPECT_EQ(2.0,pose_r2.x());
124 EXPECT_EQ(1.0,pose_r2.y());
125 EXPECT_GT(1.15, pose_r2.heading());
126 EXPECT_LT(1.13, pose_r2.heading());
128 EXPECT_EQ(1.0,pose_r2.x());
129 EXPECT_EQ(2.0,pose_r2.y());
130 EXPECT_GT(3.15, pose_r2.heading());
131 EXPECT_LT(3.13, pose_r2.heading());
133 EXPECT_EQ(2.0,pose_a2.x());
134 EXPECT_EQ(1.0,pose_a2.y());
135 EXPECT_GT(1.15, pose_a2.heading());
136 EXPECT_LT(1.13, pose_a2.heading());
138 EXPECT_EQ(1.0,pose_a2.x());
139 EXPECT_EQ(2.0,pose_a2.y());
140 EXPECT_GT(3.15, pose_a2.heading());
141 EXPECT_LT(3.13, pose_a2.heading());
144 TEST(LegacyPose2DTests,eigenStyle) {
145 Vector2d trans; trans << 1.0, 2.0;
149 rpose.translation(trans);
150 EXPECT_EQ(1.0,rpose.x());
151 EXPECT_EQ(2.0,rpose.y());
152 EXPECT_GT(3.15, rpose.heading());
153 EXPECT_LT(3.13, rpose.heading());
154 rot = rpose.rotation();
155 trans = rpose.translation();
156 EXPECT_EQ(1.0,trans[0]);
157 EXPECT_EQ(2.0,trans[1]);
161 apose.rotation(3.14);
162 apose.translation(trans);
163 EXPECT_EQ(1.0,apose.x());
164 EXPECT_EQ(2.0,apose.y());
165 EXPECT_GT(3.15, apose.heading());
166 EXPECT_LT(3.13, apose.heading());
167 double angle = apose.rotation();
168 trans = apose.translation();
169 EXPECT_EQ(1.0,trans[0]);
170 EXPECT_EQ(2.0,trans[1]);
171 EXPECT_GT(3.15, angle);
172 EXPECT_LT(3.13, angle);
174 TEST(LegacyPose2DTests,convenienceStyle) {
175 Vector2d trans; trans << 1.0, 2.0;
178 rpose.x(1.0); rpose.y(2.0); rpose.heading(3.14);
179 EXPECT_EQ(1.0,rpose.x());
180 EXPECT_EQ(2.0,rpose.y());
181 EXPECT_GT(3.15, rpose.heading());
182 EXPECT_LT(3.13, rpose.heading());
184 rot = rpose.rotationMatrix();
185 EXPECT_GT(1.01,rot(0,0));
186 EXPECT_LT(0.99,rot(0,0));
187 EXPECT_GT(0.01,rot(0,1));
188 EXPECT_LT(-0.01,rot(0,1));
189 EXPECT_GT(0.01,rot(1,0));
190 EXPECT_LT(-0.01,rot(1,0));
191 EXPECT_GT(1.01,rot(1,1));
192 EXPECT_LT(0.99,rot(1,1));
194 apose.x(1.0); apose.y(2.0); apose.heading(3.14);
195 EXPECT_EQ(1.0,apose.x());
196 EXPECT_EQ(2.0,apose.y());
197 EXPECT_GT(3.15, apose.heading());
198 EXPECT_LT(3.13, apose.heading());
200 rot = apose.rotationMatrix();
201 EXPECT_GT(1.01,rot(0,0));
202 EXPECT_LT(0.99,rot(0,0));
203 EXPECT_GT(0.01,rot(0,1));
204 EXPECT_LT(-0.01,rot(0,1));
205 EXPECT_GT(0.01,rot(1,0));
206 EXPECT_LT(-0.01,rot(1,0));
207 EXPECT_GT(1.01,rot(1,1));
208 EXPECT_LT(0.99,rot(1,1));
210 TEST(LegacyPose2DTests,inverse) {
213 EXPECT_GT(1.01,inverse.x());
214 EXPECT_LT(0.99,inverse.x());
215 EXPECT_GT(2.01,inverse.y());
216 EXPECT_LT(1.99,inverse.y());
217 EXPECT_GT(-
ecl::pi+0.01,inverse.heading());
218 EXPECT_LT(-
ecl::pi-0.01,inverse.heading());
221 EXPECT_GT(1.01,ainverse.x());
222 EXPECT_LT(0.99,ainverse.x());
223 EXPECT_GT(2.01,ainverse.y());
224 EXPECT_LT(1.99,ainverse.y());
225 EXPECT_GT(-
ecl::pi+0.01,ainverse.heading());
226 EXPECT_LT(-
ecl::pi-0.01,ainverse.heading());
229 TEST(LegacyPose2DTests,operators) {
232 EXPECT_EQ(0.0,diff.x());
233 EXPECT_EQ(1.0,diff.y());
234 EXPECT_EQ(
ecl::pi,diff.heading());
237 EXPECT_EQ(0.0,diff2.x());
238 EXPECT_EQ(1.0,diff2.y());
239 EXPECT_EQ(
ecl::pi,diff2.heading());
241 TEST(LegacyPose2DTests,relative) {
244 EXPECT_GT(1.01,brela.x());
245 EXPECT_LT(0.99,brela.x());
246 EXPECT_GT(0.01,brela.y());
247 EXPECT_LT(-0.01,brela.y());
248 EXPECT_GT(1.58,brela.heading());
249 EXPECT_LT(1.56,brela.heading());
252 EXPECT_GT(1.01,brela_.x());
253 EXPECT_LT(0.99,brela_.x());
254 EXPECT_GT(0.01,brela_.y());
255 EXPECT_LT(-0.01,brela_.y());
256 EXPECT_GT(1.58,brela_.heading());
257 EXPECT_LT(1.56,brela_.heading());
264 int main(
int argc,
char **argv) {
265 testing::InitGoogleTest(&argc,argv);
266 return RUN_ALL_TESTS();
int main(int argc, char **argv)
Parent template definition for Pose2D.
TEST(LegacyPose2DTests, RotationMatrixConstructors)
Parent template definition for angles.