HD-Map lanelet2(3)

系列文章目录

一、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中添加元素。

3.part3QueryingInformation();

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