Monodyee

这个屌丝很懒，什么也没留下！

热门标签

【读代码】fast-planner-PRM算法解析-topo_prm.cpp_topologyprm

作者：Monodyee | 2024-04-20 18:59:09

踩

topologyprm

1. 初始化函数（TopologyPRM::init）

初始化参数值

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);
  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);
  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());
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

2. TopologyPRM::findTopoPaths

对路径搜索，修剪，筛选，并计算每一步消耗时间。
输入参数：起始点和终点，流程如下图所示：
在这里插入图片描述

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) {
  ros::Time t1, t2;

  double graph_time, search_time, short_time, prune_time, select_time;
//       生成节点时间  路径搜索时间 路径最短时间 路径修剪时间 选择路径用时
  /* ---------- create the topo graph ---------- */
  t1 = ros::Time::now();

  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_
  t1 = ros::Time::now();
  // 路径修剪
  shortcutPaths();

  short_time = (ros::Time::now() - t1).toSec();

  /* ---------- prune equivalent paths ---------- */
  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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

2.1采样节点：TopologyPRM::createGraph(Eigen::Vector3d start, Eigen::Vector3d end)

根据起始点和终点的位置信息，采样地图节点

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);  //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();
  /**  函数2.1.1   **/
    pt = getSample();
    ++sample_num;
    double dist;      
    Eigen::Vector3d grad;    //梯度
    // edt_environment_->evaluateEDTWithGrad(pt, -1.0, dist, grad);
    /**  函数2.1.2   **/
    dist = edt_environment_->evaluateCoarseEDT(pt, -1.0);      //距离esdf地图最近的障碍物的距离
    if (dist <= clearance_) {                      //最小距离小于安全距离
      sample_time += (ros::Time::now() - t1).toSec();
      continue;
    }

    /* find visible guard */
      // 查找梯度点
      /**   函数2.1.3      **/
    vector<GraphNode::Ptr> visib_guards = findVisibGuard(pt);
    if (visib_guards.size() == 0) {
      GraphNode::Ptr guard = GraphNode::Ptr(new GraphNode(pt, GraphNode::Guard, ++node_id));
      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);
        // 在两点之间添加新的节点
        /**  函数2.1.4   **/
      bool need_connect = needConnection(visib_guards[0], visib_guards[1], pt);
      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
      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);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96

2.1.1 getsample()

通过平移和旋转得到采样节点

/***
返回参数pt  x y z
*/

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;//返回采样节点的位置
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

2.1.2 evaluateCoarseEDT(Eigen::Vector3d& pos, double time)

判断采样得点是否在障碍物上，并返回最小距离

double EDTEnvironment::evaluateCoarseEDT(Eigen::Vector3d& pos, double time) {
  double d1 = sdf_map_->getDistance(pos);
  if (time < 0.0) {
    return d1;
  } else {
    double d2 = minDistToAllBox(pos, time);
    return min(d1, d2);
  }
}

1
2
3
4
5
6
7
8
9
10

2.1.3 TopologyPRM::findVisibGuard(Eigen::Vector3d pt)

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

