【c++】Lanelet2 Examples笔记(一)
01_dealing_with_lanelet_primitives
#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Area.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/Area.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/primitives/LineString.h>
#include <lanelet2_core/primitives/Point.h>
#include <lanelet2_core/primitives/Polygon.h>
#include <lanelet2_core/utility/Units.h>
#include "lanelet2_examples/internal/ExampleHelpers.h"
// we want assert statements to work in release mode
#undef NDEBUG
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"
// we compare floats a couple of times and know this is save in this context
#pragma GCC diagnostic ignored "-Wfloat-equal"
void part0Primitives();
void part1Points();
void part2LineStrings();
void part3Polygons();
void part4Lanelets();
void part5Areas();
void part6Geometry();
int main() {
part0Primitives();
part1Points();
part2LineStrings();
part3Polygons();
part4Lanelets();
part5Areas();
part6Geometry();
return 0;
}
void part0Primitives() {
using namespace lanelet;
using namespace lanelet::units::literals; // this enables us to use _kmh for velocities
// 创建一个3D点p(x=0, y=0, z=0),getId()给3D点p一个唯一的id
Point3d p(utils::getId(), 0, 0, 0);
// 点pCopy和点p共享一个数据
Point3d pCopy = p;
assert(p == pCopy);
// 点p的x、y、z值可以重新赋予
p.z() = 2;
// 使用.setId()重新给点p赋予一个唯一的id
p.setId(utils::getId());
// 因为pCopy和p共享一个数据,所以pCopy的值也随p的值变化
assert(p.id() == pCopy.id());
assert(p.z() == pCopy.z());
// 点有key-value形式的属性
p.attributes()["type"] = "point";
p.attributes()["pi"] = 3.14;
p.attributes()["velocity"] = "5 kmh";
assert(p.attributes() == pCopy.attributes());
assert(p.attribute("type") == "point");
// .asDouble()转成double型
assert(!!p.attribute("pi").asDouble());
// .asVelocity()转成速度型
assert(p.attribute("velocity").asVelocity() == 5_kmh);
// .attributeOr(), 如果属性key不存在,或不能转换成.attributeOr()中设置的值的类型,则返回.attributeOr()中设置的值。否则返回对应的value
assert(p.attributeOr("nonexistent", -1) == -1);
assert(p.attributeOr("type", 0) == 0); // "point" can not be converted to a number
assert(p.attributeOr("velocity", 0_kmh) == 5_kmh);
// ConstPoint3d类型的3D点不能直接对id和坐标进行修改
ConstPoint3d pConst = p;
assert(pConst.z() == 2);
// pConst.z() = 3; // 报错
// 但是可通过共享相同数据的Point3d类型数据对id和坐标进行修改
p.z() = 3;
assert(pConst.z() == 3);
ConstPoint3d& pConstRef = p;
assert(pConstRef.z() == 3);
// 因为ConstPoint3d的id和坐标值不能进行修改,所以不能让Point3d类型的点和ConstPoints3d类型的点共享同一数据
// Point3d pNonConst = pConst; // 报错
// we can also access the underlying data (this is the thing that the point copies share). However this is not
// really meant for daily usage:
assert(p.constData() == pConst.constData());
}
void part1Points() {
using namespace lanelet;
// x=1, y=2, z=3.
Point3d p3d(utils::getId(), 1, 2, 3);
assert(p3d.x() == 1);
assert(p3d.y() == 2);
assert(p3d.z() == 3);
// BasicPoint which is actually an Eigen point and can be used in time critical computations:
BasicPoint3d& p3dBasic = p3d.basicPoint();
p3dBasic.z() = 4;
assert(p3d.z() == 4);
// BasicPoint3d类型的点可以进行数学运算
BasicPoint3d pTwice = p3d.basicPoint() * 2;
assert(pTwice.z() == 8);
// 使用to2D将3D点转换为2D点
Point2d p2d = utils::to2D(p3d);
// 2D点和3D点的区别是没有Z坐标,但是实际上to2D转换得到的2D点和3D点仍然共享同一数据
p2d.x() = 3;
assert(p3d.x() == 3);
assert(p3d.constData() == p2d.constData());
// 也可以使用to3D从2D点转换为3D点,且z值不会丢失
Point3d p3dNew = utils::to3D(p2d);
assert(p3dNew.z() == p3d.z());
assert(p3dNew.constData() == p3d.constData());
}
void part2LineStrings() {
using namespace lanelet;
Point3d p1{utils::getId(), 0, 0, 0};
Point3d p2{utils::getId(), 1, 0, 0};
Point3d p3{utils::getId(), 2, 0, 0};
LineString3d ls(utils::getId(), {p1, p2, p3});
// LineString3d类似std::vector
assert(ls[1] == p2);
assert(ls.size() == 3);
for (Point3d& p : ls) {
assert(p.y() == 0);
}
ls.push_back(Point3d(utils::getId(), 3, 0, 0));
assert(ls.size() == 4);
ls.pop_back();
assert(ls.size() == 3);
// LineString3d也有key-value格式的属性,key和value可以从预定义的内容中选择
ls.attributes()[AttributeName::Type] = AttributeValueString::LineThin;
ls.attributes()[AttributeName::Subtype] = AttributeValueString::Dashed;
// Segment3d类可以表示LineString3d中的一段,一个Segment3d包含两个端点
// LineString3d的.segment函数按LineString3d中包含的点的顺序取出Segment3d
assert(ls.numSegments() >= 2);
Segment3d segment = ls.segment(1);
assert(segment.first == p2);
assert(segment.second == p3);
// .invert()将LineString3d倒置
LineString3d lsInv = ls.invert();
//.front()取出LineString3d中的第一个点
assert(lsInv.front() == p3);
//.inverted()判断一个LineString3d是否是倒置的, 是则返回True, 否则返回False
assert(lsInv.inverted());
//倒置的和原始的共享同一数据
assert(lsInv.constData() == ls.constData());
// .invert()可叠加
assert(lsInv != ls);
LineString3d lsInvInv = lsInv.invert();
assert(lsInvInv == ls);
assert(lsInvInv.front() == p1);
// ConstLineString3d不可修改的常数版本
ConstLineString3d lsConst = ls;
// lsConst.pop_back() // 报错
ConstPoint3d p1Const = ls[0]; // const linestrings 返回 const points
assert(lsConst.constData() == ls.constData());
// 2D版本,和点的操作相同
LineString2d ls2d = utils::to2D(ls);
Point2d front2d = ls2d.front();
assert(front2d == utils::to2D(p1));
// ConstHybridLineString3d类,可使用toHybrid转换,其中的点是BasicPoint3d类,用于加速数学运算
ConstHybridLineString3d lsHybrid = utils::toHybrid(ls);
BasicPoint3d p1Basic = lsHybrid[0];
assert(p1Basic.x() == p1.x());
// BasicLineString3d类,用.basicLineString()转换,其中的点也是BasicPoint3d类,用于加速数学运算
BasicLineString3d lsBasic = ls.basicLineString();
assert(lsBasic.front() == lsHybrid.front());
}
void part3Polygons() {
using namespace lanelet;
// Polygon3d和LineString3d类似,最大的区别是Polygon3d的最后一个点和第一个点也连成segment
Point3d p1{utils::getId(), 0, 0, 0};
Point3d p2{utils::getId(), 1, 0, 0};
Point3d p3{utils::getId(), 2, -1, 0};
Polygon3d poly(utils::getId(), {p1, p2, p3});
assert(poly.size() == 3);
assert(poly.numSegments() == 3);
Segment3d lastSegment = poly.segment(2);
assert(lastSegment.first == p3);
assert(lastSegment.second == p1);
// Polygon3d类的数据不能倒置,Polygon3d类总是按顺时针方向
// poly.invert(); // no.
ConstPolygon3d polyConst = poly;
ConstHybridPolygon3d polyHybrid = utils::toHybrid(poly);
ConstPolygon2d poly2dConst = utils::to2D(polyConst);
assert(polyHybrid.constData() == poly2dConst.constData());
}
void part4Lanelets() {
using namespace lanelet;
// lanelet使用两个LineString3d分别作为车道的左边界和右边界
LineString3d left = examples::getLineStringAtY(1);
LineString3d right = examples::getLineStringAtY(0);
Lanelet lanelet(utils::getId(), left, right);
assert(lanelet.leftBound() == left);
assert(lanelet.rightBound() == right);
// 可以通过.setLeftBound()改变lanelet车道的边界
lanelet.setLeftBound(left);
// 通过.centerline()返回lanelet的中心线
ConstLineString3d centerline = lanelet.centerline();
// the centerline is cached, because computation is not so cheap. When we set a new boundary, the cache is cleared
ConstLineString3d centerline2 = lanelet.centerline(); // from the cache
assert(centerline2 == centerline);
lanelet.setLeftBound(examples::getLineStringAtY(2));
ConstLineString3d centerline3 = lanelet.centerline();
assert(centerline3 != centerline); // new centerline
// 如果不是通过.setLeftBound()改变车道边界,而是直接改原始的车道边界,需要手动resetCache(),centerline才会发生变化
right.push_back(Point3d(utils::getId(), 4, 0, 0));
assert(centerline3 == lanelet.centerline()); // centerline is still the same, which is wrong.
lanelet.resetCache();
assert(centerline3 != lanelet.centerline()); // new centerline is computed
right.pop_back();
lanelet.resetCache();
// Lanelet可以通过.invert()倒置,倒置后左右边界兑换,且左右边界作为LineString3d也被倒置
Lanelet laneletInv = lanelet.invert();
assert(laneletInv.leftBound().front() == lanelet.rightBound().back());
assert(laneletInv.constData() == lanelet.constData());
// .polygon3d()将Lanelet类转成CompoundPolygon3d类, CompoundPolygon3d类和Polygon3d类似,以左车道的第一个点起始,以右车道的第一个点结束,点总是顺时针排列
CompoundPolygon3d polygon = lanelet.polygon3d();
assert(polygon.size() == 6); // both boundaries have 3 points
assert(polygon[0] == lanelet.leftBound().front()); // the polygon starts at the first point of the left bound
assert(polygon.back() == lanelet.rightBound().front()); // and ends at the start of the right bound
// Lanelet类没有2D或者3D一说,Lanelet类是没有量纲的
// there is also a const version, but no 3d or 2d version (lanelets are "dimensionless").
ConstLanelet laneletConst = lanelet;
assert(laneletConst.constData() == lanelet.constData());
ConstLineString3d leftConst = lanelet.leftBound(); // the bounds are now const as well
// laneletConst.setLeftBound(left); // no
// lanelets可以包含regulatory elements
assert(lanelet.regulatoryElements().empty());
}
void part5Areas() {
using namespace lanelet;
// Area和Lanelet类似,但是Area是包含上右下左的闭合区域,按顺时针
LineString3d top = examples::getLineStringAtY(2);
LineString3d right = examples::getLineStringAtX(2).invert();
LineString3d bottom = examples::getLineStringAtY(0).invert();
LineString3d left = examples::getLineStringAtY(0);
Area area(utils::getId(), {top, right, bottom, left});
// .outerBound()得到Area的外边界
LineStrings3d outer = area.outerBound();
// .setOuterBound()设置Area的外边界
area.setOuterBound(outer);
// .outerBoundPolygon()将Area类转换成CompoundPolygon3d类
CompoundPolygon3d outerPolygon = area.outerBoundPolygon();
// Area类可以有内边界,但是内边界按逆时针排序
assert(area.innerBounds().empty());
// areas can be const
ConstArea areaConst = area;
ConstLineStrings3d outerConst = areaConst.outerBound(); // now the outer bound linestrings are also const
// area可以包含regulatory elements
assert(area.regulatoryElements().empty());
}
void part6Geometry() {
using namespace lanelet;
// lanelet2 allows lots of geometry calculations with all kinds of primitives. This tutorial only shows some of them
// to show you the basic idea. There are many more algorithms that you can find in the geometry folder in
// lanelet2_core.
// the primitives we are going to work with
Point3d point(utils::getId(), 1, 4, 1);
LineString3d ls = examples::getLineStringAtY(2); // linestring that goes from (0,2,0) to (2,2,0)
Polygon3d poly = examples::getAPolygon(); // polygon with points (0,0,0), (2,0,0), (2, -2, 0)
Lanelet lanelet = examples::getALanelet(); // lanelet that goes from x=0 to x=3 and extends from y=2 to y=0
Area area = examples::getAnArea(); // quadratic area with the edge points (0,0,0) and (2,2,0)
// 数学运算最好使用Hybrid类型
ConstHybridLineString3d lsHybrid = utils::toHybrid(ls);
ConstHybridPolygon3d polyHybrid = utils::toHybrid(poly);
// the points, linestrings and polygons directly interface with boost.geometry:
auto dP2Line3d = geometry::distance(point, lsHybrid);
// 3d点的结果也是3d的
assert(dP2Line3d > 2);
// 2d点的结果也是2d的
auto dP2Line2d = geometry::distance(utils::to2D(point), utils::to2D(lsHybrid));
assert(dP2Line2d == 2);
// other algorithms would be the length or the area
auto l3d = geometry::length(lsHybrid);
assert(l3d == 2);
auto ar = geometry::area(utils::to2D(polyHybrid)); // not defined in 3d
assert(ar == 2);
// lanelet2 provides more algorithms where boost.geometry lacks inplementations. For those implementations you don't
// have to use the hybrid type:
BasicPoint3d pProj = geometry::project(ls, point); // 得到点在线上的投影点
assert(pProj.y() == 2);
ArcCoordinates arcCoordinates =
geometry::toArcCoordinates(utils::to2D(ls), utils::to2D(point)); // transforms the point into arc coordinates
assert(arcCoordinates.distance == 2); // signed distance to linestring
assert(arcCoordinates.length == 1); // length along linestring
// bounding boxes can be calculated for all types:
BoundingBox3d pointBox = geometry::boundingBox3d(point); // trivial box for points
BoundingBox3d lsBox = geometry::boundingBox3d(ls);
BoundingBox2d laneletBox = geometry::boundingBox2d(lanelet);
BoundingBox2d areaBox = geometry::boundingBox2d(area);
// we can now use these boxes for efficient intersection estimation. Intersects returns true also if the boxes only
// touch but the shared area is still 0.
// 判断边框是否有交叉,如果有接触但是没有重叠也返回True
assert(geometry::intersects(laneletBox, areaBox));
assert(!geometry::intersects(pointBox, lsBox));
// if you want more than an estimation you can use the primitives directly. Overlaps only returns true if the shared
// area of the two primitives is >0. In 3d, the distance in z must also be smaller than a margin.
assert(geometry::overlaps3d(area, lanelet, 3));
}
结语
如果您有修改意见或问题,欢迎留言或者通过邮箱和我联系。
手打很辛苦,如果我的文章对您有帮助,转载请注明出处。