百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\speed\STBoundary类代码解析

目录

概述

1.st_boundary.h

2.st_boundary.cc

3. st_boundary_test.c

概述

对st_boundary.cc和.h进行解析，以及通过st_boundary_test.cc测试用例来了解STBoundary类的使用。

STBoundary的数据结构就是ST点对集，对应每个时间t根据动态障碍物及结构道路信息，纵向距离限制在，smin,smax间

ST点对集的数据结构如下：

{((smin0,t0),(smax0,t0))，

((smin1,t1),(smax1,t1))，

...}

t0时刻下边界点(smin,t0)，t1时刻上边界点(smax0,t0)，

STPoint ST点的数据结构 (s,t)

STBoundary类其实就是起到存储当前时间附近一段纵向位置上下边界点集的作用，以及对这个上下边界进行的一些查询，删除冗余点，插值等操作。

手写笔记列出STBoundary的成员总览如下：

1.st_boundary.h

#pragma once

#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "gtest/gtest_prod.h"
#include "modules/common/math/box2d.h"
#include "modules/common/math/polygon2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/proto/planning.pb.h"

namespace apollo {
namespace planning {

//继承Polygon2d类，Polygon2d类定义参见modules\common\math\polygon2d.h
//Polygon2d类就是Vec2d类型的vector，简单说就是一系列的(s,t)点对
//这个vector每个元素包含两个ST点
class STBoundary : public common::math::Polygon2d {
 public:
  /** 构造函数:
   *   STBoundary类必须被用一系列的(t,s)点对vector来初始化
   *   每一个时间t对应着(lower_s, upper_s)，上界和下界
   */
  STBoundary() = default; //默认构造函数
  //STBoundary类的带参构造函数
  //输入的第一个参数是一个vector，这个vector point_pairs每个元素包含两个ST点
  //这两个ST点对应着每个时刻的s的上下界
  //输入的第二个参数是个布尔量，is_accurate_boundary 是否为准确的边界？
  explicit STBoundary(
      const std::vector<std::pair<STPoint, STPoint>>& point_pairs,
      bool is_accurate_boundary = false);
  //对于派生类STBoundary禁用基类Polygon2d的带参(参数为Box2d类对象)构造函数
  explicit STBoundary(const common::math::Box2d& box) = delete;
  //对于派生类STBoundary禁用基类Polygon2d的带参(参数为Vec2d类vector对象)构造函数
  explicit STBoundary(std::vector<common::math::Vec2d> points) = delete;

  /** 旧版的构造函数
   */
  //输入参数就是ST图的下界STPoint类对象点vector，上界STPoint类对象点vector
  static STBoundary CreateInstance(const std::vector<STPoint>& lower_points,
                                   const std::vector<STPoint>& upper_points);

  /** 新版构造函数.它没有使用RemoveRedundantPoints移除冗余点函数
   *  并且产生一个精确的ST边界。
   */
  //输入参数就是ST图的下界STPoint类对象点vector，上界STPoint类对象点vector
  static STBoundary CreateInstanceAccurate(
      const std::vector<STPoint>& lower_points,
      const std::vector<STPoint>& upper_points);

  /** 默认的析构函数
   */
  ~STBoundary() = default;

  //判断是否为空,直接判断下界STPoint类对象点vector lower_points_是否为空
  bool IsEmpty() const { return lower_points_.empty(); }

  //获取未阻塞的s范围，代入当前时间，此刻s上界，此刻s下界？
  bool GetUnblockSRange(const double curr_time, double* s_upper,
                        double* s_lower) const;

  //获取边界的s范围，代入当前时间，此刻s上界，此刻s下界？
  bool GetBoundarySRange(const double curr_time, double* s_upper,
                         double* s_lower) const;

  //获取边界的变化率，代入当前时间，此刻s上界变化，此刻s下界变化？
  bool GetBoundarySlopes(const double curr_time, double* ds_upper,
                         double* ds_lower) const;

  // 如果你要增加边界类型，请同时修改GetUnblockSRange函数
  // GetUnblockSRange accordingly.
  //枚举的边界类型变量BoundaryType 
  //UNKNOWN未知，STOP停车，FOLLOW跟车，YIELD避让，OVERTAKE超车
  //KEEP_CLEAR不能占用的路肩
  enum class BoundaryType {
    UNKNOWN,
    STOP,
    FOLLOW,
    YIELD,
    OVERTAKE,
    KEEP_CLEAR,
  };

  //返回边界类型的名称字符串
  static std::string TypeName(BoundaryType type);

  //返回边界类型函数
  BoundaryType boundary_type() const;
  //获取边界id函数
  const std::string& id() const;
  //返回数据成员characteristic_length_特征长度
  double characteristic_length() const;

  //设置数据成员边界id_
  void set_id(const std::string& id);
  //设置数据成员边界类型boundary_type_
  void SetBoundaryType(const BoundaryType& boundary_type);
  //设置数据成员特征长度characteristic_length_
  void SetCharacteristicLength(const double characteristic_length);

  //返回ST边界中最小/大的s或t
  double min_s() const;
  double min_t() const;
  double max_s() const;
  double max_t() const;

  //获取ST边界的上界upper_points_
  std::vector<STPoint> upper_points() const { return upper_points_; }
  //获取ST边界的下界lower_points_
  std::vector<STPoint> lower_points() const { return lower_points_; }

