MRPT  1.9.9
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 |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
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);
33  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
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:
47  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l1, l2)), +45.0, 1e-5);
48  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l2, l1)), -45.0, 1e-5);
49 
50  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l1, l1)), 0.0, 1e-5);
51  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l2, l2)), 0.0, 1e-5);
52 
53  const TLine2D l3(TPoint2D(1, 0), TPoint2D(0, 0));
54  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l1, l3))), 180.0, 1e-5);
55  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l3, l1))), 180.0, 1e-5);
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:
64  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l1, l2)), 45.0, 1e-5);
65  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l2, l1)), 45.0, 1e-5);
66 
67  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l1, l1)), 0.0, 1e-5);
68  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l2, l2)), 0.0, 1e-5);
69 
70  const TLine3D l3(TPoint3D(1, 0, 0), TPoint3D(0, 0, 0));
71  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l1, l3))), 180.0, 1e-5);
72  EXPECT_NEAR(RAD2DEG(std::abs(mrpt::math::getAngle(l3, l1))), 180.0, 1e-5);
73 
74  const TLine3D l4(
75  TPoint3D(0, 0, 0), TPoint3D(cos(30.0_deg), sin(30.0_deg), 0));
76  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l1, l4)), 30.0, 1e-5);
77  EXPECT_NEAR(mrpt::RAD2DEG(mrpt::math::getAngle(l4, l1)), 30.0, 1e-5);
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);
91  EXPECT_EQ(inter.getType(), GEOMETRIC_TYPE_POINT);
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  {
271  mrpt::math::CPolygon p;
272  p.AddVertex(0, -0.322);
273  p.AddVertex(-0.644, -0.322);
274  p.AddVertex(-0.210377, -0.324673);
275  p.AddVertex(0.433623, -0.324673);
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:
308  mrpt::math::setEpsilon(0.1);
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;
317  mrpt::math::setEpsilon(0.1);
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.}};
332  EXPECT_TRUE(mrpt::math::conformAPlane(pts));
333  }
334  {
335  std::vector<TPoint3D> pts = {
336  {0., 0., 0.}, {0., 0., 1.}, {0., 1., 1.}, {0., 1., 0.}};
337  EXPECT_TRUE(mrpt::math::conformAPlane(pts));
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}};
349  EXPECT_TRUE(mrpt::math::conformAPlane(pts));
350  }
351 }
352 
353 TEST(Geometry, RectanglesIntersection)
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 
358  using mrpt::math::RectanglesIntersection;
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(
375  RectanglesIntersection(
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 }
mrpt::math::RectanglesIntersection
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
mrpt::math::TObject2D::getPoint
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Definition: TObject2D.h:109
TObject3D.h
myTestPolygonContainsPoint
void myTestPolygonContainsPoint(std::vector< TPoint2D > &vs, bool convex)
Definition: geometry_unittest.cpp:239
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
mrpt::math::TPoint2D
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
mrpt::math::TObject2D::getType
unsigned char getType() const
Gets content type.
Definition: TObject2D.h:105
mrpt::math::setEpsilon
void setEpsilon(double nE)
Changes the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:35
CPolygon.h
mrpt::math::TObject2D
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
mrpt::math::CPolygon
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
mrpt::math::GEOMETRIC_TYPE_POINT
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Definition: TPoseOrPoint.h:110
mrpt::math::TObject3D
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
std
STL namespace.
TObject2D.h
mrpt::math::TPolygon2D::contains
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
Definition: TPolygon2D.cpp:69
mrpt::math::conformAPlane
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:969
mrpt::math::GEOMETRIC_TYPE_PLANE
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
Definition: TPoseOrPoint.h:130
TEST
TEST(Geometry, Line2DIntersect)
Definition: geometry_unittest.cpp:23
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::TSegment2D
2D segment, consisting of two points.
Definition: TSegment2D.h:20
mrpt::math::TSegment3D
3D segment, consisting of two points.
Definition: TSegment3D.h:20
mrpt::math::TPolygon2D::isConvex
bool isConvex() const
Checks whether is convex.
Definition: TPolygon2D.cpp:117
mrpt::math::TPlane
3D Plane, represented by its equation
Definition: TPlane.h:22
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::math::getAngle
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:846
mrpt::math::TPoint2D_< double >
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::math::getEpsilon
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
mrpt::math::TPoint3D_< double >
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
TPose2D.h
mrpt::math::GEOMETRIC_TYPE_SEGMENT
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
Definition: TPoseOrPoint.h:115
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
mrpt::math::CPolygon::AddVertex
void AddVertex(double x, double y)
Add a new vertex to polygon.
Definition: CPolygon.h:28
mrpt::math::intersect
bool intersect(const TSegment3D &s1, const TSegment3D &s2, TObject3D &obj)
Gets the intersection between two 3D segments.
Definition: geometry.cpp:617
mrpt::math::GEOMETRIC_TYPE_LINE
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
Definition: TPoseOrPoint.h:120
mrpt::math::TPolygon2D
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
mrpt::math::TPolygon3D
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:20
TLine3D.h
mrpt::math::TLine3D
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020