EGO Planner代码解析bspline_optimizer部分(2)

/*** Assign data to each segment ***/

选出障碍物表面的基点pij

对于每一段调整后的障碍物控制点对

在对应的路径点集a_star_pathes中找到与该控制点i构成的向量近似垂直于前后两控制点构成的向量的路径点作为该控制点的牵引点intersection_point ，并将got_intersection_id设为当前控制点的索引。

Astar_id的索引数目只为A星路径点的一半，主要原因：计算全部路径点需要大量的计算

如果上一步中找到了牵引点并且牵引点与该控制点的距离大于0，则将该控制点对应的cps_.flag_temp置为true。

在膨胀栅格地图中，在牵引点与该控制点的距离中以分辨率为步长，找到控制点与牵引点之间障碍物表面栅格点(即第一个占用的点的位置)，并将该点压入控制点对应的cps_.base_point基点pij中，控制点到牵引点的单位方向压入对应的cps_.direction，即vij中。

否则got_intersection_id置为-1，cps_.flag_temp置为false

如果当前障碍物控制点对之间没有其他控制点，则在a_star_pathes中找到与控制点对中点构成的向量近似垂直于控制点对构成的向量所对应的路径点作为该控制点的牵引点intersection_point，got_intersection_id设为当前进入障碍物的控制点的索引，如果牵引点与中点的距离大于1cm， 该控制点对应的cps_.flag_temp置为true，该控制点压入对应的cps_.base_point，牵引点与中点的单位方向向量压入cps_.direction。

如果障碍物控制点对之间有控制点没有牵引点和牵引方向，则将最后一次得到的牵引点和方向最为默认点和方向，压入对应的cps_.base_point和cps_.direction。

最后将调整后的障碍物控制点对压入final_segment_ids并返回。

 