  //被st-optimizer(st优化器)使用？Apollo原注释
  bool IsPointInBoundary(const STPoint& st_point) const; //判断ST点是否在边界范围内？
  //现有的ST边界上下边界往外扩给定的s
  STBoundary ExpandByS(const double s) const;
  //现有的ST边界上下边界时间轴上往前后扩给定的t
  STBoundary ExpandByT(const double t) const;

  // Unused function so far.目前为止还未使用？Apollo原注释
  // 以给定的时间t未界截断原有的ST边界，只保留里面时间大于t的部分
  STBoundary CutOffByT(const double t) const;

  // Used by Lattice planners.
  //返回上下边界点集里的首尾点
  STPoint upper_left_point() const;
  STPoint upper_right_point() const;
  STPoint bottom_left_point() const;
  STPoint bottom_right_point() const;

  //设置上下边界点集里的首尾点
  void set_upper_left_point(STPoint st_point);
  void set_upper_right_point(STPoint st_point);
  void set_bottom_left_point(STPoint st_point);
  void set_bottom_right_point(STPoint st_point);

  //设置返回障碍物道路右边终点？这个成员是用来干什么的？
  void set_obstacle_road_right_ending_t(double road_right_ending_t) {
    obstacle_road_right_ending_t_ = road_right_ending_t;
  }
  double obstacle_road_right_ending_t() const {
    return obstacle_road_right_ending_t_;
  }

 private:

  //判断ST边界是否有效，主要看上下边界点时间是否对齐
  bool IsValid(
      const std::vector<std::pair<STPoint, STPoint>>& point_pairs) const;

  //判断点到线段的距离是否小于给定值
  bool IsPointNear(const common::math::LineSegment2d& seg,
                   const common::math::Vec2d& point, const double max_dist);

  //移除ST上下边界点中的冗余点，每相邻的3个点间，若中间点到前后连线距离小于0.1就删除
  void RemoveRedundantPoints(
      std::vector<std::pair<STPoint, STPoint>>* point_pairs);
  FRIEND_TEST(StBoundaryTest, remove_redundant_points);

  //查询给定时间t在ST边界对象上下边界点对应的时间的区间的下表left,right
  bool GetIndexRange(const std::vector<STPoint>& points, const double t,
                     size_t* left, size_t* right) const;
  FRIEND_TEST(StBoundaryTest, get_index_range);

 private:
  BoundaryType boundary_type_ = BoundaryType::UNKNOWN;

  std::vector<STPoint> upper_points_;
  std::vector<STPoint> lower_points_;

  std::string id_;
  double characteristic_length_ = 1.0;
  double min_s_ = std::numeric_limits<double>::max();
  double max_s_ = std::numeric_limits<double>::lowest();
  double min_t_ = std::numeric_limits<double>::max();
  double max_t_ = std::numeric_limits<double>::lowest();

  STPoint bottom_left_point_;
  STPoint bottom_right_point_;
  STPoint upper_left_point_;
  STPoint upper_right_point_;

  double obstacle_road_right_ending_t_;
};

}  // namespace planning
}  // namespace apollo

2.st_boundary.cc

#include "modules/planning/common/speed/st_boundary.h"

#include "cyber/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::math::LineSegment2d; //apollo自定义的LineSegment2d,二维线段类
using apollo::common::math::Vec2d;  //apollo自定义的二维向量类

//STBoundary类的带参构造函数
//输入的第一个参数是一个vector，这个vector point_pairs每个元素包含两个ST点
//这两个ST点对应着每个时刻的s的上下界
//输入的第二个参数是个布尔量，is_accurate_boundary 是否为准确的边界？
STBoundary::STBoundary(
    const std::vector<std::pair<STPoint, STPoint>>& point_pairs,
    bool is_accurate_boundary) {
  //检查输入参数point_pairs点对集合是否为有效？IsValid也是STBoundary类的成员函数下面会讲到
  ACHECK(IsValid(point_pairs)) << "The input point_pairs are NOT valid";

  //用point_pairs来初始化reduced_pairs,用来在函数内部做运算的临时变量，储存剔除冗余点
  //后的ST边界点对，点对的first就是下边界,second是上边界
  std::vector<std::pair<STPoint, STPoint>> reduced_pairs(point_pairs);
  if (!is_accurate_boundary) {  //is_accurate_boundary是函数第二个输入参数，若为false
    //表示不是精确的ST边界的话，那么就调用RemoveRedundantPoint函数删除ST边界中的冗余点
    //RemoveRedundantPoints函数会判断ST上下边界中每相邻3个点中，中间点到前后两点连线距离小于0.1
    //的话就直接删除中间点，那么此时reduced_pairs就代表删除冗余点后的简化边界点
    RemoveRedundantPoints(&reduced_pairs);
  }
  //遍历这个ST边界(上下边界点对)
  for (const auto& item : reduced_pairs) {
    // 首先将ST上下边界点的时间对齐
    const double t = item.first.t();
    lower_points_.emplace_back(item.first.s(), t);
    upper_points_.emplace_back(item.second.s(), t);
  }

  //points_在哪定义？用来存放下边界点用来做什么
  //STBoundary类是public继承基类Polygon2d二维多边形类，points_是Polygon2d类的数据成员自动被
  //派生类继承，
  for (const auto& point : lower_points_) { //先把下边界点全部塞进points_
    points_.emplace_back(point.t(), point.s());
  }
  //再把上边界点倒序全部塞进points_，这样顺序是为了创建多边形
  for (auto rit = upper_points_.rbegin(); rit != upper_points_.rend(); ++rit) {
    points_.emplace_back(rit->t(), rit->s());
  }

  //从点points_按照顺序下边界起点-下边界终点-上边界终点-上边界起点建立多边形，封闭多边形？
  BuildFromPoints();

  //找出STBoundary上最小，大的s,t
  for (const auto& point : lower_points_) {
    min_s_ = std::fmin(min_s_, point.s());
  }
  for (const auto& point : upper_points_) {
    max_s_ = std::fmax(max_s_, point.s());
  }
  min_t_ = lower_points_.front().t();
  max_t_ = lower_points_.back().t();

  //障碍物道路右边的终点t等于double能表示的最大负数？这个意义是什么？后续涉及到再看
  obstacle_road_right_ending_t_ = std::numeric_limits<double>::lowest();
}

