系列文章目录
一、lanelet2简介
二、lanelet2地图制作
三、官方示例阅读(上篇 01~03)
目录
系列文章目录
前言
一、01_dealing_with_lanelet_primitives
1.part0Primitives();
2.part1Points();
3.part2LineStrings();
4.part3Polygons();
5.part4Lanelets();
6.part5Areas();
7.part6Geometry();
二、02_regulatory_elements
1.part1BasicRegulatoryElements();
2.part2HandlingRegulatoryElements();
3.part3AddingNewRegulatoryElements();
三、03_lanelet_map
1.part1AboutLaneletMaps();
2.part2CreatingLaneletMaps();
3.part3QueryingInformation();
4.part4LaneletSubmaps();
总结
?
前言
lanelet2官方给了6个示例程序,本文逐一分析。
本文将源码assert()调试命令,统一改为if(),作用相似,均是判断某变量是否按照我们预想的情况执行。
一、01_dealing_with_lanelet_primitives
1.part0Primitives();
Point3d p(utils::getId(), 0, 0, 0);
创建一个Point3d点,调用构造函数point3d(id, x, y, z),utils::getId()为该元素产生唯一的id序号,x, y, z分别是该点的三维直角坐标系坐标。
Point3d pCopy = p;
创建一个Point3d点,调用赋值函数,pCopy相当于p的引用。
p.z() = 2;
p.setId(utils::getId());
p是普通变量,可以更改p的值。
p.attributes()["type"] = "point";
p.attributes()["pi"] = 3.14;
p.attributes()["velocity"] = "5 kmh";
//另一种调用方式
if(p.attribute("type") == "point");
//p.attribute("pi").asDouble();
//p.attribute("velocity").asVelocity() == 5_kmh
p.attributes()得到一张hashmap,hashmap使用键-值对保存该点属性,使用hashmap.["键"]得到对应的值。
//传入的第一个参数是要确认的属性,第二个参数为如果不存在则返回的默认值
p.attributeOr("nonexistent", -1) == -1
p.attributeOr("velocity", 0_kmh) == 5_kmh
当不确定某个属性是否存在时,可以使用“attributeOr”。当属性不存在或无法转换为作为默认值传递的类型时,将返回设定的默认值。
ConstPoint3d pConst = p;
常量Point3d,只能读取数值不能修改。
2.part1Points();
Point3d p3d(utils::getId(), 1, 2, 3);
BasicPoint3d& p3dBasic = p3d.basicPoint();
p3dBasic.z() = 4;
BasicPoint3d pTwice = p3d.basicPoint() * 2;
BasicPoint3d = Eigen::Vector3d,把Point3d点转换为Eigen::Vector3d格式,使其能参与+/-/*//等常规运算,并且p3dBasic为p3d的引用,对其的操作不仅改变自己的值,还改变原始数据的值。
//3d点转2d点
Point2d p2d = utils::to2D(p3d);
p2d.x() = 3;
//2d点转3d点
Point3d p3dNew = utils::to3D(p2d);
p3dNew.z() == p3d.z();
3d点与2d点相互转换,3d转2d相当于把z值隐藏,并且属于值引用,修改2d点同样会修改3d点。
当把2d点转3d时,若存在z值,则数据保留,否则补0,同样属于值引用。
3.part2LineStrings();
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用法
if(ls[1] == p2);
if(ls.size() == 3);
for (Point3d& p : ls) {
if(p.y() == 0);
}
ls.push_back(Point3d(utils::getId(), 3, 0, 0));
ls.pop_back();
由若干Point3d点创建LineString3d,linestrings与std::vector用法类似
ls.attributes()[AttributeName::Type] = AttributeValueString::LineThin;
ls.attributes()[AttributeName::Subtype] = AttributeValueString::Dashed;
LineString3d的属性与osm文件下way的Type和SubType对应。
if(ls.numSegments() >= 2)
Segment3d segment = ls.segment(1);
if(segment.first == p2);
if(segment.second == p3);
ls.numSegments()返回该LineString3d形成的线段数量,为其点数量-1
在LineString3d点集上取相邻两点形成线段,传入参数为起点,终点为该点的下一点。
线段起点segment.first,线段终点segment.second
LineString3d lsInv = ls.invert();
assert(lsInv.front() == p3);
反转LineString3d的顺序并返回,返回值与原值共享数据。
ConstLineString3d lsConst = ls;
ConstPoint3d p1Const = ls[0];
//lsConst.pop_back(); 非法
LineString3d常量形式,只支持相应的读操作,不支持所有可能改变原值的操作。
LineString2d ls2d = utils::to2D(ls);
Point2d front2d = ls2d.front();
3d数据转2d数据,和Point3d与Point2d相似。
ConstHybridLineString3d lsHybrid = utils::toHybrid(ls);
BasicPoint3d p1Basic = lsHybrid[0];
toHybrid(ConstLineString3d ls)返回BasicPoints点集,适合进行运算。
BasicLineString3d lsBasic = ls.basicLineString();
BasicLineString3d = BasicPoints3d,同样将ConstLineString3d ls转换为一系列点。
4.part3Polygons();
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});
//闭合多边形比线段多一条边,numSegments()返回值与点数量相同
//其他与linestrings相同
if(poly.size() == 3);
if(poly.numSegments() == 3);
Segment3d lastSegment = poly.segment(2);
if(lastSegment.first == p3);
if(lastSegment.second == p1);
//不能反转多边形方向,其方向始终为顺时针
//poly.invert(); 非法
多边形(polygons)和线段(linestrings)类似,多了一条最后一点与第一点的连线,形成一个闭合图形。
ConstPolygon3d polyConst = poly;
ConstHybridPolygon3d polyHybrid = utils::toHybrid(poly);
ConstPolygon2d poly2dConst = utils::to2D(polyConst);
const, 2d, 3d, hybrid特性与linestrings相同。
5.part4Lanelets();
//生成道路左右边线
LineString3d left = examples::getLineStringAtY(1);
LineString3d right = examples::getLineStringAtY(0);
//创建路段lanelet
Lanelet lanelet(utils::getId(), left, right);
//可以改变左右边线
lanelet.setLeftBound(left);
Lanelet类的创建需要指明左右边线,左右边线类型均是LineString3d,
ConstLineString3d centerline = lanelet.centerline();
ConstLineString3d centerline2 = lanelet.centerline();
//centerline2 与 centerline相同
if(centerline2 == centerline);
lanelet.setLeftBound(examples::getLineStringAtY(2));
ConstLineString3d centerline3 = lanelet.centerline();
if(centerline3 != centerline);
根据左右边界线产生道路中心线,返回值为常量ConstLineString3d,保存在类缓存中。
right.push_back(Point3d(utils::getId(), 4, 0, 0));
//centerline不能自动更新
if(centerline3 == lanelet.centerline());
//手动更新centerline
lanelet.resetCache();
if(centerline3 != lanelet.centerline());
right.pop_back();
lanelet.resetCache();
当改变左右边线数据时,centerline不会自动更新,必须手动更新。
Lanelet laneletInv = lanelet.invert();
if(laneletInv.leftBound().front() == lanelet.rightBound().back());
lanelet可以反转,反转后,左右边线对换,左右边线的点反序排列,数据保存在原lanelet中。
CompoundPolygon3d polygon = lanelet.polygon3d();
//这里左右边线各有3个点
if(polygon.size() == 6);
//多变线起点为左边线第一点
if(polygon[0] == lanelet.leftBound().front());
//多边形终点为右边线最后一点
if(polygon.back() == lanelet.rightBound().front());
由lanelet产生多边形,顺时针连接左右边线各点。
ConstLanelet laneletConst = lanelet;
ConstLineString3d leftConst = lanelet.leftBound();
// laneletConst.setLeftBound(left); // no
lanelet常量,lanelet不存在维度变换。
6.part5Areas();
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});
Area类创建需要4个LineString3d类,分别表示区域的上、下、左、右。
//从Area得到上下左右边界
LineStrings3d outer = area.outerBound();
//设置Area边界,需要形成完整闭合区域的LineStrings3d
area.setOuterBound(outer);
//从Area得到多边形CompoundPolygon3d
CompoundPolygon3d outerPolygon = area.outerBoundPolygon();
//Area相关的常量
ConstArea areaConst = area;
ConstLineStrings3d outerConst = areaConst.outerBound();
Area常用的相关操作如上。
7.part6Geometry();
道路法规部分,略。
二、02_regulatory_elements
1.part1BasicRegulatoryElements();
LineString3d trafficLight = examples::getLineStringAtY(1);
trafficLight.attributes()[AttributeName::Type] = AttributeValueString::TrafficLight;
//创建红绿灯元素指针
RegulatoryElementPtr trafficLightRegelem = lanelet::TrafficLight::make(utils::getId(), {}, {trafficLight});
红绿灯元素基本构件是linestring类的一条线,另外还可能有一条停止线与之对应。
Lanelet lanelet = examples::getALanelet();
lanelet.addRegulatoryElement(trafficLightRegelem);
RegulatoryElementPtr regelem = lanelet.regulatoryElements()[0];
将上一步创建的红绿灯“固定”到道路(lanelet)上。从道路lanelet信息中读取红绿灯。
if(lanelet.regulatoryElementsAs<SpeedLimit>().empty());
std::vector<TrafficLight::Ptr> trafficLightRegelems = lanelet.regulatoryElementsAs<TrafficLight>();
if(trafficLightRegelems.size() == 1);
TrafficLight::Ptr tlRegelem = trafficLightRegelems.front();
if(tlRegelem->constData() == trafficLightRegelem->constData());
确认道路(lanelet)是否有限速信息。
读取道路红绿灯信息,返回红绿灯元素指针数组。
从红绿灯指针数组取出第一个红绿灯。
LineStringOrPolygon3d theLight = tlRegelem->trafficLights().front();
//添加停止线
tlRegelem->setStopLine(examples::getLineStringAtY(2));
if(!!tlRegelem->stopLine());
红绿灯既可能是多边形类型(polygon),也可能是线类型(linestring),因此这里选用一个(LineStringOrPolygon3d)可以同时兼容这两种类型的数据结构。
对红绿灯元素可以修改其停止线信息,并且可以判断一个红绿灯是否具有停止线。
2.part2HandlingRegulatoryElements();
GenericRegulatoryElement regelem(utils::getId());
Lanelet lanelet = examples::getALanelet();
regelem.addParameter(RoleName::Refers, lanelet);
Point3d point(utils::getId(), 0, 0, 0);
regelem.addParameter(RoleName::Refers, point);
Points3d pts = regelem.getParameters<Point3d>(RoleName::Refers);
if(!pts.empty() && pts.front() == point);
GenericRegulatoryElement类是一种可以容纳任何交通信号标志的类,结构复杂,不建议直接使用。
3.part3AddingNewRegulatoryElements();
LineString3d fromWhere = examples::getLineStringAtX(1);
RuleParameterMap rules{{RoleNameString::RefLine, {fromWhere}}};
RegulatoryElementPtr regelem = RegulatoryElementFactory::create("lights_on", utils::getId(), rules);
Lanelet lanelet = examples::getALanelet();
lanelet.addRegulatoryElement(regelem);
if(!lanelet.regulatoryElementsAs<example::LightsOn>().empty());
在道路lanelet上添加新的交通信号标志。RegulatoryElementFactory::create()函数创建交通信号类。
三、03_lanelet_map
1.part1AboutLaneletMaps();
LaneletMap map = examples::getALaneletMap();
PointLayer& points = map.pointLayer;
LineStringLayer& linestrings = map.lineStringLayer;
创建laneletmap,从地图中获得点元素和线元素。
if(points.size() > 1);
Point3d aPoint = *points.begin();
Point3d samePoint = *points.find(aPoint.id());
if(samePoint == aPoint);
if(points.exists(aPoint.id()));
if(!linestrings.empty());
从map的每一层layer取出的数据类似哈系表,可以通过id获得原始数据。
LaneletMap newMap = std::move(map);
// map.exists(aPoint.id()); 非法
if(newMap.pointLayer.exists(aPoint.id()));
//通过指针移动地图
LaneletMapUPtr mapPtr = std::make_unique<LaneletMap>(std::move(newMap));
map不可复制,可以移动,但移动后数据失效。通过指针移动会得到原始数据的指针,可以访问原始数据。
const LaneletMap& constMap = *mapPtr;
ConstPoint3d aConstPoint = *constMap.pointLayer.begin();
if(aConstPoint == aPoint);
地图的常量变量。
2.part2CreatingLaneletMaps();
LaneletMap laneletMap;
Lanelet lanelet = examples::getALanelet();
laneletMap.add(lanelet);
if(laneletMap.laneletLayer.size() == 1);
if(!laneletMap.pointLayer.empty());
if(laneletMap.pointLayer.exists(lanelet.leftBound().front().id()));
一种构建地图的方式,向地图map逐个添加元素(points, linestrings, regulatory elements),向map添加的元素的子元素同样属于map。
Point3d invalPoint1(InvalId, 0, 0, 0);
Point3d invalPoint2(InvalId, 1, 0, 0);
LineString3d invalLs(InvalId, {invalPoint1, invalPoint2});
laneletMap.add(invalLs);
if(invalPoint1.id() != InvalId);
两点构成一条线,再把该线添加到map。InvalId=0,用于指明该元素目前不属于map,向map添加后,由lanelet自动为其分配全新的id。
Lanelets lanelets{examples::getALanelet(), examples::getALanelet(), examples::getALanelet()};
LaneletMapUPtr laneletsMap = utils::createMap(lanelets);
if(laneletsMap->laneletLayer.exists(lanelets.front().id()));
另一种构建地图的方式,使用utils::createMap()函数,批量向map中添加元素。
LaneletMap laneletMap = examples::getALaneletMap();
Lanelet mapLanelet = *laneletMap.laneletLayer.begin();
TrafficLight::Ptr trafficLight = mapLanelet.regulatoryElementsAs<TrafficLight>().front();
从map自顶向下查询子元素,子元素类型有:laneletLayer、areaLayer、polygonLayer、regulatoryElementLayer、lineStringLayer、pointLayer
auto laneletsOwningLinestring = laneletMap.laneletLayer.findUsages(mapLanelet.leftBound());
if(laneletsOwningLinestring.size() == 1 && laneletsOwningLinestring.front() == mapLanelet);
根据元素拥有的子元素查询。
auto regelemsOwningLs =
laneletMap.regulatoryElementLayer.findUsages(*trafficLight->trafficLights().front().lineString());
if(regelemsOwningLs.size() == 1 && regelemsOwningLs.front() == trafficLight);
从linestrings查询交通信号。
auto laneletsOwningRegelems = laneletMap.laneletLayer.findUsages(trafficLight);
if(!laneletsOwningRegelems.empty());
从lanelet查询交通信号。
Lanelets lanelets = laneletMap.laneletLayer.nearest(BasicPoint2d(0, 0), 1);//1表示最靠近的1条lanelet
if(!lanelets.empty());
通过laneletMap.laneletLayer.search()函数查询最靠近(0, 0)点的1条lanelet。
std::vector<std::pair<double, Lanelet>> actuallyNearestLanelets =
geometry::findNearest(laneletMap.laneletLayer, BasicPoint2d(0, 0), 1);
if(!actuallyNearestLanelets.empty());
通过utility函数查询最靠近(0, 0)点的1条lanelet。
Lanelets inRegion = laneletMap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(10, 10)));
if(!inRegion.empty());
通过bounding boxes查询与该BoundingBox2d有交集的所有lanelet,返回这些lanelets。
BasicPoint2d searchPoint = BasicPoint2d(10, 10);
auto searchFunc = [&searchPoint](const BoundingBox2d& lltBox, const Lanelet& /*llt*/) {
return geometry::distance(searchPoint, lltBox) > 3;
};
Optional<Lanelet> lanelet = laneletMap.laneletLayer.nearestUntil(searchPoint, searchFunc);
if(!!lanelet && geometry::distance(geometry::boundingBox2d(*lanelet), searchPoint) > 3);
查询距离目标点>3米的第一个lanelet。
4.part4LaneletSubmaps();
//LaneletSubmap
LaneletSubmap submap{examples::getALaneletMap()};
Lanelets inRegion = submap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(10, 10)));
LaneletSubmapUPtr newSubmap = utils::createSubmap(inRegion);
if(newSubmap->pointLayer.empty());
if(newSubmap->size() == inRegion.size());
//LaneletSubmap类返回LaneletMap类,原始数据得到还原
LaneletMapUPtr newMap = newSubmap->laneletMap();
if(!newMap->pointLayer.empty());
LaneletMap与LaneletSubmap区别,LaneletSubmap类似于Laneletmap使用了move函数得到的结果,原始数据不能通过LaneletSubmap直接获得。
总结
未完,待续...
欢迎交流,联系Q1456055290
|