test_lanelet_or_area_path.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include <lanelet2_core/geometry/Polygon.h>
3 
4 #include <boost/geometry/algorithms/equals.hpp>
5 
6 #include "lanelet2_routing/LaneletPath.h"
7 
8 using namespace lanelet;
9 using namespace lanelet::routing;
10 
11 /*
12  * 6 l8 7 l7 8 l6 9 l5 10
13  * X<------X<------X<------X<------X
14  * ^ ^ ^ ^
15  * |l9 |l10 |l11 |l12
16  * | a1 | a2 | ll1 | ll2
17  * | | ll3 | ll4 |
18  * X------>X------>X------>X------>X
19  * 1 l1 2 l2 3 l3 4 l4 5
20  * | a51 | WE COME
21  * | V IN PEACE
22  * X<------X
23  * 12 11
24  */
25 
26 class LaneletOrAreaTest : public ::testing::Test {
27  private:
28  void SetUp() override {
29  Id id(1000);
30  p1 = Point3d(++id, 0, 0, 0);
31  p2 = Point3d(++id, 1, 0, 0);
32  p3 = Point3d(++id, 2, 0, 0);
33  p4 = Point3d(++id, 3, 0, 0);
34  p5 = Point3d(++id, 4, 0, 0);
35  p6 = Point3d(++id, 0, 1, 0);
36  p7 = Point3d(++id, 1, 1, 0);
37  p8 = Point3d(++id, 2, 1, 0);
38  p9 = Point3d(++id, 3, 1, 0);
39  p10 = Point3d(++id, 4, 1, 0);
40  p11 = Point3d(++id, 4, -1, 0);
41  p12 = Point3d(++id, 3, -1, 0);
42 
43  ls1 = LineString3d(++id, {p1, p2});
44  ls2 = LineString3d(++id, {p2, p3});
45  ls3 = LineString3d(++id, {p3, p4});
46  ls4 = LineString3d(++id, {p4, p5});
47  ls5 = LineString3d(++id, {p10, p9});
48  ls6 = LineString3d(++id, {p9, p8});
49  ls7 = LineString3d(++id, {p8, p7});
50  ls8 = LineString3d(++id, {p7, p6});
51  ls9 = LineString3d(++id, {p1, p6});
52  ls10 = LineString3d(++id, {p2, p7});
53  ls11 = LineString3d(++id, {p3, p8});
54  ls12 = LineString3d(++id, {p4, p9});
55  ls13 = LineString3d(++id, {p5, p11, p12, p4});
56 
57  l1 = Lanelet(++id, ls6.invert(), ls3);
58  l2 = Lanelet(++id, ls5.invert(), ls4);
59  l3 = Lanelet(++id, ls10, ls11);
60  l4 = Lanelet(++id, ls11, ls12);
61 
62  a1 = Area(++id, {ls9, ls8.invert(), ls10.invert(), ls1.invert()});
63  a2 = Area(++id, {ls10, ls7.invert(), ls11.invert(), ls2.invert()});
64  a51 = Area(++id, {ls4, ls13});
65 
66  areaPath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(a2)});
67  laneletPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)});
68  bothPath = LaneletOrAreaPath({ConstLaneletOrArea(a2), ConstLaneletOrArea(l1)});
69  invAreaPath = LaneletOrAreaPath({ConstLaneletOrArea(l1.invert()), ConstLaneletOrArea(a2)});
70  invLLPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())});
71  longPath = LaneletOrAreaPath(
72  {ConstLaneletOrArea(a1), ConstLaneletOrArea(a2), ConstLaneletOrArea(l1), ConstLaneletOrArea(l2)});
73  longInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert()),
74  ConstLaneletOrArea(a2), ConstLaneletOrArea(a1)});
75  sidePath = LaneletOrAreaPath({ConstLaneletOrArea(a1), ConstLaneletOrArea(l3), ConstLaneletOrArea(l4)});
76  sideInvPath = LaneletOrAreaPath({ConstLaneletOrArea(l3), ConstLaneletOrArea(a1)});
77  cornerPath = LaneletOrAreaPath({ConstLaneletOrArea(l1), ConstLaneletOrArea(l2), ConstLaneletOrArea(a51)});
78  cornerPathInv =
79  LaneletOrAreaPath({ConstLaneletOrArea(a51), ConstLaneletOrArea(l2.invert()), ConstLaneletOrArea(l1.invert())});
80  }
81 
82  public:
83  Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11, p12;
84  LineString3d ls1, ls2, ls3, ls4, ls5, ls6, ls7, ls8, ls9, ls10, ls11, ls12, ls13;
85  Lanelet l1, l2, l3, l4;
86  Area a1, a2, a51;
87  LaneletOrAreaPath areaPath, laneletPath, bothPath, invAreaPath, invLLPath, longPath, longInvPath, sidePath,
88  sideInvPath, cornerPath, cornerPathInv;
89 };
90 namespace {
91 void checkIdentical(const BasicPolygon3d& lhs, const BasicPolygon3d& rhs) {
92  auto to2D = [](const BasicPolygon3d& p3d) {
93  BasicPolygon2d p2d(p3d.size());
94  for (size_t i = 0; i < p3d.size(); ++i) {
95  p2d[i] = p3d.at(i).head<2>();
96  }
97  return p2d;
98  };
99  BasicPolygon2d lhs2d = to2D(lhs);
100  BasicPolygon2d rhs2d = to2D(rhs);
101  // equals is buggy in older boost versions
102 #if BOOST_VERSION > 106500
103  EXPECT_TRUE(boost::geometry::equals(lhs2d, rhs2d));
104 #endif
105 }
106 
107 void checkEvenlySpaced(const BasicPolygon3d& poly, const double dist = 1.) {
108  for (size_t i = 0; i + 1 < poly.size(); ++i) {
109  EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.at(i), poly.at(i + 1)), dist);
110  }
111  EXPECT_DOUBLE_EQ(boost::geometry::distance(poly.back(), poly.front()), dist);
112 }
113 
114 } // namespace
115 
116 TEST_F(LaneletOrAreaTest, enclosingPolygonAreas) { // NOLINT
117  BasicPolygons2d intersect;
118  BasicPolygon3d joined = getEnclosingPolygon3d(areaPath);
119  BasicPolygon3d expected{p1, p6, p8, p3};
120  checkIdentical(joined, expected);
121  checkEvenlySpaced(joined);
122  EXPECT_EQ(joined.size(), 6ul);
123 }
124 
125 TEST_F(LaneletOrAreaTest, enclosingPolygonLanelets) { // NOLINT
126  BasicPolygons2d intersect;
127  BasicPolygon3d joined = getEnclosingPolygon3d(laneletPath);
128  BasicPolygon3d expected{p3, p8, p10, p5};
129  checkIdentical(joined, expected);
130  checkEvenlySpaced(joined);
131  EXPECT_EQ(joined.size(), 6ul);
132 }
133 
134 TEST_F(LaneletOrAreaTest, enclosingPolygonLaneletsInverted) { // NOLINT
135  BasicPolygons2d intersect;
136  BasicPolygon3d joined = getEnclosingPolygon3d(invLLPath);
137  BasicPolygon3d expected{p3, p8, p10, p5};
138  checkIdentical(joined, expected);
139  checkEvenlySpaced(joined);
140  EXPECT_EQ(joined.size(), 6ul);
141 }
142 
143 TEST_F(LaneletOrAreaTest, enclosingPolygonMixed) { // NOLINT
144  BasicPolygons2d intersect;
145  BasicPolygon3d joined = getEnclosingPolygon3d(bothPath);
146  BasicPolygon3d expected{p2, p7, p9, p4};
147  checkIdentical(joined, expected);
148  checkEvenlySpaced(joined);
149  EXPECT_EQ(joined.size(), 6ul);
150 }
151 
152 TEST_F(LaneletOrAreaTest, enclosingPolygonAreaInverted) { // NOLINT
153  BasicPolygons2d intersect;
154  BasicPolygon3d joined = getEnclosingPolygon3d(invAreaPath);
155  BasicPolygon3d expected{p2, p7, p9, p4};
156  checkIdentical(joined, expected);
157  checkEvenlySpaced(joined);
158  EXPECT_EQ(joined.size(), 6ul);
159 }
160 
161 TEST_F(LaneletOrAreaTest, enclosingPolygonLong) { // NOLINT
162  BasicPolygons2d intersect;
163  BasicPolygon3d joined = getEnclosingPolygon3d(longPath);
164  BasicPolygon3d expected{p1, p6, p10, p5};
165  checkIdentical(joined, expected);
166  checkEvenlySpaced(joined);
167  EXPECT_EQ(joined.size(), 10ul);
168 }
169 
170 TEST_F(LaneletOrAreaTest, enclosingPolygonLongInverted) { // NOLINT
171  BasicPolygons2d intersect;
172  BasicPolygon3d joined = getEnclosingPolygon3d(longInvPath);
173  BasicPolygon3d expected{p1, p6, p10, p5};
174  checkIdentical(joined, expected);
175  checkEvenlySpaced(joined);
176  EXPECT_EQ(joined.size(), 10ul);
177 }
178 
179 TEST_F(LaneletOrAreaTest, enclosingPolygonSideways) { // NOLINT
180  BasicPolygons2d intersect;
181  BasicPolygon3d joined = getEnclosingPolygon3d(sidePath);
182  BasicPolygon3d expected{p1, p6, p9, p4};
183  checkIdentical(joined, expected);
184  checkEvenlySpaced(joined);
185  EXPECT_EQ(joined.size(), 8ul);
186 }
187 
188 TEST_F(LaneletOrAreaTest, enclosingPolygonSidewaysInvertrd) { // NOLINT
189  BasicPolygons2d intersect;
190  BasicPolygon3d joined = getEnclosingPolygon3d(sideInvPath);
191  BasicPolygon3d expected{p1, p6, p8, p3};
192  checkIdentical(joined, expected);
193  checkEvenlySpaced(joined);
194  EXPECT_EQ(joined.size(), 6ul);
195 }
196 
197 TEST_F(LaneletOrAreaTest, enclosingPolygonCorner) { // NOLINT
198  BasicPolygons2d intersect;
199  BasicPolygon3d joined = getEnclosingPolygon3d(cornerPath);
200  BasicPolygon3d expected{p8, p10, p11, p12, p4, p3};
201  checkIdentical(joined, expected);
202  checkEvenlySpaced(joined);
203  EXPECT_EQ(joined.size(), 8ul);
204 }
205 
206 TEST_F(LaneletOrAreaTest, enclosingPolygonCornerInv) { // NOLINT
207  BasicPolygons2d intersect;
208  BasicPolygon3d joined = getEnclosingPolygon3d(cornerPathInv);
209  BasicPolygon3d expected{p8, p10, p11, p12, p4, p3};
210  checkIdentical(joined, expected);
211  checkEvenlySpaced(joined);
212  EXPECT_EQ(joined.size(), 8ul);
213 }
BasicPolygons2d
std::vector< BasicPolygon2d > BasicPolygons2d
TEST_F
TEST_F(LaneletOrAreaTest, enclosingPolygonAreas)
Definition: test_lanelet_or_area_path.cpp:116
to2D
BoundingBox2d to2D(const BoundingBox3d &primitive)
lanelet
LaneletOrAreaTest::SetUp
void SetUp() override
Definition: test_lanelet_or_area_path.cpp:28
LaneletOrAreaTest::sidePath
LaneletOrAreaPath sidePath
Definition: test_lanelet_or_area_path.cpp:87
p2
BasicPoint p2
lanelet::ConstLaneletOrArea
LaneletOrAreaTest::ls9
LineString3d ls9
Definition: test_lanelet_or_area_path.cpp:84
Id
int64_t Id
LaneletOrAreaTest
Definition: test_lanelet_or_area_path.cpp:26
lanelet::routing
Definition: Forward.h:11
id
Id id
lanelet::Area
lanelet::Lanelet
lanelet::BasicPolygon3d
lanelet::LineString3d::invert
LineString3d invert() const noexcept
LaneletPath.h
p1
BasicPoint p1
lanelet::routing::LaneletOrAreaPath
Similar to LaneletPath, but can also contain areas.
Definition: LaneletPath.h:49
lanelet::Point3d
LaneletOrAreaTest::a51
Area a51
Definition: test_lanelet_or_area_path.cpp:86
LaneletOrAreaTest::sideInvPath
LaneletOrAreaPath sideInvPath
Definition: test_lanelet_or_area_path.cpp:88
LaneletOrAreaTest::p9
Point3d p9
Definition: test_lanelet_or_area_path.cpp:83
LaneletOrAreaTest::l4
Lanelet l4
Definition: test_lanelet_or_area_path.cpp:85
lanelet::LineString3d
lanelet::BasicPolygon2d
lanelet::routing::getEnclosingPolygon3d
BasicPolygon3d getEnclosingPolygon3d(const LaneletOrAreaPath &path)
Definition: LaneletPath.cpp:338

lanelet2_routing
Author(s): Matthias Mayr
autogenerated on Thu Mar 6 2025 03:26:10