fast planner代码解析--topo_prm.cpp

X uuuer.

已于 2023-03-09 21:07:56 修改

阅读量918

点赞数

文章标签：经验分享

于 2022-07-31 21:34:07 首次发布

本文链接：https://blog.csdn.net/weixin_45868890/article/details/126090236

版权

1、void TopologyPRM::init(ros::NodeHandle& nh)

2、void TopologyPRM::findTopoPaths()

3、list TopologyPRM::createGraph()

3.1 getSample()

3.2 dist = edt_environment_->evaluateCoarseEDT(pt, -1.0)

3.3 vector TopologyPRM::findVisibGuard()

3.3.1 不可见的判断用函数linVisb()

3.4 bool TopologyPRM::needConnection()

3.4.1 判断同伦：使用函数bool TopologyPRM::sameTopoPath()

3.5 void TopologyPRM::pruneGraph()

4、vector> TopologyPRM::searchPaths()

4.1深度优先搜索算法DFS：void TopologyPRM::depthFirstSearch()

5、void TopologyPRM::shortcutPaths()

5.1 void TopologyPRM::shortcutPath() 路径缩短

5.1.1 vector TopologyPRM::discretizeLine()

6、vector> TopologyPRM::pruneEquivalent()

7、vector> TopologyPRM::selectShortPaths()

1、void TopologyPRM::init(ros::NodeHandle& nh)

该函数用于初始化所有参数

void TopologyPRM::init(ros::NodeHandle& nh) {
  //初始化
  graph_.clear();//图
  eng_ = default_random_engine(rd_());//默认随机数生成器
  rand_pos_ = uniform_real_distribution<double>(-1.0, 1.0);//均匀实分布

  // init parameter初始化参数
  nh.param("topo_prm/sample_inflate_x", sample_inflate_(0), -1.0);//inflate膨胀
  nh.param("topo_prm/sample_inflate_y", sample_inflate_(1), -1.0);
  nh.param("topo_prm/sample_inflate_z", sample_inflate_(2), -1.0);
  nh.param("topo_prm/clearance", clearance_, -1.0);//安全阈值
  nh.param("topo_prm/short_cut_num", short_cut_num_, -1);//短_修剪数目
  nh.param("topo_prm/reserve_num", reserve_num_, -1);//保留的数目
  nh.param("topo_prm/ratio_to_short", ratio_to_short_, -1.0);//长度比值
  nh.param("topo_prm/max_sample_num", max_sample_num_, -1);//最大采样数目
  nh.param("topo_prm/max_sample_time", max_sample_time_, -1.0);//最大采样时间
  nh.param("topo_prm/max_raw_path", max_raw_path_, -1);//最大偏航路径
  nh.param("topo_prm/max_raw_path2", max_raw_path2_, -1);//最大偏航路径2
  nh.param("topo_prm/parallel_shortcut", parallel_shortcut_, false);//并行剪枝
  resolution_ = edt_environment_->sdf_map_->getResolution();//分辨率
  offset_ = Eigen::Vector3d(0.5, 0.5, 0.5) - edt_environment_->sdf_map_->getOrigin() / resolution_;//偏差

  for (int i = 0; i < max_raw_path_; ++i) {
    casters_.push_back(RayCaster());//光线投射
  }
}

2、void TopologyPRM::findTopoPaths()

该函数用于寻找拓扑路径，即Topopath 路径搜索算法的调用入口，由五部分组成，下面将一一进行分析。

1.createGraph；2.searchPaths； 3.shortcutPaths(); 4.pruneEquivalent();5.selectShortPaths();

