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));
}