/*** Assign data to each segment ***/
    //为每个障碍段分配数据
    for (size_t i = 0; i < segment_ids.size(); i++)
    //遍历每个控制点对segment_ids即进障碍物段
    {
      
      // step 1
      for (int j = final_segment_ids[i].first; j <= final_segment_ids[i].second; ++j)
      //对于每一段调整后的障碍物控制点对
        cps_.flag_temp[j] = false;
      
      // step 2
      int got_intersection_id = -1;
      //got_intersection_id设为控制点的牵引点的索引
      for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j)
      {
        Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;
        //ctrl_pts_law：第j+1个控制点减去第j-1个控制点 （即j-1这个位置的控制点指向j+1这个位置的控制点）
        //定义控制点law为当前控制点的前后两控制点
        //控制点的牵引点intersection_point
        //每个障碍段都有一个Asatr算法生成的路径a_star_pathes
        int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; 
        //Astar_id的索引数目只为A星路径点的一半，主要原因：计算全部路径点需要大量的计算
        // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
        double val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;
        //值=A星索引点-(障碍段中的控制点(从第一个进障碍物前的控制点开始))与ctrl_pts_law向量的乘积
        //向量是有(大小和方向)
        while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()) 
        {//如果Astar索引的数目>=0且Astar索引点的数目小于全部Astar路径点的值
          last_Astar_id = Astar_id;//Astar索引点就为最后一个索引

          if (val >= 0)//如何这个值>=0,则
            --Astar_id;//新的牵引点为：Astar牵引点-1
          else//如何这个值<0,则
            ++Astar_id;//新的牵引点为：Astar牵引点+1

          val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);
          //值=A星索引点-(障碍段中的控制点(从第一个进障碍物前的控制点+1开始))与ctrl_pts_law向量的乘积
          //a.dot(b) 与 np.dot(a,b)效果相同,算内积


          if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) 
          //如果该值*最后的值<=0且val>0或最后的last_val > 0
          // val = last_val = 0.0 is not allowed
          {
            intersection_point =//牵引点
                a_star_pathes[i][Astar_id] +
                ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *
                 (ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t
                );
                //=第i条Astar规划的路径中的第几个路径点-最后一个A星路径点*（(障碍段中的控制点与Astar规划的路径中的第几个路径点)与ctrl_pts_law向量的乘积/(障碍段中的控制点与Astar规划的路径中的第几个路径点)与ctrl_pts_law向量的乘积.
                //找到与该控制点构成的向量近似垂直于前后两控制点构成的向量的路径点
            //cout << "i=" << i << " j=" << j << " Astar_id=" << Astar_id << " last_Astar_id=" << last_Astar_id << " intersection_point = " << intersection_point.transpose() << endl;

            got_intersection_id = j;//牵引点的索引
            break;
          }
        }

        if (got_intersection_id >= 0)//如果上一步中找到了牵引点并且牵引点与该控制点的距离大于0
        {
          cps_.flag_temp[j] = true;
          //则将该控制点对应的cps_.flag_temp置为true
          double length = (intersection_point - cps_.points.col(j)).norm();
          if (length > 1e-5)//如果牵引点与控制点的距离大于1cm
          {
            for (double a = length; a >= 0.0; a -= grid_map_->getResolution())
            {
              occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));
              //在膨胀栅格地图中，找到控制点与牵引点之间障碍物表面栅格点
              if (occ || a < grid_map_->getResolution())
              {
                if (occ)
                  a += grid_map_->getResolution();
                
                cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));
                //并将障碍物表面栅格点压入控制点对应的cps_.base_point基点中
                cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());
                //控制点到牵引点的单位方向压入对应的cps_.direction。
                break;
                //否则got_intersection_id置为-1，cps_.flag_temp置为false
              }
            }
          }
        }
      }

      /* Corner case: the segment length is too short. Here the control points may outside the A* path, leading to opposite gradient direction. So I have to take special care of it */
      //切线 转角情况：线段长度太短。这里，控制点可能位于A*路径之外，导致相反的渐变方向。
      if (segment_ids[i].second - segment_ids[i].first == 1)
      // 如果当前障碍物控制点对之间没有其他控制点
      {
        Eigen::Vector3d ctrl_pts_law(cps_.points.col(segment_ids[i].second) - cps_.points.col(segment_ids[i].first)), intersection_point;
        //ctrl_pts_law为出障碍物后的第一个控制点-进障碍物前的第一个控制点
        Eigen::Vector3d middle_point = (cps_.points.col(segment_ids[i].second) + cps_.points.col(segment_ids[i].first)) / 2;
        //控制点对中点，
        int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computation
        //Astar_id索引
        double val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law), last_val = val;
        //在a_star_pathes中找到与控制点对中点构成的向量近似垂直于控制点对构成的向量的点
        while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size())
        {
          last_Astar_id = Astar_id;

          if (val >= 0)
            --Astar_id;
          else
            ++Astar_id;

          val = (a_star_pathes[i][Astar_id] - middle_point).dot(ctrl_pts_law);

          if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed
          {
            intersection_point =
                a_star_pathes[i][Astar_id] +
                ((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *
                 (ctrl_pts_law.dot(middle_point - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t
                );
                //所对应的路径点作为该控制点的牵引点intersection_point

            if ((intersection_point - middle_point).norm() > 0.01) // 1cm.
            //如果牵引点与中点的距离大于1cm
            {
              cps_.flag_temp[segment_ids[i].first] = true;
              //该控制点对应的cps_.flag_temp置为true
              cps_.base_point[segment_ids[i].first].push_back(cps_.points.col(segment_ids[i].first));
              //该控制点压入对应的cps_.base_point
              cps_.direction[segment_ids[i].first].push_back((intersection_point - middle_point).normalized());
              //牵引点与中点的单位方向向量压入cps_.direction

              got_intersection_id = segment_ids[i].first;
              //got_intersection_id设为当前进入障碍物的控制点的索引
            }
            break;
          }
        }
      }

      //step 3
      if (got_intersection_id >= 0)
      //如果障碍物控制点对之间有控制点
      {
        for (int j = got_intersection_id + 1; j <= final_segment_ids[i].second; ++j)
          if (!cps_.flag_temp[j])//没有牵引点和牵引方向
          {
            //则将最后一次得到的牵引点和方向最为默认点和方向
            cps_.base_point[j].push_back(cps_.base_point[j - 1].back());
            cps_.direction[j].push_back(cps_.direction[j - 1].back());
          }

        for (int j = got_intersection_id - 1; j >= final_segment_ids[i].first; --j)
          if (!cps_.flag_temp[j])//没有牵引点和牵引方向
          {
            //则将最后一次得到的牵引点和方向最为默认点和方向
            cps_.base_point[j].push_back(cps_.base_point[j + 1].back());
            cps_.direction[j].push_back(cps_.direction[j + 1].back());
            //加入对应的cps_.base_point和cps_.direction
          }
      }
      else
      {
        // Just ignore, it does not matter ^_^.
        // ROS_ERROR("Failed to generate direction! segment_id=%d", i);
      }
    }

    return a_star_pathes;//最后将调整后的障碍物控制点对压入final_segment_ids并返回
  }