//创造实例函数，输入参数是
//ST下边界点集((t0,smin0),(t1,smin1),...)和
//ST上边界点集((t0,smax0),(t1,smax1),...)
STBoundary STBoundary::CreateInstance(
    const std::vector<STPoint>& lower_points,
    const std::vector<STPoint>& upper_points) {
  //如果下边界点集中点的个数和上边界点的个数不相等或者是他们的size小于2
  //就直接调用STBoundary类的默认构造函数
  if (lower_points.size() != upper_points.size() || lower_points.size() < 2) {
    return STBoundary();
  }

  //定义一个ST点对集类对象point_pairs，每一对起始就是一个下边界点，一个上边界点
  //point_pairs = {((t0,smin0),(t0,smax0)),
                   ((t1,smin1),(t1,smax1)),
                    ...}
  std::vector<std::pair<STPoint, STPoint>> point_pairs;
  //遍历下边界点，就把输入参数的上下边界点构成点对往point_pairs里塞
  for (size_t i = 0; i < lower_points.size(); ++i) {
    point_pairs.emplace_back(
        STPoint(lower_points.at(i).s(), lower_points.at(i).t()),
        STPoint(upper_points.at(i).s(), upper_points.at(i).t()));
  }
  //最后用point_pairs去初始化STBoundary类对象，并返回该对象
  return STBoundary(point_pairs);
}

//创建精确的实例
//输入参数lower_points  ST下边界
//upper_points ST上边界
STBoundary STBoundary::CreateInstanceAccurate(
    const std::vector<STPoint>& lower_points,
    const std::vector<STPoint>& upper_points) {
  //如果上下边界点个数不相等或个数小于2直接返回空的ST边界
  if (lower_points.size() != upper_points.size() || lower_points.size() < 2) {
    return STBoundary();
  }

  //定义上下边界点对，其实就是储存每个时间上下边界点
  std::vector<std::pair<STPoint, STPoint>> point_pairs;
  //遍历下边界点，就把输入参数的上下边界点构成点对往point_pairs里塞
  for (size_t i = 0; i < lower_points.size(); ++i) {
    point_pairs.emplace_back(
        STPoint(lower_points.at(i).s(), lower_points.at(i).t()),
        STPoint(upper_points.at(i).s(), upper_points.at(i).t()));
  }
  //最后用point_pairs去初始化STBoundary类对象，并返回该对象
  //同时将精确创建实例的标志位置为true，这个是true与否决定是否调用删除冗余点函数
  return STBoundary(point_pairs, true);
}

//返回ST边界的类型 FOLLOW跟车、KEEP_CLEAR清空绕道？，OVERTAKE超车，STOP停车
//YIELD减速避让，UNKNOWN未知
std::string STBoundary::TypeName(BoundaryType type) {
  if (type == BoundaryType::FOLLOW) {
    return "FOLLOW";
  } else if (type == BoundaryType::KEEP_CLEAR) {
    return "KEEP_CLEAR";
  } else if (type == BoundaryType::OVERTAKE) {
    return "OVERTAKE";
  } else if (type == BoundaryType::STOP) {
    return "STOP";
  } else if (type == BoundaryType::YIELD) {
    return "YIELD";
  } else if (type == BoundaryType::UNKNOWN) {
    return "UNKNOWN";
  }
  AWARN << "Unknown boundary type " << static_cast<int>(type)
        << ", treated as UNKNOWN";
  return "UNKNOWN";
}