void TopologyPRM::findTopoPaths(Eigen::Vector3d start, Eigen::Vector3d end,
                                vector<Eigen::Vector3d> start_pts, vector<Eigen::Vector3d> end_pts,
                                list<GraphNode::Ptr>& graph, vector<vector<Eigen::Vector3d>>& raw_paths,
                                vector<vector<Eigen::Vector3d>>& filtered_paths,
                                vector<vector<Eigen::Vector3d>>& select_paths) {
                                  //寻找Topo路径(多条)
  ros::Time t1, t2;//时间

  double graph_time, search_time, short_time, prune_time, select_time;
  //生成节点图的时间、路径搜索时间、路径缩短时间、路径修剪时间、路径选择时间
  /* ---------- create the topo graph ---------- */
  //创建空间中的采样节点图
  t1 = ros::Time::now();//t1为当前时间

  start_pts_ = start_pts;//起始位置
  end_pts_ = end_pts;//目标位置

  graph = createGraph(start, end);//生成节点图(以起点和目标点为节点)

  graph_time = (ros::Time::now() - t1).toSec();//节点图生成的时间

  /* ---------- search paths in the graph ---------- */
  //在节点图中搜索路径
  t1 = ros::Time::now();

  raw_paths = searchPaths();//搜索的路径

  search_time = (ros::Time::now() - t1).toSec();//搜索时间

  /* ---------- path shortening ---------- */
  //路径缩短
  // for parallel, save result in short_paths_
  //对于并行，将结果保存在短_路径 short_paths_中_
  t1 = ros::Time::now();

  shortcutPaths();//

  short_time = (ros::Time::now() - t1).toSec();//路径缩短时间

  /* ---------- prune equivalent paths ---------- */
  //等效路径d的修剪，只保留属于不同UVD class
  t1 = ros::Time::now();

  filtered_paths = pruneEquivalent(short_paths_);//已筛选的路径

  prune_time = (ros::Time::now() - t1).toSec();//修剪时间
  // cout << "prune: " << (t2 - t1).toSec() << endl;

  /* ---------- select N shortest paths ---------- */
  t1 = ros::Time::now();

  select_paths = selectShortPaths(filtered_paths, 1);//搜索最佳路径

  select_time = (ros::Time::now() - t1).toSec();//搜索时间

  final_paths_ = select_paths;//搜索的路径为最终的路径

  double total_time = graph_time + search_time + short_time + prune_time + select_time;//总时间的计算

  std::cout << "\n[Topo]: total time: " << total_time << ", graph: " << graph_time
            << ", search: " << search_time << ", short: " << short_time << ", prune: " << prune_time
            << ", select: " << select_time << std::endl;
}

3、list<GraphNode::Ptr> TopologyPRM::createGraph()

根据起始和目标的位置信息和载入的参数来设置采样点的范围，在采样时间大于最大采样时间或采样点数大于最大采样点数时停止采样。