vector<GraphNode::Ptr> TopologyPRM::findVisibGuard(Eigen::Vector3d pt) {
  vector<GraphNode::Ptr> visib_guards;
  Eigen::Vector3d pc;

  int visib_num = 0;

  /* find visible GUARD from pt */
  for (list<GraphNode::Ptr>::iterator iter = graph_.begin(); iter != graph_.end(); ++iter) {
    if ((*iter)->type_ == GraphNode::Connector) continue;
//   根据pt得到guard 点
    if (lineVisib(pt, (*iter)->pos_, resolution_, pc)) {
      visib_guards.push_back((*iter));
      ++visib_num;
      if (visib_num > 2) break;
    }
  }

  return visib_guards;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

2.1.3.1 TopologyPRM::lineVisib(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double thresh,Eigen::Vector3d& pc, int caster_id)

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

bool TopologyPRM::lineVisib(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2, double thresh,
                            Eigen::Vector3d& pc, int caster_id) {
  Eigen::Vector3d ray_pt;
  Eigen::Vector3i pt_id;    //位置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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

2.1.4 TopologyPRM::needConnection()

即判断当前路径path1与path2长度关系，则要判断path1是否比path2短，则替换之前两个guard的connector的位置，返回true。否则返回false。

bool TopologyPRM::needConnection(GraphNode::Ptr g1, GraphNode::Ptr g2, Eigen::Vector3d pt) {
  vector<Eigen::Vector3d> path1(3), path2(3);
  path1[0] = g1->pos_;
  path1[1] = pt;
  path1[2] = g2->pos_;

  path2[0] = g1->pos_;
  path2[2] = g2->pos_;

  vector<Eigen::Vector3d> connect_pts;    // 连接点得位置
  bool has_connect = 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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

2.1.4.1 TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d >& path1, const vector<Eigen::Vector3d >& path2, double thresh)

比较两条路径cost是否相同，是否有障碍物

bool TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d>& path1,
                               const vector<Eigen::Vector3d>& path2, double thresh) {
  // calc the length
  double len1 = pathLength(path1);
  double len2 = pathLength(path2);

  double max_len = max(len1, len2);

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

  // std::cout << "pt num: " << pt_num << std::endl;
  //    离散path的路径点
  vector<Eigen::Vector3d> pts1 = discretizePath(path1, pt_num);
  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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

2.1.4.2 TopologyPRM::discretizePath(const vector<Eigen::Vector3d >& path, int pt_num)

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

vector<Eigen::Vector3d> TopologyPRM::discretizePath(const vector<Eigen::Vector3d>& path, int pt_num) {
  vector<double> len_list;
  len_list.push_back(0.0);
// 计算路径点之间的距离
  for (int i = 0; i < path.size() - 1; ++i) {
    double inc_l = (path[i + 1] - path[i]).norm();
    len_list.push_back(inc_l + len_list[i]);
  }

  // calc pt_num points along the path
  double len_total = len_list.back();
// 每段轨迹距离
  double dl = len_total / double(pt_num - 1);
  double cur_l;

  vector<Eigen::Vector3d> dis_path;
  for (int i = 0; i < pt_num; ++i) {
    cur_l = double(i) * dl;

    // find the range cur_l in
    int idx = -1;
    for (int j = 0; j < len_list.size() - 1; ++j) {
      if (cur_l >= len_list[j] - 1e-4 && cur_l <= len_list[j + 1] + 1e-4) {
        idx = j;
        break;
      }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

2.1.5 搜索路径 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_;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

2.1.4.1TopologyPRM::depthFirstSearch(vector<GraphNode::Ptr >& vis)

得到深度优先算法的路径

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();
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

2.1.6 路径裁剪shotcutPaths()

void TopologyPRM::shortcutPaths() {
  short_paths_.resize(raw_paths_.size());

  if (parallel_shortcut_) {
    vector<thread> short_threads;
    for (int i = 0; i < raw_paths_.size(); ++i) {
        //多线程运行shortcutPath                    调用函数      参数        
      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);
  }
} 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

2.1.6.1 void TopologyPRM::shortcutPath(vectorEigen::Vector3d path, int path_id, int iter_num)

// 路径路径id
缩短路径。对于原始上的每一个点，都与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) {
  vector<Eigen::Vector3d> short_path = path;
  vector<Eigen::Vector3d> last_path;

  for (int k = 0; k < iter_num; ++k) {
    last_path = short_path;   //将last_path改为最短的路径
	// 将路径稠密化
    vector<Eigen::Vector3d> dis_path = discretizePath(short_path);

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

    /* visibility path shortening */
      //             障碍物id  梯度 方向 推力方向
    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) {
        //利用lineVisb来衡量可见性。若不可见，则将线上不可见的点往外推至一个新的位置。
      if (lineVisib(short_path.back(), dis_path[i], resolution_, colli_pt, path_id)) continue;
	// 梯度
      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);
    }
    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;
      break;
    }
  }

  short_paths_[path_id] = short_path;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

2.1.7 TopologyPRM::pruneEquivalent(vector<vector<Eigen::Vector3d >>& paths)

原有路径集中的每一条路径与添加路径进行比较。

vector<vector<Eigen::Vector3d>> TopologyPRM::pruneEquivalent(vector<vector<Eigen::Vector3d>>& paths) {
  vector<vector<Eigen::Vector3d>> pruned_paths;
  if (paths.size() < 1) return pruned_paths;

  /* ---------- prune topo equivalent path ---------- */
  // 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);
	// 添加路径与之前路径一样，则为false
      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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

2.1.8 sameTopoPath()

两条路径通过稠密处理，比较是否相同，如果相同则输出false，否则为true

bool TopologyPRM::sameTopoPath(const vector<Eigen::Vector3d>& path1,
                               const vector<Eigen::Vector3d>& path2, double thresh) {
  // calc the length
  double len1 = pathLength(path1);
  double len2 = pathLength(path2);

  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);
  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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

2.1.9 7. selectShortPaths()

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

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];
  }
//并再做一遍shortcutPath和pruneEquivalent得到最终路径。
  short_paths = pruneEquivalent(short_paths);

  return short_paths;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

3参考资料

代码
 代码解析

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Monodyee/article/detail/458877