//获取未被阻塞的s范围
//输入参数是当前时间curr_time，当前时间对应的S上边界值，当前时间对应的S下边界值
//获取的未被阻塞的s的范围放在s_upper，s_lower里
//其实该函数的作用就是先用当前时间去当前ST边界上插值出此时的上下边界点，然后根据
//场景，需要加速的场景将ST下边界置为插值出的上边界点，压着限速走，需要减速停车的场景将ST上边界置
//为插值出的下边界点，压着低速走
bool STBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
                                  double* s_lower) const {
  CHECK_NOTNULL(s_upper);
  CHECK_NOTNULL(s_lower);

  //gflag老套路去配置文件中取出speed_lon_decision_horizon纵向速度决策域默认200m
  //初始s上界为200m, s下界为0m
  *s_upper = FLAGS_speed_lon_decision_horizon;
  *s_lower = 0.0;
  //如果当前时间小于ST边界最小值，或超出最大时间直接返回true，那意思就是整个
  //0-200m都未被阻塞了
  if (curr_time < min_t_ || curr_time > max_t_) {
    return true;
  }

  //找出当前时间在ST边界上对应的时间区间[left,right]
  size_t left = 0;
  size_t right = 0;
  if (!GetIndexRange(lower_points_, curr_time, &left, &right)) {
    AERROR << "Fail to get index range.";
    return false;
  }

  //如果当前时间大于上边界点的right时间对应点的时间，直接返回true
  //0-200m都被阻塞了
  if (curr_time > upper_points_[right].t()) {
    return true;
  }

  //有定义比例r是为了根据当前时间去插值出s?
  const double r =
      (left == right
           ? 0.0
           : (curr_time - upper_points_[left].t()) /
                 (upper_points_[right].t() - upper_points_[left].t()));

  //当前时间对应的上边界点s
  double upper_cross_s =
      upper_points_[left].s() +
      r * (upper_points_[right].s() - upper_points_[left].s());
  //当前时间对应的下边界点s
  double lower_cross_s =
      lower_points_[left].s() +
      r * (lower_points_[right].s() - lower_points_[left].s());

  //如果ST边界的类型是STOP停止或者YIELD减速避让或者FOLLOW跟车
  if (boundary_type_ == BoundaryType::STOP ||
      boundary_type_ == BoundaryType::YIELD ||
      boundary_type_ == BoundaryType::FOLLOW) {
    //则将上边界点置为当前时间插值出来的下边界s,就是压着最低速走？
    *s_upper = lower_cross_s;
  } else if (boundary_type_ == BoundaryType::OVERTAKE) {
    //ST边界类型是OVERTAKE超车
    //则将下边界点置为当前时间插值出来的上边界s，和原有下边界的较大值
    //其实就是压着当前时间插值出的上边界S走？
    *s_lower = std::fmax(*s_lower, upper_cross_s);
  } else {
    ADEBUG << "boundary_type is not supported. boundary_type: "
           << static_cast<int>(boundary_type_);
    return false;
  }
  return true;
}

//查询当前时间对应的ST边界的s范围，输入参数是当前时间curr_time
//查询结果放入上边界指针s_upper，下边界指针s_lower中
bool STBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
                                   double* s_lower) const {
  CHECK_NOTNULL(s_upper);
  CHECK_NOTNULL(s_lower);
  //如果当前时间curr_time不在ST边界涵盖的时间范围内，直接返回false
  if (curr_time < min_t_ || curr_time > max_t_) {
    return false;
  }

  //当前时间所处的时间区间[left,right],left,right调用GetIndexRange去查询
  //查询curr_time所处的时间区间[left,right]，结果放入left,right
  size_t left = 0;
  size_t right = 0;
  if (!GetIndexRange(lower_points_, curr_time, &left, &right)) {
    AERROR << "Fail to get index range.";
    return false;
  }
  //r是ratio,若left=right ratio就是0
  //curr_time所处的时间区间[left,right]
  //否则ratio r = (当前时间-上边界[left这个时间对应点]的时间)/(上边界[right这个时间对应点]的时
  //间-上边界[left这个时间对应点]的时间),起始就是当前时间既然已经在left和right这个区间内，
  //那么 r=当前时间到left的时间距离/right-left时间的距离就是这个点到左边距离占整个区间长度的比例
  const double r =
      (left == right
           ? 0.0
           : (curr_time - upper_points_[left].t()) /
                 (upper_points_[right].t() - upper_points_[left].t()));

  //当前时间所处的时间区间的左时间left对应的上边界点的s + 
  //比例r * (时间区间的右时间right对应的上边界点s - 时间区间的左时间left对应的上边界点s)
  //其实不就是根据当前时间curr_time去已知的ST上边界点插值吗
  *s_upper = upper_points_[left].s() +
             r * (upper_points_[right].s() - upper_points_[left].s());
  //就是根据当前时间curr_time去已知的ST下边界点插值
  *s_lower = lower_points_[left].s() +
             r * (lower_points_[right].s() - lower_points_[left].s());

  //FLAGS_speed_lon_decision_horizon是gflags老套路是去
  //modules\planning\common\planning_gflags.cc里取出speed_lon_decision_horizon的值
  //speed_lon_decision_horizon默认为200m，就是s边界的上界插值点和200m之间的较小值
  *s_upper = std::fmin(*s_upper, FLAGS_speed_lon_decision_horizon);
  //当前时间对应的s下边界至少>=0
  *s_lower = std::fmax(*s_lower, 0.0);
  return true;
}