list<GraphNode::Ptr> TopologyPRM::createGraph(Eigen::Vector3d start, Eigen::Vector3d end) {
  //创建节点图
  // std::cout << "[Topo]: searching----------------------" << std::endl;

  /* init the start, end and sample region */
  //初始化开始、结束和采样区域
  graph_.clear();//清除所有的图节点
  // collis_.clear();

  GraphNode::Ptr start_node = GraphNode::Ptr(new GraphNode(start, GraphNode::Guard, 0));//开始节点为开始位置
  GraphNode::Ptr end_node = GraphNode::Ptr(new GraphNode(end, GraphNode::Guard, 1));//结束节点为目标位置
  //开始节点和结束节点加入节点图中
  graph_.push_back(start_node);
  graph_.push_back(end_node);

  // sample region采样区域
  sample_r_(0) = 0.5 * (end - start).norm() + sample_inflate_(0);
  //sample_inflate_x为sample_inflate_(0)，x方向上采样的半径
  sample_r_(1) = sample_inflate_(1);//y方向上采样的半径
  sample_r_(2) = sample_inflate_(2);//z方向上采样的半径

  // transformation转移
  translation_ = 0.5 * (start + end);

  Eigen::Vector3d xtf, ytf, ztf, downward(0, 0, -1);
  xtf = (end - translation_).normalized();
  ytf = xtf.cross(downward).normalized();
  ztf = xtf.cross(ytf);

  rotation_.col(0) = xtf;
  rotation_.col(1) = ytf;
  rotation_.col(2) = ztf;

  int node_id = 1;//节点索引

  /* ---------- main loop ---------- */
  //主循环
  int sample_num = 0;//采样数目
  double sample_time = 0.0;//采样时间
  Eigen::Vector3d pt;//位置
  ros::Time t1, t2;//时间
  while (sample_time < max_sample_time_ && sample_num < max_sample_num_) {
    //若采样时间大于最大采样时间，采样点数大于最大采样点数时，结束采样循环
    t1 = ros::Time::now();

    pt = getSample();//获取采样
    ++sample_num;//采样数目++
    double dist;//距离
    Eigen::Vector3d grad;//梯度
    // edt_environment_->evaluateEDTWithGrad(pt, -1.0, dist, grad);
    dist = edt_environment_->evaluateCoarseEDT(pt, -1.0);//距离为ESDF地图的离最近障碍物的距离
    if (dist <= clearance_) {//如果距离小于安全阈值
      sample_time += (ros::Time::now() - t1).toSec();//采样时间+，说明采样点在障碍物中
      continue;//结束
    }

    /* find visible guard */
    //寻找可见的guard点
    vector<GraphNode::Ptr> visib_guards = findVisibGuard(pt);//可见的guard点
    if (visib_guards.size() == 0) {//如果可见的guard的数目为0
      GraphNode::Ptr guard = GraphNode::Ptr(new GraphNode(pt, GraphNode::Guard, ++node_id));
      //该节点创建为一个新的guard点
      graph_.push_back(guard);
    } else if (visib_guards.size() == 2) {
      /* try adding new connection between two guard */
      // vector<pair<GraphNode::Ptr, GraphNode::Ptr>> sort_guards =
      // sortVisibGuard(visib_guards);
      //该采样点(节点)正好对两个guard的可见时
      bool need_connect = needConnection(visib_guards[0], visib_guards[1], pt);
      //利用needConnection函数判断是否要添加新的connector
      if (!need_connect) {
        sample_time += (ros::Time::now() - t1).toSec();//如果不需要连接则采样时间+
        continue;
      }
      // new useful connection needed, add new connector
      //需要新的有用连接，添加新连接器
      GraphNode::Ptr connector = GraphNode::Ptr(new GraphNode(pt, GraphNode::Connector, ++node_id));
      graph_.push_back(connector);//节点图中添加新的连接节点

      // connect guards，连接guards点
      visib_guards[0]->neighbors_.push_back(connector);
      visib_guards[1]->neighbors_.push_back(connector);

      connector->neighbors_.push_back(visib_guards[0]);
      connector->neighbors_.push_back(visib_guards[1]);
    }

    sample_time += (ros::Time::now() - t1).toSec();//采样时间
  }

  /* print record */
  std::cout << "[Topo]: sample num: " << sample_num;

  pruneGraph();//修剪节点图
  // std::cout << "[Topo]: node num: " << graph_.size() << std::endl;

  return graph_;//返回节点图
  // return searchPaths(start_node, end_node);
}

3.1 getSample()

该函数用于获取采样的节点

Eigen::Vector3d TopologyPRM::getSample() {
  //获取采样点
  /* sampling */
  Eigen::Vector3d pt;//位置
  pt(0) = rand_pos_(eng_) * sample_r_(0);//x
  pt(1) = rand_pos_(eng_) * sample_r_(1);//y
  pt(2) = rand_pos_(eng_) * sample_r_(2);//z

  pt = rotation_ * pt + translation_;//最终位置

  return pt;//返回采样节点的位置
}

3.2 dist = edt_environment_->evaluateCoarseEDT(pt, -1.0)

用于判断采样的节点是否位于障碍物中，是则continue;

3.3 vector<GraphNode::Ptr> TopologyPRM::findVisibGuard()

前采样点与任一个guard都不可见时，则把它当做新的guard添加到gragh中，如果有且只有两个可见guard，则要利用needConnection函数判断是否要添加新的connector.

