test_lanelet.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include <iostream>
4 
10 
11 using namespace std::literals;
12 using namespace lanelet;
13 
14 Lanelet bufferLanelet(Lanelet llt, double z) {
15  auto bufferPoints = [&llt, z](const auto& elem) {
16  return Point3d(llt.id() + elem.id(), elem.x(), elem.y(), elem.z() + z);
17  };
18  LineString3d left(llt.id() + llt.leftBound().id(), utils::transform(llt.leftBound(), bufferPoints));
19  LineString3d right(llt.id() + llt.rightBound().id(), utils::transform(llt.rightBound(), bufferPoints));
20  return Lanelet(llt.id(), left, right);
21 };
22 
24  Points2d intersectionPts;
25  boost::geometry::intersection(ls, lsRef, intersectionPts);
26  for (auto& pt : intersectionPts) {
27  EXPECT_TRUE(geometry::distance(pt, lsRef.front()) < 0.01 || geometry::distance(pt, lsRef.back()) < 0.01);
28  }
29 }
30 
31 void testCenterline(const ConstLineString3d& centerline, const ConstLineString3d& leftBound,
32  const ConstLineString3d& rightBound) {
33  EXPECT_GE(centerline.size(), 2ul);
34  ConstHybridLineString2d lb(leftBound);
35  ConstHybridLineString2d rb(rightBound);
36  ConstHybridLineString2d c(centerline);
37  testHasIntersection(c, lb);
38  testHasIntersection(c, rb);
39 }
40 
41 class LaneletTest : public ::testing::Test {
42  protected:
43  void SetUp() override {
44  id = 0;
45  p1 = Point3d(++id, 0., 1., 1.);
46  p2 = Point3d(++id, 1., 1., 1.);
47  p3 = Point3d(++id, 0., 0., 0.);
48  p4 = Point3d(++id, 1., 0., 0.);
49  p5 = Point3d(++id, 0., 0.5, 0.5);
50  p6 = Point3d(++id, 0.5, 0.5, 0.5);
51  p7 = Point3d(++id, 1., 0.5, 0.);
52  p8 = Point3d(++id, 0., -1., 0.);
53  p9 = Point3d(++id, 1., -1., 0.);
54  left = LineString3d(++id, Points3d{p1, p2});
55  right = LineString3d(++id, Points3d{p3, p4});
56  other = LineString3d(++id, Points3d{p5, p6, p7});
57  outside = LineString3d(++id, Points3d{p8, p9});
58  ritterLanelet = Lanelet(++id, left, right);
59  constRitterLanelet = ritterLanelet;
60  }
61 
62  public:
63  Id id{1};
64  Point3d p1, p2, p3, p4, p5, p6, p7, p8, p9;
65  LineString3d left, right, other, outside;
68 };
69 
70 TEST_F(LaneletTest, id) { // NOLINT
71  ritterLanelet.setId(100);
72  EXPECT_EQ(100, ritterLanelet.id());
73  EXPECT_EQ(100, constRitterLanelet.id());
74 }
75 
76 TEST_F(LaneletTest, nullptrConstruct) { // NOLINT
77  EXPECT_THROW(Lanelet(std::shared_ptr<LaneletData>()), NullptrError); // NOLINT
78  EXPECT_THROW(ConstLanelet(std::shared_ptr<LaneletData>()), NullptrError); // NOLINT
79 }
80 
81 TEST_F(LaneletTest, bounds) { // NOLINT
82  EXPECT_EQ(left, ritterLanelet.leftBound());
83  EXPECT_EQ(right, ritterLanelet.rightBound());
84  EXPECT_EQ(left, constRitterLanelet.leftBound());
85  EXPECT_EQ(right, constRitterLanelet.rightBound());
86  ritterLanelet.setLeftBound(other);
87  EXPECT_EQ(other, ritterLanelet.leftBound());
88  EXPECT_EQ(other, constRitterLanelet.leftBound());
89 }
90 
91 TEST_F(LaneletTest, attributes) { // NOLINT
92  ritterLanelet.setAttribute("test", "value");
93  ritterLanelet.setAttribute(AttributeName::Subtype, AttributeValueString::Road);
94  EXPECT_TRUE(ritterLanelet.hasAttribute("test"));
95  EXPECT_EQ("value"s, ritterLanelet.attribute("test").value());
96  EXPECT_EQ("value"s, constRitterLanelet.attribute("test").value());
97  EXPECT_EQ(ritterLanelet.attribute(AttributeName::Subtype), AttributeValueString::Road);
98  EXPECT_EQ(constRitterLanelet.attribute(AttributeName::Subtype), AttributeValueString::Road);
99  EXPECT_THROW(ritterLanelet.attribute("doesnotexist"), NoSuchAttributeError); // NOLINT
100 }
101 
102 TEST_F(LaneletTest, invert) { // NOLINT
103  auto invertedLanelet = ritterLanelet.invert();
104  EXPECT_TRUE(invertedLanelet.inverted());
105  EXPECT_NE(ritterLanelet, invertedLanelet);
106  EXPECT_EQ(ritterLanelet, invertedLanelet.invert());
107  EXPECT_EQ(right.id(), invertedLanelet.leftBound().id());
108  EXPECT_EQ(left.id(), invertedLanelet.rightBound().id());
109 
110  auto constInvertedLanelet = constRitterLanelet.invert();
111  EXPECT_EQ(right.id(), constInvertedLanelet.leftBound().id());
112  EXPECT_EQ(left.id(), constInvertedLanelet.rightBound().id());
113 
114  auto constInvertedInvertedLanelet = constInvertedLanelet.invert();
115  EXPECT_EQ(constRitterLanelet, constInvertedInvertedLanelet);
116  EXPECT_EQ(right, constInvertedInvertedLanelet.rightBound());
117  EXPECT_EQ(left, constInvertedInvertedLanelet.leftBound());
118 }
119 
120 TEST_F(LaneletTest, modifyInvert) { // NOLINT
121  auto invertedLanelet = ritterLanelet.invert();
122  invertedLanelet.setLeftBound(right);
123  invertedLanelet.setRightBound(left);
124 
125  EXPECT_EQ(invertedLanelet.leftBound().id(), right.id());
126  EXPECT_EQ(invertedLanelet.leftBound().front(), right.front());
127  EXPECT_EQ(ritterLanelet.leftBound().front(), left.back());
128  EXPECT_EQ(ritterLanelet.rightBound().front(), right.back());
129 }
130 
131 TEST_F(LaneletTest, centerline) { // NOLINT
132  auto centerline = ritterLanelet.centerline();
133  testCenterline(centerline, ritterLanelet.leftBound(), ritterLanelet.rightBound());
134  EXPECT_DOUBLE_EQ(double(geometry::length(centerline)), 1);
135 
136  auto invCenterline = ritterLanelet.invert().centerline();
137  EXPECT_EQ(invCenterline.front().x(), centerline.back().x());
138  EXPECT_EQ(invCenterline.front().y(), centerline.back().y());
139 }
140 
141 TEST_F(LaneletTest, setCenterline) { // NOLINT
142  ritterLanelet.setCenterline(other);
143  EXPECT_EQ(other.id(), ritterLanelet.centerline().id());
144  ritterLanelet.setRightBound(outside);
145  EXPECT_TRUE(ritterLanelet.hasCustomCenterline());
146  EXPECT_EQ(other.id(), ritterLanelet.centerline().id());
147 }
148 
149 TEST_F(LaneletTest, area) { // NOLINT
150  EXPECT_DOUBLE_EQ(1, geometry::area(ritterLanelet.polygon2d()));
151  EXPECT_DOUBLE_EQ(1, geometry::area(constRitterLanelet.polygon2d()));
152 }
153 
154 TEST_F(LaneletTest, inside) { // NOLINT
155  using traits::to2D;
156  EXPECT_TRUE(geometry::inside(ritterLanelet, to2D(p5))); // point on edge
157  EXPECT_TRUE(geometry::inside(ritterLanelet, to2D(p6))); // point in middle
158  EXPECT_FALSE(geometry::inside(ritterLanelet, to2D(p8))); // point outside
159 
160  EXPECT_TRUE(geometry::inside(ritterLanelet, BasicPoint2d(0.5, 0.5)));
161  EXPECT_FALSE(geometry::inside(ritterLanelet, BasicPoint2d(-1, -1)));
162 }
163 
164 TEST_F(LaneletTest, boundingbox) { // NOLINT
165  auto box = geometry::boundingBox2d(ritterLanelet);
166  EXPECT_DOUBLE_EQ(0, box.corner(BoundingBox2d::BottomLeft).x());
167  EXPECT_DOUBLE_EQ(0, box.corner(BoundingBox2d::BottomLeft).y());
168  EXPECT_DOUBLE_EQ(1, box.corner(BoundingBox2d::TopRight).x());
169  EXPECT_DOUBLE_EQ(1, box.corner(BoundingBox2d::TopRight).y());
170 }
171 
172 TEST_F(LaneletTest, intersects) { // NOLINT
173  EXPECT_TRUE(geometry::intersects2d(ritterLanelet, constRitterLanelet));
174  auto lanelet1 = Lanelet(++id, left, other);
175  auto lanelet2 = Lanelet(++id, right, outside);
176  EXPECT_FALSE(geometry::intersects2d(lanelet1, lanelet2));
177  EXPECT_TRUE(geometry::intersects2d(constRitterLanelet, lanelet2));
178  EXPECT_TRUE(geometry::intersects2d(constRitterLanelet, lanelet1));
179 }
180 
181 TEST_F(LaneletTest, overlaps) { // NOLINT
182  EXPECT_TRUE(geometry::overlaps2d(ritterLanelet, constRitterLanelet));
183  auto lanelet1 = Lanelet(++id, left, other);
184  auto lanelet2 = Lanelet(++id, right, outside);
185  EXPECT_FALSE(geometry::overlaps2d(lanelet1, lanelet2));
186  EXPECT_FALSE(geometry::overlaps2d(constRitterLanelet, lanelet2));
187  EXPECT_TRUE(geometry::overlaps2d(constRitterLanelet, lanelet1));
188 }
189 
190 TEST_F(LaneletTest, length) { // NOLINT
191  EXPECT_FLOAT_EQ(geometry::length2d(ritterLanelet), 1);
192 }
193 
194 TEST_F(LaneletTest, approxLength) { // NOLINT
195  auto l = geometry::approximatedLength2d(ritterLanelet);
196  EXPECT_LT(0.5, l);
197  EXPECT_GE(1, l);
198 }
199 
202  auto lanelet1 = Lanelet(++id, left, other);
203  auto lanelet2 = Lanelet(++id, right, outside);
204  EXPECT_TRUE(intersects3d(ritterLanelet, constRitterLanelet));
205  EXPECT_TRUE(intersects3d(ritterLanelet, bufferLanelet(ritterLanelet, 0), 1.));
206  EXPECT_FALSE(intersects3d(ritterLanelet, bufferLanelet(ritterLanelet, 2), 1.));
207  EXPECT_FALSE(intersects3d(ritterLanelet, bufferLanelet(ritterLanelet, -2), 1.));
208  EXPECT_TRUE(intersects3d(bufferLanelet(ritterLanelet, -100), bufferLanelet(ritterLanelet, -101), 3.));
209  EXPECT_FALSE(intersects3d(lanelet1, lanelet2));
210  EXPECT_TRUE(intersects3d(ritterLanelet, lanelet2, 3));
211 }
212 
214  using geometry::overlaps3d;
215  auto lanelet1 = Lanelet(++id, left, other);
216  auto lanelet2 = Lanelet(++id, right, outside);
217  EXPECT_TRUE(overlaps3d(ritterLanelet, constRitterLanelet));
218  EXPECT_TRUE(overlaps3d(ritterLanelet, bufferLanelet(ritterLanelet, 0), 1.));
219  EXPECT_FALSE(overlaps3d(this->ritterLanelet, bufferLanelet(ritterLanelet, 2), 1.));
220  EXPECT_FALSE(overlaps3d(lanelet1, lanelet2));
221  EXPECT_FALSE(overlaps3d(ritterLanelet, lanelet2, 3));
222 }
223 
225  using traits::to2D;
226  EXPECT_DOUBLE_EQ(0., geometry::distance2d(constRitterLanelet, to2D(p1)));
227  EXPECT_DOUBLE_EQ(0., geometry::distance3d(constRitterLanelet, p5));
228  EXPECT_DOUBLE_EQ(0., geometry::distance2d(constRitterLanelet, to2D(p6)));
229  EXPECT_DOUBLE_EQ(1., geometry::distance2d(constRitterLanelet, to2D(p8)));
230  EXPECT_DOUBLE_EQ(0., geometry::distance2d(constRitterLanelet, Point2d(InvalId, 0, 0)));
231  EXPECT_DOUBLE_EQ(1., geometry::distance2d(constRitterLanelet, Point2d(InvalId, -1, 0)));
232  EXPECT_DOUBLE_EQ(std::sqrt(.5), geometry::distanceToCenterline3d(constRitterLanelet, p1));
233  EXPECT_DOUBLE_EQ(0.5, geometry::distanceToCenterline2d(constRitterLanelet, Point2d(InvalId, 0, 0)));
234 }
235 
236 TEST_F(LaneletTest, comparison) { // NOLINT
237  EXPECT_EQ(constRitterLanelet, ritterLanelet);
238 
239  auto invLanelet = ritterLanelet.invert();
240  EXPECT_NE(ritterLanelet, invLanelet);
241  EXPECT_NE(ritterLanelet, Lanelet());
242  EXPECT_NE(Lanelet(), Lanelet());
243 }
244 
245 TEST_F(LaneletTest, weakLanelet) { // NOLINT
246  WeakLanelet wll = ritterLanelet;
247  EXPECT_EQ(wll, ritterLanelet);
248  EXPECT_FALSE(wll.expired());
249  EXPECT_EQ(wll.lock(), ritterLanelet);
250 }
251 
253  Lanelet rightLL(++id, right, outside);
254  auto res = geometry::determineCommonLine(ritterLanelet, rightLL);
255  ASSERT_TRUE(!!res);
256  EXPECT_EQ(*res, right);
257  res = geometry::determineCommonLine(rightLL, ritterLanelet);
258  ASSERT_TRUE(!!res);
259  EXPECT_EQ(*res, right);
260  res = geometry::determineCommonLine(rightLL.invert(), ritterLanelet);
261  ASSERT_FALSE(!!res);
262  res = geometry::determineCommonLine(rightLL, ritterLanelet.invert());
263  ASSERT_FALSE(!!res);
264  res = geometry::determineCommonLine(rightLL.invert(), ritterLanelet, true);
265  ASSERT_TRUE(!!res);
266  EXPECT_EQ(*res, right.invert());
267  res = geometry::determineCommonLine(rightLL, ritterLanelet.invert(), true);
268  ASSERT_TRUE(!!res);
269  EXPECT_EQ(*res, right);
270 }
271 
272 TEST(LaneletBasic, emptyLanelet) { // NOLINT
273  Lanelet empty;
274  EXPECT_EQ(empty.polygon2d().size(), 0ul);
275  EXPECT_EQ(empty.centerline().size(), 0ul);
276 }
277 
279  /*
280  * Shape looks roughly like this:
281  * | |\ /| |
282  * | | \ / | |
283  * | | \/ | |
284  * |___________|
285  *
286  */
287  Id id{1};
288  auto p11 = Point3d(++id, 1., 5.);
289  auto p12 = Point3d(++id, 2., 8.);
290  auto p13 = Point3d(++id, 3., 2.);
291  auto p14 = Point3d(++id, 4., 10.);
292  auto p15 = Point3d(++id, 5., 4.);
293  auto p21 = Point3d(++id, 0., 10.);
294  auto p22 = Point3d(++id, 0., 0.);
295  auto p23 = Point3d(++id, 6., 0.);
296  auto p24 = Point3d(++id, 6., 10.);
297  auto left = LineString3d(++id, Points3d{p11, p12, p13, p14, p15});
298  auto right = LineString3d(++id, Points3d{p21, p22, p23, p24});
299  auto lanelet = Lanelet(++id, left, right);
300  return lanelet;
301 }
302 
303 Lanelet buildLinearTestCase(size_t numPoints) {
304  Id id{1};
305  LineString3d left(++id);
306  LineString3d right(++id);
307  for (auto i = 0u; i < numPoints; i++) {
308  left.push_back(Point3d(++id, i, 1, 0));
309  right.push_back(Point3d(++id, i, 0, 0));
310  }
311  return Lanelet(++id, left, right);
312 }
313 
315  /*
316  * Shape:
317  * \__
318  * \
319  * ----x
320  */
321  Id id{1};
322  Point3d p11(++id, 0, 3);
323  Point3d p12(++id, 1, 1);
324  Point3d p13(++id, 4, 0);
325  Point3d p21(++id, 0, 0);
326  Point3d p22(++id, 1, 0);
327  LineString3d left(++id, {p11, p12, p13});
328  LineString3d right(++id, {p21, p22, p13});
329  return inverted ? Lanelet(++id, right.invert(), left.invert()) : Lanelet(++id, left, right);
330 }
331 
332 TEST(ComplexLaneletTest, complexCenterline) { // NOLINT
333  auto lanelet = buildComplexTestCase();
334  auto centerline = lanelet.centerline();
335  testCenterline(centerline, lanelet.leftBound(), lanelet.rightBound());
336 }
337 
338 TEST(ComplexLaneletTest, linearCenterline) { // NOLINT
339  auto lanelet = buildLinearTestCase(20);
340  auto centerline = lanelet.centerline();
341  testCenterline(centerline, lanelet.leftBound(), lanelet.rightBound());
342 }
343 
344 TEST(ComplexLaneletTest, touchingCenterlineForward) { // NOLINT
345  auto lanelet = buildTouchingTestCase(false);
346  auto centerline = lanelet.centerline();
347  testCenterline(centerline, lanelet.leftBound(), lanelet.rightBound());
348 }
349 
350 TEST(ComplexLaneletTest, touchingCenterlineBackward) { // NOLINT
351  auto lanelet = buildTouchingTestCase(true);
352  auto centerline = lanelet.centerline();
353  testCenterline(centerline, lanelet.leftBound(), lanelet.rightBound());
354 }
Id id() const noexcept
get the unique id of this primitive
Definition: Primitive.h:96
IfLL< Lanelet1T, bool > intersects3d(const Lanelet1T &lanelet, const Lanelet2T &otherLanelet, double heightTolerance)
test whether two lanelets intersect in 2d.
IfAr< Area1T, bool > intersects2d(const Area1T &area, const Area2T &otherArea)
test whether two areas intersect in 2d.
IfAr< AreaT, bool > inside(const AreaT &area, const BasicPoint2d &point)
Checks whether a point is within or at the border of a area.
void testCenterline(const ConstLineString3d &centerline, const ConstLineString3d &leftBound, const ConstLineString3d &rightBound)
A Linestring that returns BasicPoint2d instead of Point2d.
int64_t Id
Definition: Forward.h:198
LineString3d right
double distanceToCenterline3d(const LaneletT &lanelet, const BasicPoint3d &point)
calculates distance in 3d to centerline of a lanelet.
double distanceToCenterline2d(const LaneletT &lanelet, const BasicPoint2d &point)
calculates distance in 2d to the centerline of a lanelet.
The famous (mutable) lanelet class.
TEST_F(LaneletTest, id)
A normal 3d linestring with immutable data.
TEST(LaneletBasic, emptyLanelet)
void SetUp() override
const BasicPointType & front() const noexcept
Get first BasicPoint2d.
double approximatedLength2d(const LaneletT &lanelet)
approximates length by sampling points along left bound
size_t size() const noexcept
Return the number of points in this linestring.
bool expired() const noexcept
tests whether the WeakLanelet is still valid
Lanelet ritterLanelet
quadratisch, praktisch, gut... [1x1]
std::vector< Point3d > Points3d
Definition: Forward.h:34
BasicPoint p2
CompoundPolygon2d polygon2d() const
returns the surface covered by this lanelet as 2-dimensional polygon.
Definition: Lanelet.cpp:316
IfAr< AreaT, bool > overlaps2d(const AreaT &area, const AreaT &otherArea)
A normal 3d linestring with mutable data.
A mutable 3d point.
ConstLineString3d centerline() const
get the (cached) centerline of this lanelet.
IfAr< AreaT, bool > overlaps3d(const AreaT &area, const AreaT &otherArea, double heightTolerance)
SharedPtrs in lanelet2 must never point to null. If this is violated, this exception is thrown (usual...
Definition: Exceptions.h:80
IfAr< AreaT, BoundingBox2d > boundingBox2d(const AreaT &area)
calculates an up-right 2d bounding box
Optional< double > distance
LineStringT invert(const LineStringT &ls)
const BasicPointType & back() const noexcept
Get last BasicPoint2d.
An immutable lanelet.
LineString3d leftBound()
Get the left bound.
Optional< ConstLineString3d > determineCommonLine(const ConstArea &ar, const ConstLanelet &ll)
auto transform(Container &&c, Func f)
Definition: Utilities.h:120
IfLS2< LineString3d1T, LineString3d2T, double > distance3d(const LineString3d1T &l1, const LineString3d2T &l2)
LineString3d rightBound()
Get the right bound.
Lanelet buildLinearTestCase(size_t numPoints)
Eigen::Matrix< double, 2, 1, Eigen::DontAlign > BasicPoint2d
a simple 2d-point
BasicPoint p1
Lanelet buildComplexTestCase()
Lanelet bufferLanelet(Lanelet llt, double z)
Thrown when an attribute has been queried that does not exist.
Definition: Exceptions.h:42
Lanelet invert() const
non-const version to invert a lanelet
void testHasIntersection(const ConstHybridLineString2d &ls, const ConstHybridLineString2d &lsRef)
Lanelet lock() const
Obtains the original Lanelet.
BoundingBox2d to2D(const BoundingBox3d &primitive)
Lanelet buildTouchingTestCase(bool inverted)
std::vector< Point2d > Points2d
Definition: Forward.h:36
size_t size() const noexcept
return the total number of unique points
ConstLanelet constRitterLanelet
void push_back(const PointType &point)
inserts a new element at the end
A mutable 2d point.
double distance2d(const RegulatoryElement &regElem, const BasicPoint2d &p)
constexpr Id InvalId
indicates a primitive that is not part of a map
Definition: Forward.h:199
double length2d(const LaneletT &lanelet)
calculate length of centerline in 2d


lanelet2_core
Author(s): Fabian Poggenhans
autogenerated on Tue Jun 6 2023 02:23:32