//该函数的作用是获取边界线的斜率？从命名来看
//输入参数是curr_time当前时间，ds_upper，ds_lower推测应该是计算的
//上下边界的变化率计算出后放入ds_upper，ds_lower是函数的计算结果
bool STBoundary::GetBoundarySlopes(const double curr_time, double* ds_upper,
                                   double* ds_lower) const {
  //若有空指针则直接返回false
  if (ds_upper == nullptr || ds_lower == nullptr) {
    return false;
  }
  //如果当前时间不在STBoundary所覆盖的ST点时间范围内，也直接返回false
  if (curr_time < min_t_ || curr_time > max_t_) {
    return false;
  }

  //静态常量kTimeIncrement 明显是一个时间增量为0.05
  static constexpr double kTimeIncrement = 0.05;
  //用当前时间curr_time - 一个时间增量得到前继点的t
  double t_prev = curr_time - kTimeIncrement;
  //初始化前继点的s上下边界为0
  double prev_s_upper = 0.0;
  double prev_s_lower = 0.0;
  //调用GetBoundarySRange函数就根据时间t去STBoundary中查询出相应时间对应的s的上下边界
  //查询结果放在prev_s_upper前继点s的上边界，prev_s_lower前继点s的下边界
  bool has_prev = GetBoundarySRange(t_prev, &prev_s_upper, &prev_s_lower);
  //定义后继点的时间为当前时间再加上时间增量kTimeIncrement
  double t_next = curr_time + kTimeIncrement;
  //初始化后继点的s上下边界为0
  double next_s_upper = 0.0;
  double next_s_lower = 0.0;
  //调用GetBoundarySRange函数就根据时间t去STBoundary中查询出相应时间对应的s的上下边界
  //查询结果放在next_s_upper后继点s的上边界，next_s_lower后继点s的下边界
  bool has_next = GetBoundarySRange(t_next, &next_s_upper, &next_s_lower);
  //同上述前继点，后继点的操作求出当前时间对应的上下s边界 curr_s_upper  curr_s_lower
  double curr_s_upper = 0.0;
  double curr_s_lower = 0.0;
  GetBoundarySRange(curr_time, &curr_s_upper, &curr_s_lower);
  //如果既没有前继点又没有后继点条件直接返回false
  if (!has_prev && !has_next) {
    return false;
  }
  //如果既有前继点又有后继点，则利用三点信息计算上下边界的纵向位置s变化率，并直接返回true
  if (has_prev && has_next) {
    *ds_upper = ((next_s_upper - curr_s_upper) / kTimeIncrement +
                 (curr_s_upper - prev_s_upper) / kTimeIncrement) *
                0.5;
    *ds_lower = ((next_s_lower - curr_s_lower) / kTimeIncrement +
                 (curr_s_lower - prev_s_lower) / kTimeIncrement) *
                0.5;
    return true;
  }
  //如果只有前继点没有后继点，则利用当前和前继点信息计算上下边界的纵向位置s变化率
  if (has_prev) {
    *ds_upper = (curr_s_upper - prev_s_upper) / kTimeIncrement;
    *ds_lower = (curr_s_lower - prev_s_lower) / kTimeIncrement;
  } else {
  //如果只有后继点没有前继点，则利用当前和后继点信息计算上下边界的纵向位置s变化率
    *ds_upper = (next_s_upper - curr_s_upper) / kTimeIncrement;
    *ds_lower = (next_s_lower - curr_s_lower) / kTimeIncrement;
  }
  return true;
}

//判断输入参数ST点在ST边界范围内？
bool STBoundary::IsPointInBoundary(const STPoint& st_point) const {
  //输入的ST点的t在ST边界的范围内？t,s均没有超出最大最小范围？
  //若超出，说明该点就不在ST边界上
  if (st_point.t() <= min_t_ || st_point.t() >= max_t_) {
    return false;
  }
  //定义临时变量left,right size_t类似无符号整型
  size_t left = 0;
  size_t right = 0;
  //用给定的ST点的时间t去lower_points_去找位于哪两个点之间，这个区间[left,right]
  //找到的区间左下标放在left，右下标放在right
  if (!GetIndexRange(lower_points_, st_point.t(), &left, &right)) {
    AERROR << "failed to get index range.";
    return false;
  }
  //这一块不懂数学原理，但大致推测是计算几何的内容，
  //上述判断有没有超出t,和s的最大最小范围属于快速排斥实验
  //下面通过叉乘计算判断点和线段的位置叫跨立实验，暂不深究
  //CrossProd函数是Apollo自己写的向量叉乘的函数
  const double check_upper = common::math::CrossProd(
      st_point, upper_points_[left], upper_points_[right]);
  const double check_lower = common::math::CrossProd(
      st_point, lower_points_[left], lower_points_[right]);

  //若check_upper和check_lower 异号则说明给定ST点在ST边界范围内
  return (check_upper * check_lower < 0);
}

//该函数的主要作用就是根据给定的纵向位置s，在STBoundary中的每个点对的上下边界点上叠加
//每个点对的下边界点的纵向位置减去s
//每个点对的上边界点的纵向位置加上s
//这样产生的新的点对再塞到空的point_pairs里，相当于是把原有的STBoundary边界变宽了2s?
STBoundary STBoundary::ExpandByS(const double s) const {
  if (lower_points_.empty()) {
    return STBoundary();
  }
  std::vector<std::pair<STPoint, STPoint>> point_pairs;
  for (size_t i = 0; i < lower_points_.size(); ++i) {
    point_pairs.emplace_back(
        STPoint(lower_points_[i].s() - s, lower_points_[i].t()),
        STPoint(upper_points_[i].s() + s, upper_points_[i].t()));
  }
  return STBoundary(std::move(point_pairs));
}