vector<GraphNode::Ptr> TopologyPRM::findVisibGuard(Eigen::Vector3d pt) {
  //当一个采样点正好对两个guards可见时，创建一个新的connectors
  //发现可见的guards点
  vector<GraphNode::Ptr> visib_guards;//可见的guards点
  Eigen::Vector3d pc;

  int visib_num = 0;//可见的数目

  /* find visible GUARD from pt */
  //从pt发现可见的guard点
  for (list<GraphNode::Ptr>::iterator iter = graph_.begin(); iter != graph_.end(); ++iter) {
    //迭代
    if ((*iter)->type_ == GraphNode::Connector) continue;

    if (lineVisib(pt, (*iter)->pos_, resolution_, pc)) {
      visib_guards.push_back((*iter));
      ++visib_num;
      if (visib_num > 2) break;
    }
  }

  return visib_guards;//返回可见的guard点
}

3.3.1 不可见的判断用函数linVisb()

将两个节点连接起来，利用利用raycast步进和ESDF中的距离信息来逐步检验连线上是否有障碍物，有障碍物则者两个节点不可见。

bool TopologyPRM::lineVisib(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double thresh,
                            Eigen::Vector3d& pc, int caster_id) {
                              //linVisb（）判断两个guard之间是否可见的。
                              //即把两个节点连起来，利用raycast步进和ESDF地图来逐步检验连线上的每一个点是否是障碍物。
                             
  Eigen::Vector3d ray_pt;//投射位置
  Eigen::Vector3i pt_id;//位置索引
  double dist;//距离

  casters_[caster_id].setInput(p1 / resolution_, p2 / resolution_);//光线投射
  while (casters_[caster_id].step(ray_pt)) {
    pt_id(0) = ray_pt(0) + offset_(0);
    pt_id(1) = ray_pt(1) + offset_(1);
    pt_id(2) = ray_pt(2) + offset_(2);
    dist = edt_environment_->sdf_map_->getDistance(pt_id);//获取距离
    if (dist <= thresh) {//距离小于阈值
      edt_environment_->sdf_map_->indexToPos(pt_id, pc);
      return false;
    }
  }
  return true;
}

3.4 bool TopologyPRM::needConnection()

即判断当前路径（可见guard1,当前采样点，可见guard2）为path1，是否与（可见guard1,可见guard2,…）形成的其他路径path2同伦，如果同伦，则要判断path1是否比path2短，如果path1<path2则当前的路径更短，则替换之前两个guard的connector的位置；如果不同伦，则直接返回true。

bool TopologyPRM::needConnection(GraphNode::Ptr g1, GraphNode::Ptr g2, Eigen::Vector3d pt) {
  //是否需要连接
  //即判断当前路径（可见guard1,当前采样点，可见guard2）path1，是否与（可见guard1,可见guard2,…）形成的其他路径同伦sameTopoPath
  //如果同伦，则直接返回false.
  //如果不同伦，则要判断新的路径是否比之前的路径短，如果短，则替换之前两个guard的connector的位置并返回true,
  //将当前采样点添加为新的connector至graph中。。
  //如果更长，则直接返回fase.
  vector<Eigen::Vector3d> path1(3), path2(3);
  path1[0] = g1->pos_;//guard1的位置
  path1[1] = pt;//采样点的位置
  path1[2] = g2->pos_;//guard2的位置

  path2[0] = g1->pos_;//guard1的位置
  path2[2] = g2->pos_;//guard2的位置

  vector<Eigen::Vector3d> connect_pts;//连接点的位置
  bool has_connect = false;//初始化即两个guard点有连接为false 
  for (int i = 0; i < g1->neighbors_.size(); ++i) {
    for (int j = 0; j < g2->neighbors_.size(); ++j) {
      if (g1->neighbors_[i]->id_ == g2->neighbors_[j]->id_) {
        path2[1] = g1->neighbors_[i]->pos_;
        bool same_topo = sameTopoPath(path1, path2, 0.0);
        if (same_topo) {
          // get shorter connection ?
          if (pathLength(path1) < pathLength(path2)) {
            g1->neighbors_[i]->pos_ = pt;
            // ROS_WARN("shorter!");
          }
          return false;
        }
      }
    }
  }
  return true;
}

