legacy_pose2d.cpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Defines
12 *****************************************************************************/
13 
14 #define ECL_USE_EIGEN3
15 
16 /*****************************************************************************
17 ** Includes
18 *****************************************************************************/
19 
20 #include <gtest/gtest.h>
21 #include <ecl/linear_algebra.hpp>
22 #include <ecl/math/constants.hpp>
23 #include "../../include/ecl/geometry/angle.hpp"
24 #include "../../include/ecl/geometry/legacy_pose2d.hpp"
25 
26 /*****************************************************************************
27 ** Using
28 *****************************************************************************/
29 
30 using ecl::Angle;
31 using ecl::LegacyPose2D;
32 using ecl::RotationAngleStorage;
33 using ecl::linear_algebra::Matrix2d;
34 using ecl::linear_algebra::Vector2d;
35 using ecl::linear_algebra::Vector3d;
36 
37 /*****************************************************************************
38 ** Tests
39 *****************************************************************************/
40 
41 TEST(LegacyPose2DTests,RotationMatrixConstructors) {
42  LegacyPose2D<double> pose1;
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));
53  LegacyPose2D<double> pose2(1.0, 2.0, 3.14);
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;
58  Matrix2d rot = Angle<double>(3.14).rotationMatrix();
59  LegacyPose2D<double> pose3(rot,trans);
60  EXPECT_EQ(1.0,pose3.x());
61  EXPECT_EQ(2.0,pose3.y());
62  EXPECT_GT(3.15, pose3.heading());// Allow some roundoff error here
63  EXPECT_LT(3.13, pose3.heading());
64  LegacyPose2D<double> pose4(3.14,trans);
65  EXPECT_EQ(1.0,pose4.x());
66  EXPECT_EQ(2.0,pose4.y());
67  EXPECT_GT(3.15, pose4.heading());// Allow some roundoff error here
68  EXPECT_LT(3.13, pose4.heading());
69  LegacyPose2D<double> pose5(pose4);
70  EXPECT_EQ(1.0,pose5.x());
71  EXPECT_EQ(2.0,pose5.y());
72  EXPECT_GT(3.15, pose5.heading());// Allow some roundoff error here
73  EXPECT_LT(3.13, pose5.heading());
74  LegacyPose2D<double,RotationAngleStorage> pose_other(3.14,trans);
75  LegacyPose2D<double> pose6(pose_other);
76  EXPECT_EQ(1.0,pose6.x());
77  EXPECT_EQ(2.0,pose6.y());
78  EXPECT_GT(3.15, pose6.heading());// Allow some roundoff error here
79  EXPECT_LT(3.13, pose6.heading());
80 }
81 
82 TEST(LegacyPose2DTests,RotationAngleConstructors) {
83  LegacyPose2D<double,RotationAngleStorage> pose1;
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());
88  LegacyPose2D<double,RotationAngleStorage> pose2(1.0, 2.0, 3.14);
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;
93  Matrix2d rot = Angle<double>(3.14).rotationMatrix();
94  LegacyPose2D<double,RotationAngleStorage> pose3(rot,trans);
95  EXPECT_EQ(1.0,pose3.x());
96  EXPECT_EQ(2.0,pose3.y());
97  EXPECT_GT(3.15, pose3.heading());// Allow some roundoff error here
98  EXPECT_LT(3.13, pose3.heading());
99  LegacyPose2D<double,RotationAngleStorage> pose4(3.14,trans);
100  EXPECT_EQ(1.0,pose4.x());
101  EXPECT_EQ(2.0,pose4.y());
102  EXPECT_GT(3.15, pose4.heading());// Allow some roundoff error here
103  EXPECT_LT(3.13, pose4.heading());
104  LegacyPose2D<double,RotationAngleStorage> pose5(pose4);
105  EXPECT_EQ(1.0,pose5.x());
106  EXPECT_EQ(2.0,pose5.y());
107  EXPECT_GT(3.15, pose5.heading());// Allow some roundoff error here
108  EXPECT_LT(3.13, pose5.heading());
109  LegacyPose2D<double> pose_other(1.0, 2.0, 3.14);
110  LegacyPose2D<double,RotationAngleStorage> pose6(pose_other);
111  EXPECT_EQ(1.0,pose6.x());
112  EXPECT_EQ(2.0,pose6.y());
113  EXPECT_GT(3.15, pose6.heading());// Allow some roundoff error here
114  EXPECT_LT(3.13, pose6.heading());
115 }
116 
117 TEST(LegacyPose2DTests,assignment) {
118  LegacyPose2D<double> pose_r1(2.0, 1.0, 1.14);
119  LegacyPose2D<double,RotationAngleStorage> pose_a1(1.0, 2.0, 3.14);
120  LegacyPose2D<double> pose_r2;
121  LegacyPose2D<double,RotationAngleStorage> pose_a2;
122  pose_r2 = pose_r1;
123  EXPECT_EQ(2.0,pose_r2.x());
124  EXPECT_EQ(1.0,pose_r2.y());
125  EXPECT_GT(1.15, pose_r2.heading());// Allow some roundoff error here
126  EXPECT_LT(1.13, pose_r2.heading());
127  pose_r2 = pose_a1;
128  EXPECT_EQ(1.0,pose_r2.x());
129  EXPECT_EQ(2.0,pose_r2.y());
130  EXPECT_GT(3.15, pose_r2.heading());// Allow some roundoff error here
131  EXPECT_LT(3.13, pose_r2.heading());
132  pose_a2 = pose_r1;
133  EXPECT_EQ(2.0,pose_a2.x());
134  EXPECT_EQ(1.0,pose_a2.y());
135  EXPECT_GT(1.15, pose_a2.heading());// Allow some roundoff error here
136  EXPECT_LT(1.13, pose_a2.heading());
137  pose_a2 = pose_a1;
138  EXPECT_EQ(1.0,pose_a2.x());
139  EXPECT_EQ(2.0,pose_a2.y());
140  EXPECT_GT(3.15, pose_a2.heading());// Allow some roundoff error here
141  EXPECT_LT(3.13, pose_a2.heading());
142 }
143 
144 TEST(LegacyPose2DTests,eigenStyle) {
145  Vector2d trans; trans << 1.0, 2.0;
146  Matrix2d rot = Angle<double>(3.14).rotationMatrix();
147  LegacyPose2D<double> rpose;
148  rpose.rotation(rot);
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());// Allow some roundoff error here
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]);
158  EXPECT_GT(3.15, Angle<double>(rot));// Allow some roundoff error here
159  EXPECT_LT(3.13, Angle<double>(rot));
160  LegacyPose2D<double,RotationAngleStorage> apose;
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());// Allow some roundoff error here
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);// Allow some roundoff error here
172  EXPECT_LT(3.13, angle);
173 }
174 TEST(LegacyPose2DTests,convenienceStyle) {
175  Vector2d trans; trans << 1.0, 2.0;
176  Matrix2d rot;
177  LegacyPose2D<double> rpose;
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());// Allow some roundoff error here
182  EXPECT_LT(3.13, rpose.heading());
183  rpose.heading(0.0);
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));
193  LegacyPose2D<double,RotationAngleStorage> apose;
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());// Allow some roundoff error here
198  EXPECT_LT(3.13, apose.heading());
199  apose.heading(0.0);
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));
209 }
210 TEST(LegacyPose2DTests,inverse) {
211  LegacyPose2D<double> pose(1.0, 2.0, ecl::pi);
212  LegacyPose2D<double> inverse = pose.inverse();
213  EXPECT_GT(1.01,inverse.x()); // Allow some roundoff error here
214  EXPECT_LT(0.99,inverse.x());
215  EXPECT_GT(2.01,inverse.y()); // Allow some roundoff error here
216  EXPECT_LT(1.99,inverse.y());
217  EXPECT_GT(-ecl::pi+0.01,inverse.heading()); // Allow some roundoff error here
218  EXPECT_LT(-ecl::pi-0.01,inverse.heading());
219  LegacyPose2D<double,RotationAngleStorage> apose(1.0, 2.0, ecl::pi);
220  LegacyPose2D<double,RotationAngleStorage> ainverse = pose.inverse();
221  EXPECT_GT(1.01,ainverse.x()); // Allow some roundoff error here
222  EXPECT_LT(0.99,ainverse.x());
223  EXPECT_GT(2.01,ainverse.y()); // Allow some roundoff error here
224  EXPECT_LT(1.99,ainverse.y());
225  EXPECT_GT(-ecl::pi+0.01,ainverse.heading()); // Allow some roundoff error here
226  EXPECT_LT(-ecl::pi-0.01,ainverse.heading());
227 }
228 
229 TEST(LegacyPose2DTests,operators) {
230  LegacyPose2D<double> a(1.0, 2.0, 0.0), b(1.0, 3.0, ecl::pi);
231  LegacyPose2D<double> diff = a.inverse()*b; // diff = b - a;
232  EXPECT_EQ(0.0,diff.x());
233  EXPECT_EQ(1.0,diff.y());
234  EXPECT_EQ(ecl::pi,diff.heading());
235  LegacyPose2D<double,RotationAngleStorage> a2(1.0, 2.0, 0.0), b2(1.0, 3.0, ecl::pi);
236  LegacyPose2D<double,RotationAngleStorage> diff2 = a2.inverse()*b2; // diff = b - a;
237  EXPECT_EQ(0.0,diff2.x());
238  EXPECT_EQ(1.0,diff2.y());
239  EXPECT_EQ(ecl::pi,diff2.heading());
240 }
241 TEST(LegacyPose2DTests,relative) {
242  LegacyPose2D<double> a(1.0, 1.0, 1.57), b(1.0, 2.0, 3.14);
243  LegacyPose2D<double> brela = b.relative(a);
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());
250  LegacyPose2D<double,RotationAngleStorage> a_(1.0, 1.0, 1.57), b_(1.0, 2.0, 3.14);
251  LegacyPose2D<double,RotationAngleStorage> brela_ = b.relative(a);
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());
258 }
259 
260 /*****************************************************************************
261 ** Main program
262 *****************************************************************************/
263 
264 int main(int argc, char **argv) {
265  testing::InitGoogleTest(&argc,argv);
266  return RUN_ALL_TESTS();
267 }
main
int main(int argc, char **argv)
Definition: legacy_pose2d.cpp:264
ecl::RotationAngleStorage
RotationAngleStorage.
Definition: legacy_pose2d.hpp:42
ecl::pi
double const pi
linear_algebra.hpp
ecl::LegacyPose2D
Parent template definition for Pose2D.
Definition: legacy_pose2d.hpp:54
TEST
TEST(LegacyPose2DTests, RotationMatrixConstructors)
Definition: legacy_pose2d.cpp:41
ecl::Angle
Parent template definition for angles.
Definition: angle.hpp:113

ecl_geometry
Author(s): Daniel Stonier
autogenerated on Mon Feb 28 2022 22:18:49