//该函数的主要作用就是根据给定的时间t在STBoundary中再插值一个新点对，输入参数是时间t
STBoundary STBoundary::ExpandByT(const double t) const {
  //如果数据成员下边界点为空，报故障信息，调用默认构造函数初始化对象
  if (lower_points_.empty()) {
    AERROR << "The current st_boundary has NO points.";
    return STBoundary();
  }

  //定义一个ST点对集合类对象point_pairs，每一对起始就是一个下边界点，一个上边界点
  //point_pairs = {((t0,smin0),(t0,smax0)),
                   ((t1,smin1),(t1,smax1)),
                    ...}
  std::vector<std::pair<STPoint, STPoint>> point_pairs;

  //定义下边界点集相邻点之间的时间差
  const double left_delta_t = lower_points_[1].t() - lower_points_[0].t();
  //定义下边界点集相邻点之间的纵向位置s差，就是第二个下边界点的s减去第一个
  const double lower_left_delta_s = lower_points_[1].s() - lower_points_[0].s();
  //定义上边界点集相邻点之间的纵向位置s差，就是第二个上边界点的s减去第一个
  const double upper_left_delta_s = upper_points_[1].s() - upper_points_[0].s();

  //往临时定义的局部变量point_pairs里塞一个新的点对
  //这个新的点对是根据上下边界点单位时间差对应的纵向位置差插值计算出的，根据新的时间t
  //就是根据给定时间t插值了个新的上下边界点构成点对塞到point_pairs里
  //注意塞之前point_pairs为空
  point_pairs.emplace_back(
      STPoint(lower_points_[0].s() - t * lower_left_delta_s / left_delta_t,
              lower_points_[0].t() - t),
      STPoint(upper_points_[0].s() - t * upper_left_delta_s / left_delta_t,
              upper_points_.front().t() - t));

  //定义了一个很小的正数，大概又是判断条件阈值
  const double kMinSEpsilon = 1e-3;
  //点对集合的这个新加的点对，就这一个点对
  //设定这个点对的下边界点的S为min(这个点上边界点s-kMinSEpsilon, 这个点下边界点s)
  //就是下边界点的s不会超过上边界
  point_pairs.front().first.set_s(
      std::fmin(point_pairs.front().second.s() - kMinSEpsilon,
                point_pairs.front().first.s()));

  //又遍历数据成员lower_points_,把lower_points_，upper_points_构成点对直接全部塞到
  //point_pairs
  for (size_t i = 0; i < lower_points_.size(); ++i) {
    point_pairs.emplace_back(lower_points_[i], upper_points_[i]);
  }

  //定义length为lower_points_的长度
  size_t length = lower_points_.size();
  DCHECK_GE(length, 2U);

  //下边界点集里最后一个点和倒数第二个点之间的时间差right_delta_t 
  const double right_delta_t =
      lower_points_[length - 1].t() - lower_points_[length - 2].t();
  //下边界点集里最后一个点和倒数第二个点之间的纵向位置差lower_right_delta_s 
  const double lower_right_delta_s =
      lower_points_[length - 1].s() - lower_points_[length - 2].s();
  //上边界点集里最后一个点和倒数第二个点之间的纵向位置差upper_right_delta_s 
  const double upper_right_delta_s =
      upper_points_[length - 1].s() - upper_points_[length - 2].s();

  //按照离散点集lower_points_，upper_points_
  //根据给定的时间t插值出新的上下边界点构成点对塞到point_pairs里
  //就是从原有的下边界点的最后一个时间+给定时间t插值出新的上下边界点对
  point_pairs.emplace_back(STPoint(lower_points_.back().s() +
                                       t * lower_right_delta_s / right_delta_t,
                                   lower_points_.back().t() + t),
                           STPoint(upper_points_.back().s() +
                                       t * upper_right_delta_s / right_delta_t,
                                   upper_points_.back().t() + t));
  //这个设置point_pairs末尾上边界点s是确保其不会大于它之前的值？
  point_pairs.back().second.set_s(
      std::fmax(point_pairs.back().second.s(),
                point_pairs.back().first.s() + kMinSEpsilon));

  //将点对集合point_pairs move到STBoundary类对象中，并返回该对象
  //所以该函数的主要作用就是根据给定的时间t在STBoundary中再插值一个新点对
  return STBoundary(std::move(point_pairs));
}

//返回边界类型函数
STBoundary::BoundaryType STBoundary::boundary_type() const {
  return boundary_type_;
}

//设置数据成员边界类型boundary_type_
void STBoundary::SetBoundaryType(const BoundaryType& boundary_type) {
  boundary_type_ = boundary_type;
}


//获取边界id函数
const std::string& STBoundary::id() const { return id_; }

//设置数据成员边界id_
void STBoundary::set_id(const std::string& id) { id_ = id; }

//返回类数据成员的特征长度characteristic_length_
double STBoundary::characteristic_length() const {
  return characteristic_length_;
}

//用给定长度设置数据成员特征长度？这个特征长度是指什么？后续涉及到再看
void STBoundary::SetCharacteristicLength(const double characteristic_length) {
  characteristic_length_ = characteristic_length;
}