3.4.1 判断同伦：使用函数bool TopologyPRM::sameTopoPath()

判断两条路径是否可以连续映射，则使用LinVisb来查看两两映射的点的连接是否在障碍物中，所有点一一映射且无障碍物则为同伦。

bool TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d>& path1,
                               const vector<Eigen::Vector3d>& path2, double thresh) {
   //同样的topo路径                          
  // calc the length，计算长度
  double len1 = pathLength(path1);//路径1 的长度
  double len2 = pathLength(path2);//路径2 的长度

  double max_len = max(len1, len2);//最大的路径长度

  int pt_num = ceil(max_len / resolution_);//计算点的数目

  // std::cout << "pt num: " << pt_num << std::endl;

  vector<Eigen::Vector3d> pts1 = discretizePath(path1, pt_num);//位置1为离散的路径点
  vector<Eigen::Vector3d> pts2 = discretizePath(path2, pt_num);

  Eigen::Vector3d pc;
  for (int i = 0; i < pt_num; ++i) {
    if (!lineVisib(pts1[i], pts2[i], thresh, pc)) {//两条路径是否连续映射
      return false;
    }
  }

  return true;
}

3.5 void TopologyPRM::pruneGraph()

修剪无用的采样点，只有只有一个neighbor的guard节点。

void TopologyPRM::pruneGraph() {
  //修剪无用的节点，删去那些只有一个neighbor的guard节点。（connector不可能只有一个邻居节点）。
  /* prune useless node */
  if (graph_.size() > 2) {//节点图的节点数目>2
    for (list<GraphNode::Ptr>::iterator iter1 = graph_.begin();
         iter1 != graph_.end() && graph_.size() > 2; ++iter1) {
          //迭代
      if ((*iter1)->id_ <= 1) continue;//删去那些只有一个neighbor的guard节点

      /* core */
      // std::cout << "id: " << (*iter1)->id_ << std::endl;
      if ((*iter1)->neighbors_.size() <= 1) {
        // delete this node from others' neighbor，从其他节点的邻居中删除此节点
        for (list<GraphNode::Ptr>::iterator iter2 = graph_.begin(); iter2 != graph_.end(); ++iter2) {
          for (vector<GraphNode::Ptr>::iterator it_nb = (*iter2)->neighbors_.begin();
               it_nb != (*iter2)->neighbors_.end(); ++it_nb) {
            if ((*it_nb)->id_ == (*iter1)->id_) {
              (*iter2)->neighbors_.erase(it_nb);
              break;
            }
          }
        }

        // delete this node from graph, restart checking
        //从图中删除此节点，重新开始检查
        graph_.erase(iter1);
        iter1 = graph_.begin();
      }
    }
  }
}

4、vector<vector<Eigen::Vector3d>> TopologyPRM::searchPaths()

通过深度优先搜索算法DFS，在节点图中搜索有用的路径，按节点数目对路径进行排序，选择节点数目较少的路径进行保留，并存储在raw_paths_中。

vector<vector<Eigen::Vector3d>> TopologyPRM::searchPaths() {
  //搜索路径
  raw_paths_.clear();

  vector<GraphNode::Ptr> visited;
  visited.push_back(graph_.front());

  depthFirstSearch(visited);

  // sort the path by node number，按节点数目对路径排序
  int min_node_num = 100000, max_node_num = 1;
  vector<vector<int>> path_list(100);
  for (int i = 0; i < raw_paths_.size(); ++i) {
    if (int(raw_paths_[i].size()) > max_node_num) max_node_num = raw_paths_[i].size();
    if (int(raw_paths_[i].size()) < min_node_num) min_node_num = raw_paths_[i].size();
    path_list[int(raw_paths_[i].size())].push_back(i);
  }

  // select paths with less nodes，选择节点较少的路径
  vector<vector<Eigen::Vector3d>> filter_raw_paths;
  for (int i = min_node_num; i <= max_node_num; ++i) {
    bool reach_max = false;
    for (int j = 0; j < path_list[i].size(); ++j) {
      filter_raw_paths.push_back(raw_paths_[path_list[i][j]]);
      if (filter_raw_paths.size() >= max_raw_path2_) {
        reach_max = true;
        break;
      }
    }
    if (reach_max) break;
  }
  std::cout << ", raw path num: " << raw_paths_.size() << ", " << filter_raw_paths.size();

  raw_paths_ = filter_raw_paths;

  return raw_paths_;
}

