Far planner 代码系列（33）关于real_world_contour和contour

我们知道far planner前部分的流程是这样子的：

对于特定的地图，如下：

我们要从surround_obs_cloud里抽取生成realworld_contour_，如下图所示：

在这时候，每个node的z坐标都是以机器人的高度定的。如图所示：

然后我们用real_world_contour去更新我们的contour_graph，对于PILLAR的点，它用取均值去替代。

如图所示，深紫色的点为通过real_world_contour去更新得到的contour_graph，其中淡蓝色的点是之前real_world_contour内的点，他们被标记为pillar类型，所以通过mean_p去合成标记一个紫色的点。

对于有完整的一组polygon的点，我们还要把它的第一个点放入poly_ctnodes列表里，也就是图中淡紫色的两个点，我们可以通过该node的front方向和back方向去遍历该polygon里的每一个点。

之后，我们得到contour_graph，我们还需要对它每个node里的z坐标进行还原。我们取机器人的z值，在加上一个辅助的限高值kTolerZ，得到h_min和h_max：

1、首先判断该点是不是属于terrain_cloud里的点，若用kdtree查找返回的点的个数大于0，则表示该点是属于terrain_cloud的点，标识符is_ground_associate为true，并更新返回平均高度

2、若不属于terrain_cloud，则直接返回该点的z值

3、根据标识符is_ground_associate进行判断

(1)is_ground_associate为true，按kdtree查找的点云的minH和小车的高度去更新z值，

并判断该高度在不在h_min和h_max的范围内

(2)is_ground_associate为false，把该点变成(x,y,0)，然后在terrain_height_grid_下进

行一个从pos2sub的变换，得到sub，判断sub在不在terrain_height_grid_的subs范围内，如

果在的换，就用sub2ind转换成对应的ind。判断该ind在terrain_grid_traverse_list_上是否被探

索过，如果被探索过，就根据cell[ind]取出里面存放的第一个z值，并把is_ground_associate

标记为true。

如果该ind不在terrain_height_grid_范围内，但是is_search标记为true，那么我们就在

kdtree_terrain_clould里找离该ctnode最近的一个node，并用它的高度去代替该ctnode的高

度，完事以后我们要加上小车本身的高度，得到该ctnode的最终高度，并且要判断该高度在

不在h_min和h_max的范围内

Far planner 代码系列（33）关于real_world_contour和contour_graph相关推荐

Far planner代码系列（1）
读了大神的论文以后自己动手跑了一下,感觉很有趣,想细读一下大神的代码,反正当作一个自娱自乐的系列好了~MichaelFYang/far_planner: Fast, Attemptable Route ...
Far planner代码系列（2）
graph_decoder系列(2) 承接上文我们接着来看decoder_node.cpp的代码部分 void GraphDecoder::Init() {/* initialize subscri ...
Far planner代码系列（31）is_covered
今天讲的是UpdateNavGraph函数里的分析每个node的is_covered属性和is_frontier属性. 我们看代码,代码里把near_nav_nodes里的点挨个都取出来判断该node ...
SAP PM入门系列33 - IP16 维修计划报表
SAP PM入门系列33 - IP16 维修计划报表对于维修计划,SAP PM模块也提供了标准查询报表IP16,方便业务人员根据需要对维修计划做查询. 执行事务代码IP16, 进入如下界面, 输入相 ...
深入学习SAP UI5框架代码系列之八：谈谈 SAP UI5 的视图控件 ID，以及 SAP UI5 视图和 Angular 视图的异同
今天是 2021 年 4 月 27 日,周二,SAP 全球心理健康日.SAP 全球的员工,今天放假一天. 这不,早在上周五,我所在的 SAP Spartacus 开发团队的开发经理,就贴心地在 Sla ...
深入学习SAP UI5框架代码系列之七：控件数据绑定的三种模式 - One Way, Two Way和OneTime实现原理比较
这是Jerry 2021年的第 8 篇文章,也是汪子熙公众号总共第 279 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...
深入学习SAP UI5框架代码系列之六：SAP UI5控件数据绑定的实现原理
这是Jerry 2021年的第 7 篇文章,也是汪子熙公众号总共第 278 篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加 ...
深入学习SAP UI5框架代码系列之五：SAP UI5控件的实例数据修改和读取逻辑
这是Jerry 2021年的第6篇文章,也是汪子熙公众号总共第277篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) SAP UI5 module懒加载机制 ...
深入学习SAP UI5框架代码系列之四：SAP UI5控件的元数据实现
这是Jerry 2021年的第5篇文章,也是汪子熙公众号总共第276篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...

Far planner 代码系列（33）关于real_world_contour和contour_graph

Far planner 代码系列（33）关于real_world_contour和contour_graph相关推荐

最新文章

热门文章

Far planner 代码系列（33） 关于real_world_contour和contour_graph

Far planner 代码系列（33） 关于real_world_contour和contour_graph相关推荐

最新文章

热门文章

Far planner 代码系列（33）关于real_world_contour和contour_graph

Far planner 代码系列（33）关于real_world_contour和contour_graph相关推荐