1 #include <gtest/gtest.h> 15 auto bufferPoints = [&llt, z](
const auto& elem) {
16 return Point3d(llt.
id() + elem.id(), elem.x(), elem.y(), elem.z() + z);
25 boost::geometry::intersection(ls, lsRef, intersectionPts);
26 for (
auto& pt : intersectionPts) {
33 EXPECT_GE(centerline.
size(), 2ul);
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.);
58 ritterLanelet =
Lanelet(++
id, left, right);
59 constRitterLanelet = ritterLanelet;
71 ritterLanelet.setId(100);
72 EXPECT_EQ(100, ritterLanelet.id());
73 EXPECT_EQ(100, constRitterLanelet.id());
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());
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);
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());
110 auto constInvertedLanelet = constRitterLanelet.invert();
111 EXPECT_EQ(right.id(), constInvertedLanelet.leftBound().id());
112 EXPECT_EQ(left.id(), constInvertedLanelet.rightBound().id());
114 auto constInvertedInvertedLanelet = constInvertedLanelet.invert();
115 EXPECT_EQ(constRitterLanelet, constInvertedInvertedLanelet);
116 EXPECT_EQ(right, constInvertedInvertedLanelet.rightBound());
117 EXPECT_EQ(left, constInvertedInvertedLanelet.leftBound());
121 auto invertedLanelet = ritterLanelet.invert();
122 invertedLanelet.setLeftBound(right);
123 invertedLanelet.setRightBound(left);
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());
132 auto centerline = ritterLanelet.centerline();
133 testCenterline(centerline, ritterLanelet.leftBound(), ritterLanelet.rightBound());
134 EXPECT_DOUBLE_EQ(
double(geometry::length(centerline)), 1);
136 auto invCenterline = ritterLanelet.invert().centerline();
137 EXPECT_EQ(invCenterline.front().x(), centerline.back().x());
138 EXPECT_EQ(invCenterline.front().y(), centerline.back().y());
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());
150 EXPECT_DOUBLE_EQ(1, geometry::area(ritterLanelet.polygon2d()));
151 EXPECT_DOUBLE_EQ(1, geometry::area(constRitterLanelet.polygon2d()));
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());
174 auto lanelet1 =
Lanelet(++
id, left, other);
175 auto lanelet2 =
Lanelet(++
id, right, outside);
183 auto lanelet1 =
Lanelet(++
id, left, other);
184 auto lanelet2 =
Lanelet(++
id, right, outside);
202 auto lanelet1 =
Lanelet(++
id, left, other);
203 auto lanelet2 =
Lanelet(++
id, right, outside);
204 EXPECT_TRUE(
intersects3d(ritterLanelet, constRitterLanelet));
215 auto lanelet1 =
Lanelet(++
id, left, other);
216 auto lanelet2 =
Lanelet(++
id, right, outside);
217 EXPECT_TRUE(
overlaps3d(ritterLanelet, constRitterLanelet));
221 EXPECT_FALSE(
overlaps3d(ritterLanelet, lanelet2, 3));
237 EXPECT_EQ(constRitterLanelet, ritterLanelet);
239 auto invLanelet = ritterLanelet.invert();
240 EXPECT_NE(ritterLanelet, invLanelet);
241 EXPECT_NE(ritterLanelet,
Lanelet());
247 EXPECT_EQ(wll, ritterLanelet);
249 EXPECT_EQ(wll.
lock(), ritterLanelet);
253 Lanelet rightLL(++
id, right, outside);
256 EXPECT_EQ(*res, right);
259 EXPECT_EQ(*res, right);
266 EXPECT_EQ(*res, right.invert());
269 EXPECT_EQ(*res, right);
272 TEST(LaneletBasic, emptyLanelet) {
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.);
307 for (
auto i = 0u; i < numPoints; i++) {
311 return Lanelet(++
id, left, right);
329 return inverted ?
Lanelet(++
id, right.invert(), left.invert()) :
Lanelet(++
id, left, right);
332 TEST(ComplexLaneletTest, complexCenterline) {
334 auto centerline =
lanelet.centerline();
338 TEST(ComplexLaneletTest, linearCenterline) {
340 auto centerline =
lanelet.centerline();
344 TEST(ComplexLaneletTest, touchingCenterlineForward) {
346 auto centerline =
lanelet.centerline();
350 TEST(ComplexLaneletTest, touchingCenterlineBackward) {
352 auto centerline =
lanelet.centerline();
Id id() const noexcept
get the unique id of this primitive
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 ¢erline, const ConstLineString3d &leftBound, const ConstLineString3d &rightBound)
A Linestring that returns BasicPoint2d instead of Point2d.
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.
A normal 3d linestring with immutable data.
TEST(LaneletBasic, emptyLanelet)
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
CompoundPolygon2d polygon2d() const
returns the surface covered by this lanelet as 2-dimensional polygon.
IfAr< AreaT, bool > overlaps2d(const AreaT &area, const AreaT &otherArea)
A normal 3d linestring with mutable data.
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...
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.
LineString3d leftBound()
Get the left bound.
Optional< ConstLineString3d > determineCommonLine(const ConstArea &ar, const ConstLanelet &ll)
auto transform(Container &&c, Func f)
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
Lanelet buildComplexTestCase()
Lanelet bufferLanelet(Lanelet llt, double z)
Thrown when an attribute has been queried that does not exist.
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
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
double distance2d(const RegulatoryElement ®Elem, const BasicPoint2d &p)
constexpr Id InvalId
indicates a primitive that is not part of a map
double length2d(const LaneletT &lanelet)
calculate length of centerline in 2d