MRPT  2.0.5
geometry_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 #include <gtest/gtest.h>
11 #include <mrpt/math/CPolygon.h>
12 #include <mrpt/math/TLine3D.h>
13 #include <mrpt/math/TObject2D.h>
14 #include <mrpt/math/TObject3D.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/geometry.h>
17 #include <algorithm>
18
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace std;
22
23 TEST(Geometry, Line2DIntersect)
24 {
25  // Two lines that should intersect at (0.5,0.5)
26  const TLine2D l1(TPoint2D(0, 1), TPoint2D(1, 0));
27  const TLine2D l2(TPoint2D(-1, 0.5), TPoint2D(4, 0.5));
28
29  TObject2D inter;
30  bool do_inter = intersect(l1, l2, inter);
31
32  EXPECT_TRUE(do_inter);
34
35  TPoint2D i(0, 0);
36  inter.getPoint(i);
37  EXPECT_NEAR(i.x, 0.5, 1e-9);
38  EXPECT_NEAR(i.y, 0.5, 1e-9);
39 }
40
41 TEST(Geometry, Line2DAngle)
42 {
43  const TLine2D l1(TPoint2D(0, 0), TPoint2D(1, 0));
44  const TLine2D l2(TPoint2D(-1, -1), TPoint2D(5, 5));
45
46  // Angles in 2D do have sign:
49
52
53  const TLine2D l3(TPoint2D(1, 0), TPoint2D(0, 0));
56 }
57
58 TEST(Geometry, Line3DAngle)
59 {
60  const TLine3D l1(TPoint3D(0, 0, 0), TPoint3D(1, 0, 0));
61  const TLine3D l2(TPoint3D(-1, -1, 0), TPoint3D(5, 5, 0));
62
63  // Angles in 3D don't have sign:
66
69
70  const TLine3D l3(TPoint3D(1, 0, 0), TPoint3D(0, 0, 0));
73
74  const TLine3D l4(
75  TPoint3D(0, 0, 0), TPoint3D(cos(30.0_deg), sin(30.0_deg), 0));
78 }
79
80 TEST(Geometry, Segment2DIntersect)
81 {
82  {
83  // Two segments that should intersect at (0.5,0.5)
84  const TSegment2D s1(TPoint2D(0, 1), TPoint2D(1, 0));
85  const TSegment2D s2(TPoint2D(-1, 0.5), TPoint2D(4, 0.5));
86
87  TObject2D inter;
88  bool do_inter = intersect(s1, s2, inter);
89
90  EXPECT_TRUE(do_inter);
92
93  TPoint2D i(0, 0);
94  inter.getPoint(i);
95  EXPECT_NEAR(i.x, 0.5, 1e-9);
96  EXPECT_NEAR(i.y, 0.5, 1e-9);
97  }
98
99  {
100  // Two segments that do NOT intersect
101  const TSegment2D s1(TPoint2D(0, 1), TPoint2D(1, 0));
102  const TSegment2D s2(TPoint2D(0.6, 0.5), TPoint2D(4, 0.5));
103
104  TObject2D inter;
105  bool do_inter = intersect(s1, s2, inter);
106
107  EXPECT_FALSE(do_inter);
108  }
109  {
110  // Two parallel segments that do NOT intersect: result is a "segment in
111  // the middle".
112  const TSegment2D s1(TPoint2D(-0.05, 0.05), TPoint2D(-0.05, -0.05));
113  const TSegment2D s2(TPoint2D(0, 0.135), TPoint2D(0, -0.0149999));
114
115  TObject2D inter;
116  bool do_inter = intersect(s1, s2, inter);
117
118  // EXPECT_TRUE(do_inter && inter.getType()==GEOMETRIC_TYPE_SEGMENT);
119  EXPECT_FALSE(do_inter);
120  }
121 }
122
123 TEST(Geometry, Intersection3D)
124 {
125  {
126  TPolygon3D p3d({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}});
127  TSegment3D s3d({
128  {1, 0, 0},
129  {0, 1, 0},
130  });
131
132  TObject3D inter;
133  EXPECT_TRUE(intersect(p3d, s3d, inter));
134  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_SEGMENT);
135  TSegment3D test;
136  inter.getSegment(test);
137  // Should this be true? EXPECT_EQ(s3d, test);
138  }
139  {
140  TPolygon3D p3d({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}});
141  TSegment3D s3d({
142  {0, 0, 0},
143  {1, 1, 1},
144  });
145
146  TObject3D inter;
147  EXPECT_TRUE(intersect(p3d, s3d, inter));
148  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
149  }
150  {
151  TSegment3D s3d1({
152  {1, 0, 0},
153  {0, 1, 0},
154  });
155  TSegment3D s3d2({
156  {2, -1.0, 0},
157  {0, 1.0, 0},
158  });
159
160  TObject3D inter;
161  EXPECT_TRUE(intersect(s3d1, s3d2, inter));
162  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_SEGMENT);
163  }
164  {
165  TSegment3D s3d1({
166  {1, 0, 0},
167  {0, 1, 0},
168  });
169  TSegment3D s3d2({
170  {0, 0, 0},
171  {1, 1, 0},
172  });
173
174  TObject3D inter;
175  EXPECT_TRUE(intersect(s3d1, s3d2, inter));
176  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
177
178  TPoint3D test;
179  TPoint3D expect{0.5, 0.5, 0};
180  inter.getPoint(test);
181  EXPECT_EQ(expect, test);
182  }
183 }
184
185 TEST(Geometry, IntersectionPlanePlane)
186 {
187  {
188  // Parallel planes
189  TPlane plane1({
190  {1, 0, 0},
191  {0, 1, 0},
192  {0, 0, 1},
193  });
194  TPlane plane2({
195  {2, 0, 0},
196  {0, 2, 0},
197  {0, 0, 2},
198  });
199
200  TObject3D inter;
201  EXPECT_FALSE(intersect(plane1, plane2, inter));
202  }
203  {
204  // Same plane
205  TPlane plane1({
206  {1, 0, 0},
207  {0, 1, 0},
208  {0, 0, 1},
209  });
210  TPlane plane2({
211  {-1, 1, 1},
212  {1, -1, 1},
213  {1, 1, -1},
214  });
215
216  TObject3D inter;
217  EXPECT_TRUE(intersect(plane1, plane2, inter));
218  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_PLANE);
219  }
220  {
221  // Intersecting planes
222  TPlane plane1({
223  {1, 0, 0},
224  {0, 1, 0},
225  {0, 0, 1},
226  });
227  TPlane plane2({
228  {1, 0, 0},
229  {0, -1, 0},
230  {0, 0, -1},
231  });
232
233  TObject3D inter;
234  EXPECT_TRUE(intersect(plane1, plane2, inter));
235  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_LINE);
236  }
237 }
238
239 void myTestPolygonContainsPoint(std::vector<TPoint2D>& vs, bool convex)
240 {
241  const mrpt::math::TPolygon2D poly(vs);
242
243  EXPECT_EQ(poly.isConvex(), convex);
244
245  EXPECT_TRUE(poly.contains(TPoint2D(0.0, 0.0)));
246  EXPECT_TRUE(poly.contains(TPoint2D(0.0, 0.9)));
247  EXPECT_TRUE(poly.contains(TPoint2D(-0.9, -0.9)));
248  EXPECT_TRUE(poly.contains(TPoint2D(0.9, -0.9)));
249
250  EXPECT_FALSE(poly.contains(TPoint2D(-4.0, -5.1)));
251  EXPECT_FALSE(poly.contains(TPoint2D(-5.0, -0.1)));
252  EXPECT_FALSE(poly.contains(TPoint2D(1.1, -6.1)));
253  EXPECT_FALSE(poly.contains(TPoint2D(0, 5.1)));
254  EXPECT_FALSE(poly.contains(TPoint2D(0, -1.1)));
255 }
256
257 TEST(Geometry, PolygonConvexContainsPoint)
258 {
259  // Test with a polygon in one winding order:
260  std::vector<TPoint2D> vs;
261  vs.emplace_back(-1.0, -1.0);
262  vs.emplace_back(0.0, 1.0);
263  vs.emplace_back(1.0, -1.0);
264  myTestPolygonContainsPoint(vs, true);
265
266  // and the other:
267  std::reverse(vs.begin(), vs.end());
268  myTestPolygonContainsPoint(vs, true);
269
270  {
276
277  EXPECT_FALSE(p.contains(TPoint2D(0.73175, -0.325796)));
278  }
279 }
280
281 TEST(Geometry, PolygonConcaveContainsPoint)
282 {
283  // Test with a polygon in one winding order:
284  std::vector<TPoint2D> vs;
285  vs.emplace_back(-2.0, 3.0);
286  vs.emplace_back(2.0, 2.0);
287  vs.emplace_back(3.0, -4.0);
288  vs.emplace_back(0.1, -3.0);
289  vs.emplace_back(0.1, -0.1);
290  vs.emplace_back(-0.1, -0.1);
291  vs.emplace_back(-0.1, -3.0);
292  vs.emplace_back(-2.0, -2.0);
293
294  myTestPolygonContainsPoint(vs, false);
295
296  // and the other:
297  std::reverse(vs.begin(), vs.end());
298  myTestPolygonContainsPoint(vs, false);
299 }
300
301 TEST(Geometry, changeEpsilon)
302 {
303  // Default value:
304  const double default_val = 1e-5;
305  EXPECT_NEAR(mrpt::math::getEpsilon(), default_val, 1e-9);
306
307  // Test changing:
309  EXPECT_NEAR(mrpt::math::getEpsilon(), 0.1, 1e-9);
310
311  // Test actual effects of epsilon:
312  {
313  const auto l1 = TLine2D({0.0, 0.0}, {1.0, 0.0});
314  const auto l2 = TLine2D({0.0, 2.0}, {1.0, 2.0001});
315
316  TObject2D obj;
318  EXPECT_FALSE(mrpt::math::intersect(l1, l2, obj));
319  mrpt::math::setEpsilon(1e-10);
320  EXPECT_TRUE(mrpt::math::intersect(l1, l2, obj));
321  }
322
323  // Reset
324  mrpt::math::setEpsilon(default_val);
325 }
326
327 TEST(Geometry, conformAPlane)
328 {
329  {
330  std::vector<TPoint3D> pts = {
331  {0., 0., 0.}, {1., 0., 0.}, {1., 1., 0.}, {0., 1., 0.}};
333  }
334  {
335  std::vector<TPoint3D> pts = {
336  {0., 0., 0.}, {0., 0., 1.}, {0., 1., 1.}, {0., 1., 0.}};
338  }
339  {
340  std::vector<TPoint3D> pts = {
341  {0., 0., 0.}, {0., 0., 1.}, {0., 1., 1.}, {0.1, 1., 0.1}};
342  EXPECT_FALSE(mrpt::math::conformAPlane(pts));
343  }
344  {
345  std::vector<TPoint3D> pts = {{5.56496063, -2.30508217, 29.53900000},
346  {5.87949871, 0.00000000, 29.53900000},
347  {13.50000000, 0.00000000, 0.00000000},
348  {12.50465807, -7.29433126, 0.00000000}};
350  }
351 }
352
354 {
355  const auto r1_xmin = -1.0, r1_xmax = 1.0, r1_ymin = -1.0, r1_ymax = 1.0;
356  const auto r2_xmin = -2.0, r2_xmax = 2.0, r2_ymin = -3.0, r2_ymax = 3.0;
357
359
360  using tst_set_t = std::array<double, 4>;
361
362  // Test cases: x,y,phi, 0/1:false/true (expected output)
363  const std::vector<tst_set_t> tsts = {
364  {0, 0, 0.0_deg, /*result*/ 1}, {3.1, 0, 0.0_deg, /*result*/ 0},
365  {-3.1, 0, 0.0_deg, /*result*/ 0}, {2.9, 0, 0.0_deg, /*result*/ 1},
366  {-2.9, 0, 0.0_deg, /*result*/ 1}, {0, 4.1, 0.0_deg, /*result*/ 0},
367  {0, 3.9, 0.0_deg, /*result*/ 1}, {0, -4.1, 0.0_deg, /*result*/ 0},
368  {0, -3.9, 0.0_deg, /*result*/ 1}, {3.1, 0, 0.0_deg, /*result*/ 0},
369  {3.1, 0, 45.0_deg, /*result*/ 1}, {3.1, 0, -90.0_deg, /*result*/ 1}};
370
371  for (const auto& t : tsts)
372  {
373  const auto p = mrpt::math::TPose2D(t[0], t[1], t[2]);
374  EXPECT_EQ(
376  r1_xmin, r1_xmax, r1_ymin, r1_ymax, r2_xmin, r2_xmax, r2_ymin,
377  r2_ymax, p.x, p.y, p.phi),
378  t[3] != 0.0);
379  }
380 }
bool RectanglesIntersection(double R1_x_min, double R1_x_max, double R1_y_min, double R1_y_max, double R2_x_min, double R2_x_max, double R2_y_min, double R2_y_max, double R2_pose_x, double R2_pose_y, double R2_pose_phi)
Returns whether two rotated rectangles intersect.
Definition: geometry.cpp:367
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Definition: TObject2D.h:109
void myTestPolygonContainsPoint(std::vector< TPoint2D > &vs, bool convex)
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
unsigned char getType() const
Gets content type.
Definition: TObject2D.h:105
void setEpsilon(double nE)
Changes the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:35
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Definition: TPoseOrPoint.h:110
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
STL namespace.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon2D.cpp:69
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:969
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
Definition: TPoseOrPoint.h:130
TEST(Geometry, Line2DIntersect)
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
3D segment, consisting of two points.
Definition: TSegment3D.h:20
bool isConvex() const
Checks whether is convex.
Definition: TPolygon2D.cpp:117
3D Plane, represented by its equation
Definition: TPlane.h:22
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:846
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
Lightweight 2D pose.
Definition: TPose2D.h:22
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
Definition: TPoseOrPoint.h:115
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
Add a new vertex to polygon.
Definition: CPolygon.h:28
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:617
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
Definition: TPoseOrPoint.h:120
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:20
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19

 Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: eda5ade6c Tue Aug 4 12:48:50 2020 +0200 at mar ago 4 13:00:11 CEST 2020