//这4个成员函数就是获取STBoundary类对象里的最小/大s/t
double STBoundary::min_s() const { return min_s_; }
double STBoundary::min_t() const { return min_t_; }
double STBoundary::max_s() const { return max_s_; }
double STBoundary::max_t() const { return max_t_; }

//用给定时间t去截断STBoundary ST边界点对集
STBoundary STBoundary::CutOffByT(const double t) const {
  //lower_points存放ST边界的下边界点集((t0,smin0),(t1,smin1)...)
  //upper_points存放ST边界的上边界点集((t0,smax0),(t1,smax1)...)
  std::vector<STPoint> lower_points;
  std::vector<STPoint> upper_points;
  //遍历ST边界里的每一个点，lower_points_是STBoundary类的数据成员不是上面定义的局部变量
  for (size_t i = 0; i < lower_points_.size() && i < upper_points_.size();
       ++i) {
    //如果第i个下边界点的t小于给定时间t就直接继续下一轮循环
    if (lower_points_[i].t() < t) {
      continue;
    }
    //如果执行到这里说明第i个下边界点的t不小于给定时间t
    //大于t的点就往lower_points，upper_points里塞
    //所以这个函数的意思就是以给定时间t为界截断原有的ST边界，只留里面时间大于t的部分
    lower_points.push_back(lower_points_[i]);
    upper_points.push_back(upper_points_[i]);
  }
  //用截断之后的ST下边界点和上边界点去调用CreateInstance创造实例这个函数
  //去创造新的STBoundary类对象
  return CreateInstance(lower_points, upper_points);
}

//返回上部左边点
STPoint STBoundary::upper_left_point() const {
  DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
  return upper_points_.front();
}

//返回上部右边点
STPoint STBoundary::upper_right_point() const {
  DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
  return upper_points_.back();
}

//返回底部左边点
STPoint STBoundary::bottom_left_point() const {
  DCHECK(!lower_points_.empty()) << "StBoundary has zero points.";
  return lower_points_.front();
}

//返回底部右边点
STPoint STBoundary::bottom_right_point() const {
  DCHECK(!lower_points_.empty()) << "StBoundary has zero points.";
  return lower_points_.back();
}

//设定上部左边点
void STBoundary::set_upper_left_point(STPoint st_point) {
  upper_left_point_ = std::move(st_point);
}

//设定上部右边点
void STBoundary::set_upper_right_point(STPoint st_point) {
  upper_right_point_ = std::move(st_point);
}

//设定底部左边点
void STBoundary::set_bottom_left_point(STPoint st_point) {
  bottom_left_point_ = std::move(st_point);
}

//设定底部右边点
void STBoundary::set_bottom_right_point(STPoint st_point) {
  bottom_right_point_ = std::move(st_point);
}

///
// Private functions for internal usage.
//私有函数仅供内部使用

//判断是否有效，输入参数ST点对集point_pairs,每一对都是一个时间t对应的s的下边界点(t,s)和
//上边界点(t,s)
bool STBoundary::IsValid(
    const std::vector<std::pair<STPoint, STPoint>>& point_pairs) const {
  //若点对的size小于2直接返回false
  if (point_pairs.size() < 2) {
    AERROR << "point_pairs.size() must >= 2, but current point_pairs.size() = "
           << point_pairs.size();
    return false;
  }

  //定义了两个判断阈值后面会用到
  static constexpr double kStBoundaryEpsilon = 1e-9;
  static constexpr double kMinDeltaT = 1e-6;

  //遍历ST点对集point_pairs
  for (size_t i = 0; i < point_pairs.size(); ++i) {
    //第i个点对的下边界点放入curr_lower 
    //第i个点对的上边界点放入curr_lower 
    const auto& curr_lower = point_pairs[i].first;
    const auto& curr_upper = point_pairs[i].second;
    //如果上边界点的纵向位置s小于下边节点就返回false
    if (curr_upper.s() < curr_lower.s()) {
      AERROR << "ST-boundary's upper-s must >= lower-s";
      return false;
    }
    //若上下边界点的时间之差大于上面定义的阈值kMinDeltaT = 1e-6，则返回false
    if (std::fabs(curr_lower.t() - curr_upper.t()) > kStBoundaryEpsilon) {
      AERROR << "Points in every ST-point pair should be at the same time.";
      return false;
    }
    //若i+1不等于point_pairs的size,就是如果还没遍历到最后一个点对的话
    if (i + 1 != point_pairs.size()) {
      //下一个i+1点的下边界点next_lower，上边界点next_upper 
      const auto& next_lower = point_pairs[i + 1].first;
      const auto& next_upper = point_pairs[i + 1].second;
      //若第i个点对的下边界点对应的时间 + kMinDeltaT(1e-6) 大于等于
      //第i个点对的上边界点对应的时间
      //就是第i+1个点对的时间与第i个点对的时间之差没有超过这个阈值kMinDeltaT 就返回
      //false，相邻的点对的时间差必须要超过一定程度
      if (std::fmax(curr_lower.t(), curr_upper.t()) + kMinDeltaT >=
          std::fmin(next_lower.t(), next_upper.t())) {
        AERROR << "Latter points should have larger t: "
               << "curr_lower[" << curr_lower.DebugString() << "] curr_upper["
               << curr_upper.DebugString() << "] next_lower["
               << next_lower.DebugString() << "] next_upper["
               << next_upper.DebugString() << "]";
        return false;
      }
    }
  }
  //上面都没有返回false的话，就返回true
  return true;
}