4.1深度优先搜索算法DFS：void TopologyPRM::depthFirstSearch()

void TopologyPRM::depthFirstSearch(vector<GraphNode::Ptr>& vis) {
  //深度优先搜索
  GraphNode::Ptr cur = vis.back();

  for (int i = 0; i < cur->neighbors_.size(); ++i) {
    // check reach goal，检查目标
    if (cur->neighbors_[i]->id_ == 1) {
      // add this path to paths set，添加路径到路径集中
      vector<Eigen::Vector3d> path;
      for (int j = 0; j < vis.size(); ++j) {
        path.push_back(vis[j]->pos_);
      }
      path.push_back(cur->neighbors_[i]->pos_);

      raw_paths_.push_back(path);
      if (raw_paths_.size() >= max_raw_path_) return;

      break;
    }
  }

  for (int i = 0; i < cur->neighbors_.size(); ++i) {
    // skip reach goal，跳过目标点
    if (cur->neighbors_[i]->id_ == 1) continue;

    // skip already visited node，跳过已经访问过的节点
    bool revisit = false;
    for (int j = 0; j < vis.size(); ++j) {
      if (cur->neighbors_[i]->id_ == vis[j]->id_) {
        revisit = true;
        break;
      }
    }
    if (revisit) continue;

    // recursive search，保存搜索
    vis.push_back(cur->neighbors_[i]);
    depthFirstSearch(vis);
    if (raw_paths_.size() >= max_raw_path_) return;

    vis.pop_back();
  }
}

5、void TopologyPRM::shortcutPaths()

将上一步保留的raw_paths_路径执行shortcutPath函数，对这些路径进行缩短。

void TopologyPRM::shortcutPaths() {
  //得到经过排序筛选的原始路径后，还需要对这些路径进行缩短。
  //shortcutPaths这一函数利用利用了std::thread进行并行操作。
  //对每一条路径都执行shortcutPath函数，直到所有路径完成。即完成对所有路径的缩短
  short_paths_.resize(raw_paths_.size());

  if (parallel_shortcut_) {
    vector<thread> short_threads;
    for (int i = 0; i < raw_paths_.size(); ++i) {
      short_threads.push_back(thread(&TopologyPRM::shortcutPath, this, raw_paths_[i], i, 1));
    }
    for (int i = 0; i < raw_paths_.size(); ++i) {
      short_threads[i].join();
    }
  } else {
    for (int i = 0; i < raw_paths_.size(); ++i) shortcutPath(raw_paths_[i], i);
  }
}

5.1 void TopologyPRM::shortcutPath() 路径缩短

对于一条待缩短的路径，首先利用discretizePath函数将其从几个节点变为更稠密的一系列路径点。然后创建 short_path来存储新的缩短后的路径。对于原始上的每一个点，都与short_path的最后一个点（初始化时为原始路径的起点）连线并利用lineVisb来衡量可见性。若不可见，则将线上不可见的点往外推至一个新的位置。推的方向与连线垂直并和ESDF梯度方共面。并把新的位置点push_back进short_path中。直到结束循环把终点Push_back进short_path中。最后判断当前short_path是否比原来的路径短，若短，则取代原来的路径，若不是，则保持不变。

