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;
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) {
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());
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
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
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
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
80 }
81
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());
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());
92  Vector2d trans; trans << 1.0, 2.0;
93  Matrix2d rot = Angle<double>(3.14).rotationMatrix();
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
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
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
109  LegacyPose2D<double> pose_other(1.0, 2.0, 3.14);
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
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;
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
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
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
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
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
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));
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
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;
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
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));
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
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
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
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());
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());
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());
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());
258 }
259
260 /*****************************************************************************
261 ** Main program
262 *****************************************************************************/
263
264 int main(int argc, char **argv) {
266  return RUN_ALL_TESTS();
267 }
int main(int argc, char **argv)
RotationAngleStorage.
double const pi
Parent template definition for Pose2D.
TEST(LegacyPose2DTests, RotationMatrixConstructors)
Parent template definition for angles.
Definition: angle.hpp:113

ecl_geometry
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:08:37