算法1 Polygon Extraction（3）

上回讲到得到contour_gtaph ，这里面是一组所有障碍物的点，点与点之前通过属性front和back来确定是否是同一个polygon

this->AnalysisSurfAngleAndConvexity(ContourGraph::contour_graph_);

接着我们来看这个这个是来判断SurfAngle和多边形的顶点凹凸性的这里好懵我看的。我们来具体看一下函数。

void ContourGraph::AnalysisSurfAngleAndConvexity(const CTNodeStack& contour_graph) {for (const auto& ctnode_ptr : contour_graph) {//是pillar类型的 咱就不看了~if (ctnode_ptr->free_direct == NodeFreeDirect::PILLAR || ctnode_ptr->poly_ptr->is_pillar) {ctnode_ptr->surf_dirs = {Point3D(0,0,-1), Point3D(0,0,-1)};ctnode_ptr->poly_ptr->is_pillar = true;ctnode_ptr->free_direct = NodeFreeDirect::PILLAR;}//不是pillar类型的 我们就来瞅瞅 else {CTNodePtr next_ctnode;  //点 指针// front directionnext_ctnode = ctnode_ptr->front;Point3D start_p = ctnode_ptr->position;Point3D end_p = next_ctnode->position;float edist = (end_p - ctnode_ptr->position).norm_flat();  //向量的模把？ 两个坐标点相减  得到向量 然后再过一个hypotf//kNavClearDist  0.55while (next_ctnode != NULL && next_ctnode != ctnode_ptr && edist < FARUtil::kNavClearDist) {next_ctnode = next_ctnode->front;start_p = end_p;end_p = next_ctnode->position;edist = (end_p - ctnode_ptr->position).norm_flat();}if (edist < FARUtil::kNavClearDist) { // This Node should be a pillar.ctnode_ptr->surf_dirs = {Point3D(0,0,-1), Point3D(0,0,-1)};ctnode_ptr->poly_ptr->is_pillar = true;ctnode_ptr->free_direct = NodeFreeDirect::PILLAR;continue;} else {//start_p 和 end_p一定不一样， 但ctnode_ptr可能和他们一样，也可能不一样//! surfdirs返回的是一个 方向向量ctnode_ptr->surf_dirs.first = FARUtil::ContourSurfDirs(end_p, start_p, ctnode_ptr->position, FARUtil::kNavClearDist);}// back directionnext_ctnode = ctnode_ptr->back;start_p = ctnode_ptr->position;end_p   = next_ctnode->position;edist = (end_p - ctnode_ptr->position).norm_flat();while (next_ctnode != NULL && next_ctnode != ctnode_ptr && edist < FARUtil::kNavClearDist) {next_ctnode = next_ctnode->back;start_p = end_p;end_p = next_ctnode->position;edist = (end_p - ctnode_ptr->position).norm_flat();}if (edist < FARUtil::kNavClearDist) { // This Node should be a pillar.ctnode_ptr->surf_dirs = {Point3D(0,0,-1), Point3D(0,0,-1)}; // TODO!ctnode_ptr->poly_ptr->is_pillar = true;ctnode_ptr->free_direct = NodeFreeDirect::PILLAR;continue;} else {ctnode_ptr->surf_dirs.second = FARUtil::ContourSurfDirs(end_p, start_p, ctnode_ptr->position, FARUtil::kNavClearDist);}}//! 从上面可以得到当前点的 “点对” surf_dirs<first,second>// analysis convexity (except pillar)this->AnalysisConvexityOfCTNode(ctnode_ptr);    //这个就是给每个ctnode赋予属性  UNKNOW/PILLAR/CONVEX/CONCAVE}
}

对于contour_graph里的每一个点，我们都要进行判断，首先看他是不是pillar类型，如果是的画，直接把ctnode的属性赋值好就完事。

如果不是pillar类型，我们就进行计算它的两个Surf_dir 我个人认为是这个点的向量。

// front directionnext_ctnode = ctnode_ptr->front;Point3D start_p = ctnode_ptr->position;Point3D end_p = next_ctnode->position;float edist = (end_p - ctnode_ptr->position).norm_flat();  //向量的模把？ 两个坐标点相减  得到向量 然后再过一个hypotf//kNavClearDist  0.55while (next_ctnode != NULL && next_ctnode != ctnode_ptr && edist < FARUtil::kNavClearDist) {next_ctnode = next_ctnode->front;start_p = end_p;end_p = next_ctnode->position;edist = (end_p - ctnode_ptr->position).norm_flat();}if (edist < FARUtil::kNavClearDist) { // This Node should be a pillar.ctnode_ptr->surf_dirs = {Point3D(0,0,-1), Point3D(0,0,-1)};ctnode_ptr->poly_ptr->is_pillar = true;ctnode_ptr->free_direct = NodeFreeDirect::PILLAR;continue;} else {//start_p 和 end_p一定不一样， 但ctnode_ptr可能和他们一样，也可能不一样//! surfdirs返回的是一个 方向向量ctnode_ptr->surf_dirs.first = FARUtil::ContourSurfDirs(end_p, start_p, ctnode_ptr->position, FARUtil::kNavClearDist);}

首先是前向搜索，比如一组有p0,p1,p2，也就是一个三角形，那么我们遍历的时候，ctnode_ptr就先是p0 front方向就是p2，依次类推，中间有个while循环判断两点的长度满不满足要求，如果不满足，它的start就会往front方向迭代，同理end也会迭代。

然后看：

ctnode_ptr->surf_dirs.first = FARUtil::ContourSurfDirs(end_p, start_p, ctnode_ptr->position, FARUtil::kNavClearDist);

// SurfDir都是控制在-1到1之间的数字 应该是单位向量
Point3D FARUtil::ContourSurfDirs(const Point3D& end_p, const Point3D& start_p, const Point3D& center_p,const float& radius)
{const float D = (center_p - end_p).norm_flat(); //D就是center_p到end的距离const float phi = std::acos((center_p - end_p).norm_flat_dot(start_p - end_p)); //出来一个角度(单位是rad)const float H = D * sin(phi);if (H < FARUtil::kEpsilon) {  // co-linearreturn (end_p - center_p).normalize_flat();}const float theta = asin(FARUtil::ClampAbsRange(H / radius, 1.0f));// 如果H大于0.55 那么theta = 57.29const Point3D dir = (start_p - end_p).normalize_flat();const Point3D V_p = end_p + dir * D * cos(phi); //end_p + end_p->center_pconst Point3D K_p = V_p - dir * radius * cos(theta);  return (K_p - center_p).normalize_flat();;
}

这个程序就是输出一个单位向量，代表当前的ctnode_ptr的一个front方向的方向向量，同理也有一个back方向的这两个方向向量组成一个点对surf_dirs<first,second>

举个栗子

我们以p0(0,0),p1(1.2,0),p2(0.3,0.3)为例，构成一个三角形。

front方向：在计算ctnode_ptr为(0,0)的时候center_p = (0,0)，首先start_p = center_p，end_p = ctnode_ptr->front 也就是(0.3,0.3)但是end_p和ctnode_ptr 的距离为0.42 小于阈值（这里为了方便就用了far_planner的阈值 0.55）。

所以往前迭代一次 center_p = (0,0)，start_p （0.3，0.3），end_p（1.2，0）这时候满足组要求了，edist = 1.2>0.55 然后进入ContourSurfDirs的计算。

首先先算D （end_p到center_p的距离）D=1.2

然后算出夹角phi =0.321751弧度（角度约为18°左右）

接着算H 也就是三角形以end_p为底的高，H=0.379

判断是否共线，如果H几乎为0 那么就说明他俩共线。共线的话就返回

(end_p - center_p).normalize_flat()

如果不共线，那接着算：

算从 end_p到start_p 的一个单位向量 dir、向量V_p

接着再算K_p

最后得到该点center_p(0,0）的front方向的方向向量（K_p - center_p）.normalize_flat()

同理，可以得到这个点center_p(0,0）的back方向的方向向量

通常情况下，surf_dir.first指向的都是它的上一个（front) 的单位向量；而surf_dir.second指向的都是它的下一个（back）的单位向量。比如下图所示

Far_planner 代码系列（11）相关推荐

SAP PM 初级系列11 - 为维修工单触发采购申请
SAP PM 初级系列11 - 为维修工单触发采购申请执行事务代码IW32,输入维修工单号,进入维修工单的修改界面,进入components选项卡,可以为维修工单增加备品备件(non-stock i ...
SAP PM 入门系列11 - 一个维护通知单只能创建一个维护订单？
SAP PM 入门系列11 - 一个维护通知单只能创建一个维护订单? 在SAP系统里,执行事务代码IW34,输入Notification号码100314924,以及Order type ZM03,试图 ...
深入学习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) ...
深入学习SAP UI5框架代码系列之三：HTML原生事件 VS UI5 Semantic事件
这是Jerry 2020年的第80篇文章,也是汪子熙公众号总共第262篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...
深入学习SAP UI5框架代码系列之二：UI5 控件的渲染器
这是Jerry 2020年的第79篇文章,也是汪子熙公众号总共第261篇原创文章. 系列目录 (0) SAP UI5应用开发人员了解UI5框架代码的意义 (1) UI5 module懒加载机制 (2) ...
深入学习SAP UI5框架代码系列之一：UI5 Module的懒加载机制
本文是深入学习SAP UI5框架代码系列的第二篇文章. 系列目录 SAP UI5应用开发人员了解UI5框架代码的意义 UI5 module懒加载机制 UI5 控件渲染机制 HTML原生事件 VS SA ...

Far_planner 代码系列（11）

算法1 Polygon Extraction（3）

举个栗子

Far_planner 代码系列（11）相关推荐

最新文章

热门文章