apollo参考线缝合代码解析

参考线缝合的基本思路：
首先确定参考线出现的情况为一下两种：

/** Stitch current reference line with the other reference line
   * The stitching strategy is to use current reference points as much as
   * possible. The following two examples show two successful stitch cases.
   *
   * Example 1
   * this:   |--------A-----x-----B------|
   * other:                 |-----C------x--------D-------|
   * Result: |------A-----x-----B------x--------D-------|
   * In the above example, A-B is current reference line, and C-D is the other
   * reference line. If part B and part C matches, we update current reference
   * line to A-B-D.
   *
   * Example 2
   * this:                  |-----A------x--------B-------|
   * other:  |--------C-----x-----D------|
   * Result: |--------C-----x-----A------x--------B-------|
   * In the above example, A-B is current reference line, and C-D is the other
   * reference line. If part A and part D matches, we update current reference
   * line to C-A-B.
   *
   * @return false if these two reference line cannot be stitched
   */
reference_points_为当前参考线（this）
当是第二种情况时，也就是this的第一个点就和other中的点match上了，那么就可以将other的起点存放到（DA）点的前面，也就是this.begin()前。
当是第一种情况时，也就是this的最后一个点和other上的C点match上，那么就将other中的所有点都存放到B-C点之前（这里也就是this的终点之前）

/*通过生成一组参考点，将其中的值拷贝到私有变量reference_points_中
使用reference_points_生成map_path_*/
ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points)
    : reference_points_(reference_points),
      map_path_(std::move(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {
  CHECK_EQ(static_cast<size_t>(map_path_.num_points()),
           reference_points_.size());
}

/*通过地图路径生成参考线，遍历路径中的点，然后取‘lane_waypoints’中的第一个点，保存到参考点的数组中
 ReferencePoint包含有信息(k, dk, x, y, heading, s, l)
 MapPathPoint包含有信息(point(x,y,heading),LaneWaypoints)
 LaneWaypoint包含有信息(ptr lane,s,l)
 
 */
ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {// point: std::vector<MapPathPoint>
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];//lane_waypoint: std::vector<LaneWaypoint>
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0);
  }
  CHECK_EQ(static_cast<size_t>(map_path_.num_points()),
           reference_points_.size());
}
bool ReferenceLine::Stitch(const ReferenceLine& other) {
  if (other.reference_points().empty()) {
    AWARN << "The other reference line is empty.";
    return true;
  }
  //1、找到起点的交点ReferencePoint first_point（k, dk, x, y, heading, s, l）
  auto first_point = reference_points_.front();
  common::SLPoint first_sl;
  if (!other.XYToSL(first_point, &first_sl)) {
    AWARN << "Failed to project the first point to the other reference line.";
    return false;
  }
  bool first_join = first_sl.s() > 0 && first_sl.s() < other.Length();
  //2、找到终点
  auto last_point = reference_points_.back();
  common::SLPoint last_sl;
  if (!other.XYToSL(last_point, &last_sl)) {
    AWARN << "Failed to project the last point to the other reference line.";
    return false;
  }
  bool last_join = last_sl.s() > 0 && last_sl.s() < other.Length();

// 3、如果起点和终点都和other没有交点，则退出
  if (!first_join && !last_join) {
    AERROR << "These reference lines are not connected.";
    return false;
  }

 //计算累计的s值 
  const auto& accumulated_s = other.map_path().accumulated_s();
  //保存参考点
  const auto& other_points = other.reference_points();
  auto lower = accumulated_s.begin();
  /*这里不明白1e-1和0.1有什么区别，猜测有两种可能没有写成0.1：
      1、保证浮点数的精度
      2、代码编写的要求是按照IEEE标准
       constexpr（常量表达式）的作用是将kStitchingError等价于1e-1在后续的代码中使用
  */
  static constexpr double kStitchingError = 1e-1;
  //如果有是第一个点的交点，且第一个交点的的横向偏差不大于0.1则将数据保存到参考线中
  if (first_join) {
    //如果横向偏移大于0.1m时，退出
    if (first_sl.l() > kStitchingError) {
      AERROR << "lateral stitching error on first join of reference line too "
                "big, stitching fails";
      return false;
    }
    // 在accumulated_s数组中找到比第一个点的s大的第一个元素
    lower = std::lower_bound(accumulated_s.begin(), accumulated_s.end(),
                             first_sl.s());
    // start_i 找到lower在accumulated_s中的元素位置
    size_t start_i = std::distance(accumulated_s.begin(), lower);
    // 4.1 因为this的起点在other之后，插入other的起点到this的起点
    reference_points_.insert(reference_points_.begin(), other_points.begin(),
                             other_points.begin() + start_i);
  }
  if (last_join) {
    // 如果last point的横向偏移大于0.1m，则退出
    if (last_sl.l() > kStitchingError) {
      AERROR << "lateral stitching error on first join of reference line too "
                "big, stitching fails";
      return false;
    }
    // 5.2 因为this的终点小于other的终点，把other终点拼接到参考线的终点
    auto upper = std::upper_bound(lower, accumulated_s.end(), last_sl.s());
    auto end_i = std::distance(accumulated_s.begin(), upper);

    reference_points_.insert(reference_points_.end(),
                             other_points.begin() + end_i, other_points.end());
  }
  map_path_ = MapPath(std::move(std::vector<hdmap::MapPathPoint>(
      reference_points_.begin(), reference_points_.end())));
  return true;
}