void TopologyPRM::shortcutPath(vector<Eigen::Vector3d> path, int path_id, int iter_num) {
  //对于一条待缩短的路径，首先利用discretizePath函数将其从几个节点变为更稠密的一系列路径点。
  //然后创建一个空的vectorEigen::Vector3d short_path来存储新的缩短后的路径。
  //对于原始上的每一个点，都与short_path的最后一个点（初始化时为原始路径的起点）连线
  //并利用lineVisb来衡量可见性。若不可见，则将线上不可见的点往外推至一个新的位置。
  //推的方向与连线垂直并和不可见点处的距离梯度方向面。
  //并把新的位置点push_back进short_path中。直到结束循环把终点Push_back进short_path中。
  //最后判断当前short_path是否比原来的路径短，若短，则取代原来的路径，若不是，则保持不变。

  vector<Eigen::Vector3d> short_path = path;//short_path来存储新的缩短后的路径
  vector<Eigen::Vector3d> last_path;

  for (int k = 0; k < iter_num; ++k) {
    last_path = short_path;//最后的路径为缩短后的路径

    vector<Eigen::Vector3d> dis_path = discretizePath(short_path);//离散化的路径

    if (dis_path.size() < 2) {
      short_paths_[path_id] = dis_path;
      return;
    }

    /* visibility path shortening */
    //可见性路径缩短
    Eigen::Vector3d colli_pt, grad, dir, push_dir;//障碍物位置、梯度、方向、推力方向
    double dist;//距离
    short_path.clear();
    short_path.push_back(dis_path.front());
    for (int i = 1; i < dis_path.size(); ++i) {
      if (lineVisib(short_path.back(), dis_path[i], resolution_, colli_pt, path_id)) continue;
      //对于离散路径上的每一个点，都与short_path的最后一个点（初始化时为原始路径的起点）连线

      edt_environment_->evaluateEDTWithGrad(colli_pt, -1, dist, grad);
      if (grad.norm() > 1e-3) {
        grad.normalize();//梯度
        dir = (dis_path[i] - short_path.back()).normalized();//方向
        push_dir = grad - grad.dot(dir) * dir;//推力方向
        //推的方向与连线垂直并和不可见点处的距离梯度方向面
        push_dir.normalize();
        colli_pt = colli_pt + resolution_ * push_dir;
      }
      short_path.push_back(colli_pt);//并把新的位置点push_back进short_path中。
      //直到结束循环把终点Push_back进short_path中。
    }
    short_path.push_back(dis_path.back());

    /* break if no shortcut */
    double len1 = pathLength(last_path);
    double len2 = pathLength(short_path);
    if (len2 > len1) {
      // ROS_WARN("pause shortcut, l1: %lf, l2: %lf, iter: %d", len1, len2, k +
      // 1);
      short_path = last_path;//最后判断当前short_path是否比原来的路径短
      //若短，则取代原来的路径，若不是，则保持不变。
      break;
    }
  }

  short_paths_[path_id] = short_path;//返回短路径
}

5.1.1 vector<Eigen::Vector3d> TopologyPRM::discretizeLine()

将整条轨迹从几个节点离散化为更稠密的一系列路径点。

vector<Eigen::Vector3d> TopologyPRM::discretizePath(vector<Eigen::Vector3d> path) {
  //离散化路径
  vector<Eigen::Vector3d> dis_path, segment;

  if (path.size() < 2) {
    ROS_ERROR("what path? ");
    return dis_path;
  }

  for (int i = 0; i < path.size() - 1; ++i) {
    segment = discretizeLine(path[i], path[i + 1]);

    if (segment.size() < 1) continue;

    dis_path.insert(dis_path.end(), segment.begin(), segment.end());
    if (i != path.size() - 2) dis_path.pop_back();
  }
  return dis_path;
}

6、vector<vector<Eigen::Vector3d>> TopologyPRM::pruneEquivalent()

剪枝等效路径，原有路径集中的每一条路径与路径集中的路径进行比较，同伦的路径进行处理，有则不加入已存在路径且删去当前的路径，不同伦则进行保留，加入路径集。

