Apollo MatchPoint的获取方式

MatchPoint的整体寻找过程如下：

1、设主车的坐标为(x,y)

2、生成的轨迹点为trajectoriesPoints

3、遍历生成的轨迹点，找到轨迹中距离主车(x,y)最小的的点；

4、在最小距离的轨迹点附近找到两个邻近的点通过黄金分割搜索算法找到临近两个点内距离主车(x,y)最小的点，此点即为匹配点

计算过程图示如下：

        如上图所示:其中(x,y)为主车所在位置，p0~pn为规划的轨迹；假设p_i为规划轨迹中距离主车最近的点，获取p_i后，并获取点p_i-1和p_i+1;在区间[p_i-1,p_i+1]范围内通过黄金比例搜索算法找到该区间内距离主车(x,y)的点，该点即为MatchPoint。

          整体MatchPoint代码如下(Apollo源码，具体请参考百度开源项目)

PathPoint TrajectoryAnalyzer::QueryMatchedPathPoint(const double x,
                                                    const double y) const {
  CHECK_GT(trajectory_points_.size(), 0U);
  //获取距离主车最小的规划轨迹点
  double d_min = PointDistanceSquare(trajectory_points_.front(), x, y);
  size_t index_min = 0;

  for (size_t i = 1; i < trajectory_points_.size(); ++i) {
    double d_temp = PointDistanceSquare(trajectory_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }

  size_t index_start = index_min == 0 ? index_min : index_min - 1;
  size_t index_end =
      index_min + 1 == trajectory_points_.size() ? index_min : index_min + 1;
  //判断该点是否为首尾点，若为轨迹首尾点，该点即为MatchPoint
  const double kEpsilon = 0.001;
  if (index_start == index_end ||
      std::fabs(trajectory_points_[index_start].path_point().s() -
                trajectory_points_[index_end].path_point().s()) <= kEpsilon) {
    return TrajectoryPointToPathPoint(trajectory_points_[index_start]);
  }
  //若不为首尾轨迹点，通过黄金搜索算法计算出距离最小轨迹点的前后区间内真正的最小点，该点即为MatchPoint

  return FindMinDistancePoint(trajectory_points_[index_start],
                              trajectory_points_[index_end], x, y);
}