//输入参数是一个点，一个线段，一个最大距离
//本函数的作用就是判断点到线段的距离是否小于最大距离，若是，则点距线段足够近返回true，否则返回
//false
bool STBoundary::IsPointNear(const common::math::LineSegment2d& seg,
                             const Vec2d& point, const double max_dist) {
  return seg.DistanceSquareTo(point) < max_dist * max_dist;
}

//输入参数是ST点对集合point_pairs
//函数的作用是移除STBoundary上下边界冗余点
//实现功能：上下边界的散点序列中，若中间点到前后点连线距离小于0.1则可剔除中间点
//上下边界必须同时操作，必须具有相同的点的个数
//详解参考本博客下方的手写笔记
void STBoundary::RemoveRedundantPoints(
    std::vector<std::pair<STPoint, STPoint>>* point_pairs) {
  
  //!point_pairs没看懂，是表示非空吗？
  //非空或者point_pairs的大小小于等于2则直接返回无需移除冗余点
  if (!point_pairs || point_pairs->size() <= 2) {
    return;
  }

  //定义一个很小的正数，若中间点偏离前后点连线距离小于0.1则可以移除
  const double kMaxDist = 0.1;
  //定义i,j
  size_t i = 0;
  size_t j = 1;

  //当i小于point_pairs的长度时 且 j+1也小于point_pairs的长度时
  while (i < point_pairs->size() && j + 1 < point_pairs->size()) {
    //定义下边界的线段,at(i)表示访问vector的第i个元素
    //这里的意思就是lower_seg是point_pairs里第i个点对和第j+1个点对的下边界点
    //每个ST点对由 （下边界点(t0,s0)，上边界点(t1,s1)）构成
    //.first表示访问第一个元素
    //LineSegment2d类构造函数，意味着起始点是第i个下边界点和第j+1个下边界点
    LineSegment2d lower_seg(point_pairs->at(i).first,
                            point_pairs->at(j + 1).first);
    //这里的意思就是upper_seg是point_pairs里第i个点对和第j+1个点对的上边界点
    LineSegment2d upper_seg(point_pairs->at(i).second,
                            point_pairs->at(j + 1).second);
    //如果点对集合point_pairs第j个点的下边界点离lower_seg线段不够近 或 第j个点的上边界点离
    //upper_seg线段不够近
    if (!IsPointNear(lower_seg, point_pairs->at(j).first, kMaxDist) ||
        !IsPointNear(upper_seg, point_pairs->at(j).second, kMaxDist)) {
    //那么i就叠加  
    ++i;
      //如果i不等于j，说明第j个点距离前后点连线不够近，就令第i+1个点为第j个点
      //若相等，说明第j个点距离前后点连线够近直接跳过
      if (i != j) {
        //点对的第i个点就等于第j个点
        point_pairs->at(i) = point_pairs->at(j);
      }
    }
    //叠加j
    ++j;
  }
  //点对的第i+1个点为点对的最后一个点
  point_pairs->at(++i) = point_pairs->back();
  //vector resize重新分配空间，则点对point_pairs里只有i+1个点
  point_pairs->resize(i + 1);
}

//这个函数的作用就是根据给定的时间t去ST点集中找到t所在的ST点区间，[tl,tu]
//区间左边点下标和区间右边点的下标分别存放到left和right指针中作为函数的运算结果
//函数正常运行返回true
//输入参数ST点集points，给定时间t, 待输出结果的left，right都是size_t（类似无符号整型）
bool STBoundary::GetIndexRange(const std::vector<STPoint>& points,
                               const double t, size_t* left,
                               size_t* right) const {
  CHECK_NOTNULL(left);
  CHECK_NOTNULL(right);
  //首先判断给定的时间t是否在整个ST点集points对应的时间范围内
  //若不在则报错
  if (t < points.front().t() || t > points.back().t()) {
    AERROR << "t is out of range. t = " << t;
    return false;
  }
  //定义了一个条件函数，配合std::lower_bound使用
  auto comp = [](const STPoint& p, const double t) { return p.t() < t; };
  //在ST点集中倒序找到第一个时间小于给定时间t的ST点放入first_ge中
  //ST点集自动按时间升序排列
  auto first_ge = std::lower_bound(points.begin(), points.end(), t, comp);
  //求出这个first_ge的正序下标
  size_t index = std::distance(points.begin(), first_ge);
  //如果first_ge就是第一个点,left，right就都等于0
  if (index == 0) {
    *left = *right = 0;
  //如果first_ge就是最后一个点,left，right就都等于points.size() - 1
  } else if (first_ge == points.end()) {
    *left = *right = points.size() - 1;
  } else {
  //否则的话，left就是first_ge前一个点的下标，right就是first_ge这个点下标
  //有点问题，这样的话，获得的不是t所在points点集中的时间区间，而是上一个区间？
  //或许把上面的lower_bound换成upper_bound?
  //或者left = index,right = index + 1才对？
    *left = index - 1;
    *right = index;
  }
  return true;
}

}  // namespace planning
}  // namespace apollo

3. st_boundary_test.cc

Apollo测试用例真的很值得一读，帮助你了解这个类的使用。代码我这里不贴了，只上自己的代码笔记