vector<vector<Eigen::Vector3d>> TopologyPRM::pruneEquivalent(vector<vector<Eigen::Vector3d>>& paths) {
  //剪枝等效路径
  //原有路径集中的每一条路径与路径集中的路径进行比较，
  //若同伦，则不加入已存在路径集且删去当前路径。若不同伦，则加入已存在路径集。
  vector<vector<Eigen::Vector3d>> pruned_paths;//剪枝路径
  if (paths.size() < 1) return pruned_paths;//如果路径数目<1 返回该路径

  /* ---------- prune topo equivalent path ---------- */
  //修剪TOPO等效路径,同伦剪枝
  // output: pruned_paths
  vector<int> exist_paths_id;//存在的路径索引
  exist_paths_id.push_back(0);

  for (int i = 1; i < paths.size(); ++i) {
    // compare with exsit paths，对比存在的路径
    bool new_path = true;

    for (int j = 0; j < exist_paths_id.size(); ++j) {
      // compare with one path
      bool same_topo = sameTopoPath(paths[i], paths[exist_paths_id[j]], 0.0);

      if (same_topo) {
        new_path = false;
        break;
      }
    }

    if (new_path) {
      exist_paths_id.push_back(i);
    }
  }

  // save pruned paths，保存剪枝的路径
  for (int i = 0; i < exist_paths_id.size(); ++i) {
    pruned_paths.push_back(paths[exist_paths_id[i]]);
  }

  std::cout << ", pruned path num: " << pruned_paths.size();

  return pruned_paths;
}

7、vector<vector<Eigen::Vector3d>> TopologyPRM::selectShortPaths()

pruneEquivalent后的路径在进行筛选，筛出路径中最短的路径，shortest_path，将路径集中的其它路径与最短路径进行比较，产生一个长度比值，该比值小于一定阈值时，将该路径进行放入新的路径集中保留，直到达到最大路径数目或循环结束。

vector<vector<Eigen::Vector3d>> TopologyPRM::selectShortPaths(vector<vector<Eigen::Vector3d>>& paths,
                                                              int step) {
  //同伦剪枝后的函数还需要再进行一次筛选。
  //首先筛出所有路径中的最短路径。其次，循环地将除去最短路径集的剩余的路径中的最短路径与真正的最短路径进行比较，
  //若长度比值小于一定阈值，则可以将其加入新的路径集。直到达到最大路径数量或者循环结束。                                                             
  /* ---------- only reserve top short path ---------- */
  //仅仅保留最短的路径
  vector<vector<Eigen::Vector3d>> short_paths;//短的路径(多条)
  vector<Eigen::Vector3d> short_path;//（最短的路径一条）
  double min_len;//最小的长度

  for (int i = 0; i < reserve_num_ && paths.size() > 0; ++i) {
    //直到达到最大路径数量或者循环结束
    int path_id = shortestPath(paths);//路径索引
    if (i == 0) {
      short_paths.push_back(paths[path_id]);//短路径集
      min_len = pathLength(paths[path_id]);//最小长度
      paths.erase(paths.begin() + path_id);//删除路径
    } else {
      double rat = pathLength(paths[path_id]) / min_len;//路径长度比值
      if (rat < ratio_to_short_) {//长度比值小于一定阈值，则可以将其加入新的路径集
        short_paths.push_back(paths[path_id]);
        paths.erase(paths.begin() + path_id);
      } else {
        break;
      }
    }
  }
  std::cout << ", select path num: " << short_paths.size();

  /* ---------- merge with start and end segment ---------- */
  for (int i = 0; i < short_paths.size(); ++i) {
    short_paths[i].insert(short_paths[i].begin(), start_pts_.begin(), start_pts_.end());
    short_paths[i].insert(short_paths[i].end(), end_pts_.begin(), end_pts_.end());
  }
  for (int i = 0; i < short_paths.size(); ++i) {
    shortcutPath(short_paths[i], i, 5);
    short_paths[i] = short_paths_[i];
  }

  short_paths = pruneEquivalent(short_paths);//并再做一遍shortcutPath和pruneEquivalent得到最终路径。

  return short_paths;
}