FARPlanner之ContourGraph管理与更新

最新推荐文章于 2025-12-07 21:20:15 发布

原创最新推荐文章于 2025-12-07 21:20:15 发布 · 865 阅读

12 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #机器人 #自动驾驶 #图像处理

机器人&自动驾驶专栏收录该内容

4 篇文章

订阅专栏

0. 变量定义

public:
    static CTNodeStack  contour_graph_;// 这个是局部的 每次都被清空
    static std::vector<PointPair> global_contour_; // 每次也被清空
    static std::vector<PointPair> inactive_contour_;
    static std::vector<PointPair> unmatched_contour_;//这几个是在ExtractGlobalContours()
    static std::vector<PointPair> boundary_contour_;
    static std::vector<PointPair> local_boundary_;
private:
    static CTNodeStack polys_ctnodes_;
    static PolygonStack contour_polygons_;
    ContourGraphParams ctgraph_params_;
    float ALIGN_ANGLE_COS;
    NavNodePtr odom_node_ptr_ = NULL;
    bool is_robot_inside_poly_ = false;

   //global contour set
    static std::unordered_set<NavEdge, navedge_hash> global_contour_set_;
    static std::unordered_set<NavEdge, navedge_hash> boundary_contour_set_;

1. UpdateContourGraph（）

UpdateContourGraph
处理对象：odom_node_ptr filtered_contours（局部多边形轮廓）
处理逻辑：

更新odom
ClearContourGraph：
contour_graph_
contour_polygons_
polys_ctnodes_
添加多边形：将从图片提取的提取的多边形轮廓更新到ContourGraph::contour_polygons_中
并更新多边形对应属性:N vertices is_robot_inside is_pillar（是否小到可以忽略形状）perimeter（周长））
CreatePolygon
AddPolyToContourPolygon
更新FARUtil::free_odom_p
更新CTNode属性:对ContourGraph::contour_polygons_中的多边形顶点更新对应属性：
根据更新后的odom位置更新多边形is_robot_inside属性
对pillar 非pillar多边形分别更新CTNode：
！position
？is_global_match
？is_contour_necessary
？is_ground_associate
？nav_node_id
！？free_direct
！poly_ptr
！front
！back
？connect_nodes
注意这里更新的属性都是跟多边形本身轮廓形状相关的
c. 将更新后的CTNode添加到contour_graph_中
更新polys_ctnodes_:add first ctnode of each polygon to poly ctnodes stack(ContourGraph::polys_ctnodes_)
更新contour_graph_中CTNode属性：
！position
？is_global_match
？is_contour_necessary
？is_ground_associate
？nav_node_id
！free_direct
！surf_dirs(first second)---->front 方向和back方向的单位向量（可能存在特殊情况，没细看是啥样的）
！poly_ptr（按照节点更新is_pillar）
！front
！back
？connect_nodes
注意这里的is_pillar分为多边形和边两种

void ContourGraph::UpdateContourGraph(const NavNodePtr& odom_node_ptr,
                                      const std::vector<std::vector<Point3D>>& filtered_contours) {
    odom_node_ptr_ = odom_node_ptr;
    this->ClearContourGraph();
    for (const auto& poly : filtered_contours) {
        PolygonPtr new_poly_ptr = NULL;
        this->CreatePolygon(poly, new_poly_ptr);
        this->AddPolyToContourPolygon(new_poly_ptr);
    }
    ContourGraph::UpdateOdomFreePosition(odom_node_ptr_, FARUtil::free_odom_p);
    for (const auto& poly_ptr : ContourGraph::contour_polygons_) {
        poly_ptr->is_robot_inside = FARUtil::PointInsideAPoly(poly_ptr->vertices, FARUtil::free_odom_p);
        CTNodePtr new_ctnode_ptr = NULL;
        if (poly_ptr->is_pillar) {
            Point3D mean_p = FARUtil::AveragePoints(poly_ptr->vertices);
            this->CreateCTNode(mean_p, new_ctnode_ptr, poly_ptr, true);
            this->AddCTNodeToGraph(new_ctnode_ptr);
        } else {
            CTNodeStack ctnode_stack;
            ctnode_stack.clear();
            const int N = poly_ptr->vertices.size();
            for (std::size_t idx=0; idx<N; idx++) {
                this->CreateCTNode(poly_ptr->vertices[idx], new_ctnode_ptr, poly_ptr, false);
                ctnode_stack.push_back(new_ctnode_ptr);
            }
            // add connections to contour nodes
            for (int idx=0; idx<N; idx++) {
                int ref_idx = FARUtil::Mod(idx-1, N);
                ctnode_stack[idx]->front = ctnode_stack[ref_idx];
                ref_idx = FARUtil::Mod(idx+1, N);
                ctnode_stack[idx]->back = ctnode_stack[ref_idx];
                this->AddCTNodeToGraph(ctnode_stack[idx]);
            }
            // add first ctnode of each polygon to poly ctnodes stack
            if (!ctnode_stack.empty()) ContourGraph::polys_ctnodes_.push_back(ctnode_stack.front());
        }
    }
    this->AnalysisSurfAngleAndConvexity(ContourGraph::contour_graph_);      
}

1.1. AnalysisSurfAngleAndConvexity

surf_dirs计算逻辑：

边长都大于cleardist，表示的是当前节点与前后节点的方向向量
若前后节点中对应的边长存在小于cleardist的情况：
找到当前节点沿着对应方
向（front/back）最早满足边长>=cleardist的边
在该边对应的直线上找到与当前节点距离等于cleardist的点
返回当前节点到该节点的方向向量
free_direct
先看是不是pillar
再看是不是wall 若为wall 则为unkown (默认都是unknown，这里为啥单独判断呢？)
都不是再判断凹凸性：根据当前节点向着其拓扑方向（前后方向合成的向量）移动一点点后，是在多边形内部还是外部来判断（若在内部则为凸）
若机器人不在该多边形内部，凸就是凸凹就是凹
若机器人在该多边形内部，凹就是凸凸就是凹

在这里插入图片描述

/* 若当前节点的surf_dirs 对应的两个方向相反 即反向共线 则该is_wall为true 否则返回其拓扑方向*/
Point3D FARUtil::SurfTopoDirect(const PointPair& dirs, bool& _is_wall) {
  const Point3D topo_dir = dirs.first + dirs.second;
  _is_wall = false;
  if (topo_dir.norm_flat() > FARUtil::kEpsilon) {
    return topo_dir.normalize_flat();
  } else {
    _is_wall = true;
    return Point3D(0,0,0);
  }
}

void ContourGraph::AnalysisSurfAngleAndConvexity(const CTNodeStack& contour_graph) {
    for (const auto& ctnode_ptr : contour_graph) {
        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;
        } else {
            CTNodePtr next_ctnode;
            // front direction
            next_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();
            while (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 {
                ctnode_ptr->surf_dirs.first = FARUtil::ContourSurfDirs(end_p, start_p, ctnode_ptr->position, FARUtil::kNavClearDist);
            }
            // back direction
            next_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);
            }
        }
        // analysis convexity (except pillar)
        this->AnalysisConvexityOfCTNode(ctnode_ptr);
    }
}

1.2. ContourSurfDirs

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();
  const float phi = std::acos((center_p - end_p).norm_flat_dot(start_p - end_p));
  const float H = D * sin(phi);
  if (H < FARUtil::kEpsilon) { // co-linear
    return (end_p - center_p).normalize_flat();
  }
  const float theta = asin(FARUtil::ClampAbsRange(H / radius, 1.0f));//theta是被跳过的节点与当前节点
  const Point3D dir = (start_p - end_p).normalize_flat();
  const Point3D V_p = end_p + dir * D * cos(phi);
  const Point3D K_p = V_p - dir * radius * cos(theta);
  return (K_p - center_p).normalize_flat();;
}

1.3 AnalysisConvexityOfCTNode

void ContourGraph::AnalysisConvexityOfCTNode(const CTNodePtr& ctnode_ptr) {
    if (ctnode_ptr->surf_dirs.first  == Point3D(0,0,-1) || ctnode_ptr->surf_dirs.second == Point3D(0,0,-1) || ctnode_ptr->poly_ptr->is_pillar) {
        ctnode_ptr->surf_dirs.first = Point3D(0,0,-1), ctnode_ptr->surf_dirs.second == Point3D(0,0,-1);
        ctnode_ptr->poly_ptr->is_pillar = true;
        ctnode_ptr->free_direct = NodeFreeDirect::PILLAR;
        return;
    }
    bool is_wall = false;
    const Point3D topo_dir = FARUtil::SurfTopoDirect(ctnode_ptr->surf_dirs, is_wall);
    if (is_wall) {
        ctnode_ptr->free_direct = NodeFreeDirect::UNKNOW;
        return;
    }
    const Point3D ev_p = ctnode_ptr->position + topo_dir * FARUtil::kLeafSize;
    if (FARUtil::IsConvexPoint(ctnode_ptr->poly_ptr, ev_p)) {
        ctnode_ptr->free_direct = NodeFreeDirect::CONVEX;
    } else {
        ctnode_ptr->free_direct = NodeFreeDirect::CONCAVE;
    }
}

2. MatchContourWithNavGraph（）

注意：这里的ContourGraph::contour_graph_中是添加了高度的~~~在轮廓匹配的时候还有没有高度哦
MatchContourWithNavGraph：Match current contour with global navigation nodes（near nav nodes）
处理对象：global_nodes（DynamicGraph::globalGraphNodes_）,near_nodes(DynamicGraph::extend_match_nodes_), new_convex_vertices
处理逻辑：

重置global_nodes中的CTNode匹配属性
对当前所有CTNode（存放于ContourGraph::contour_graph_）在near_nodes中找到与其最匹配的NavNode节点（距离）
对匹配节点进行校验（方向？？再确认）
更新通过校验的CTNode与NavNode的匹配关系（对应属性更新）
根据匹配后的CTNode对未匹配到的CTNode进行多边形校验：
是否存在block_vertex（只有这样的节点才有is_contour_necessary），
若存在是否为必要点（更新对应节点属性）
清空new_convex_vertices
添加没有匹配到全局且free_direct属性非未知且非pillar的非wall节点到new_convex_vertices中
注意：
CTNode与待匹配NavNode顶点所在多边形顶点的角平分线方向夹角角度偏差60度范围内且两者距离在2倍车身距离内，目前找到的角度和距离最接近的点
如果当前的点没有之前最优的点好，则返回空~~~~表示当前匹配失败
匹配失败的点，会被判断是否必要点添加没有匹配到全局且free_direct属性非未知且非pillar的非wall节点到待更新节点
匹配失败但是必要节点的CTNode会被标记为is_block_frontier

调用：
// Match near nav nodes with contour
contour_graph_.MatchContourWithNavGraph(nav_graph_, near_nav_graph_, new_ctnodes_);
nav_graph_对应的是 DynamicGraph::globalGraphNodes_ ，是所有的NavNode?

/* Match current contour with global navigation nodes */
void ContourGraph::MatchContourWithNavGraph(const NodePtrStack& global_nodes, const NodePtrStack& near_nodes, CTNodeStack& new_convex_vertices) {
    // global_nodes 中所有NavNode中与CTNode匹配信息重置为默认值
    // 每次更新都要重置 匹配是动态更新的（局部是跟随当前位置变化的）
    for (const auto& node_ptr : global_nodes) {
        node_ptr->is_contour_match = false;
        node_ptr->ctnode = NULL;
    }
    //对当前得到的CTNode节点
    //1. 全局匹配属性重置为默认值
    //2. 对节点free_direct属性不为UNKNOW的节点，在near_nodes中找到其matched_node
    //3. 若matched_node不为空且IsCTMatchLineFreePolygon（参考具体操作 这里有点没太明白~~~），
    // 则建立该CTNode与matched_node(NavNode)之间的匹配关系
    for (const auto& ctnode_ptr : ContourGraph::contour_graph_) { // distance match
        ctnode_ptr->is_global_match = false;
        ctnode_ptr->nav_node_id = 0;
        if (ctnode_ptr->free_direct != NodeFreeDirect::UNKNOW) {
            const NavNodePtr matched_node = this->NearestNavNodeForCTNode(ctnode_ptr, near_nodes);
            if (matched_node != NULL && IsCTMatchLineFreePolygon(ctnode_ptr, matched_node, false)) {
                this->MatchCTNodeWithNavNode(ctnode_ptr, matched_node);
            }
        }
    }
    // 根据匹配结果 重新做多边形校验（参考-->EnclosePolygonsCheck）：
    // 判断多边形轮廓中没有匹配到全局的点中是否有block_vertex点
    // 若存在block_vertex点判断其是否是轮廓必要点（CTNode.is_contour_necessary）
    this->EnclosePolygonsCheck();
    //**  注意：至此，CTNode属性又更新了全局匹配属性及is_contour_necessary属性**//
    // 清空new_convex_vertices
    new_convex_vertices.clear();
    // 对更新属性后的CTNode:
    // 若非全局匹配且free_direct 非未知且非pillar：
    // 检测节点是否是wall（与前后顶点的夹角接近180），将非wall的顶点添加到new_convex_vertices中
    // 注意：这里是添加了没有匹配到全局且free_direct属性非未知且非pillar的非wall节点
    for (const auto& ctnode_ptr : ContourGraph::contour_graph_) { // Get new vertices
        if (!ctnode_ptr->is_global_match && ctnode_ptr->free_direct != NodeFreeDirect::UNKNOW) {
            if (ctnode_ptr->free_direct != NodeFreeDirect::PILLAR) { // check wall contour
                const float dot_value = ctnode_ptr->surf_dirs.first * ctnode_ptr->surf_dirs.second;
                if (dot_value < ALIGN_ANGLE_COS) continue; // wall detected
            }
            new_convex_vertices.push_back(ctnode_ptr);
        }
    }
}

2.1 NearestNavNodeForCTNode（）

NearestNavNodeForCTNode：轨迹点是不参与匹配的~~~~
CTNode与待匹配NavNode顶点所在多边形顶点的角平分线方向夹角角度偏差60度范围内且两者距离在2倍车身距离内，目前找到的角度和距离最接近的点
如果当前的点没有之前最优的点好，则返回空~~~~表示当前匹配失败
匹配失败的点，会被判断是否必要点添加没有匹配到全局且free_direct属性非未知且非pillar的非wall节点到待更新节点
匹配失败但是必要节点的CTNode会被标记为is_block_frontier
处理对象：ctnode_ptr near_nodes

NavNodePtr ContourGraph::NearestNavNodeForCTNode(const CTNodePtr& ctnode_ptr, const NodePtrStack& near_nodes) {
    float nearest_dist = FARUtil::kINF;
    NavNodePtr nearest_node = NULL;
    float min_edist = FARUtil::kINF;
    const float dir_thred = 0.5f; //cos(pi/3);
    for (const auto& node_ptr : near_nodes) {
        // is_odom is_navpoint IsOutsideGoal 都不进行匹配的~~~~
        if (node_ptr->is_odom || node_ptr->is_navpoint || FARUtil::IsOutsideGoal(node_ptr) || !IsInMatchHeight(ctnode_ptr, node_ptr)) continue;
        // no match with pillar to non-pillar local vertices
        if ((node_ptr->free_direct == NodeFreeDirect::PILLAR && ctnode_ptr->free_direct != NodeFreeDirect::PILLAR) ||
            (ctnode_ptr->free_direct == NodeFreeDirect::PILLAR && node_ptr->free_direct != NodeFreeDirect::PILLAR)) 
        {
            continue;
        }
        float dist_thred = FARUtil::kMatchDist;//robot_dim*2
        float dir_score = 0.0f;//dir_thred=0.5
        if (ctnode_ptr->free_direct != NodeFreeDirect::PILLAR && node_ptr->free_direct != NodeFreeDirect::UNKNOW && node_ptr->free_direct != NodeFreeDirect::PILLAR) {
            if (ctnode_ptr->free_direct == node_ptr->free_direct) {
                // SurfTopoDirect
                const Point3D topo_dir1 = FARUtil::SurfTopoDirect(node_ptr->surf_dirs);
                const Point3D topo_dir2 = FARUtil::SurfTopoDirect(ctnode_ptr->surf_dirs);
                // topo_dir1 * topo_dir2 相当于cosT T小于60或者大于300的时候  dir_score>0
                dir_score = (topo_dir1 * topo_dir2 - dir_thred) / (1.0f - dir_thred);
            }
        } else if (node_ptr->free_direct == NodeFreeDirect::PILLAR && ctnode_ptr->free_direct == NodeFreeDirect::PILLAR) {
            dir_score = 0.5f;
        }
        dist_thred *= dir_score;//T=0的时候该值最大 就是角度越接近距离限制越宽松（<60 >300）60~300夹角范围内不可能有满足条件的点
        const float edist = (node_ptr->position - ctnode_ptr->position).norm_flat();
        if (edist < dist_thred && edist < min_edist) {
            nearest_node = node_ptr;
            min_edist = edist;
        }
    }
    if (nearest_node != NULL && nearest_node->is_contour_match) {
        const float pre_dist = (nearest_node->position - nearest_node->ctnode->position).norm_flat();
        if (min_edist < pre_dist) {
            // reset matching for previous ctnode
            RemoveMatchWithNavNode(nearest_node);
        } else {
            nearest_node = NULL;
        }
    }
    return nearest_node;
}

2.2 IsCTMatchLineFreePolygon（）

这个用于匹配就是匹配的两个节点的freepoint 中和之后还是个freepoint?
没太懂是解决啥问题的？
若黑色是NavNode 其他是匹配到的轮廓
红色和黄色蓝色的节点就是符合要求的
但是紫色的就不行除非两个节点几乎重合才行（要不然找到的freepoint 会被紫色多边形包住）
所以这是为啥呢？？

在这里插入图片描述

bool ContourGraph::IsCTMatchLineFreePolygon(const CTNodePtr& matched_ctnode, const NavNodePtr& matched_navnode, const bool& is_global_check) {

    /*如果匹配到的两个点足够近，直接返回true*/
    if ((matched_ctnode->position - matched_navnode->position).norm() < FARUtil::kNavClearDist) return true;
    /*否则，进行IsPointsConnectFreePolygon检验*/
    const HeightPair h_pair(matched_ctnode->position, matched_navnode->position);
    const ConnectPair bd_cedge = ConnectPair(matched_ctnode->position, matched_navnode->position);
    const ConnectPair cedge = ContourGraph::ReprojectEdge(matched_ctnode, matched_navnode, FARUtil::kProjectDist);
    return ContourGraph::IsPointsConnectFreePolygon(cedge, bd_cedge, h_pair, is_global_check);
}

2.3 IsNavNodesConnectFreePolygon（）

两个节点相连不穿障碍物

bool ContourGraph::IsNavNodesConnectFreePolygon(const NavNodePtr& node_ptr1, const NavNodePtr& node_ptr2) {
    if (node_ptr1->is_navpoint || node_ptr2->is_navpoint) {
        if ((node_ptr1->position - node_ptr2->position).norm() < FARUtil::kNavClearDist) { // connect to internav node
            return true;
        }
    }
    const bool is_global_check = ContourGraph::IsNeedGlobalCheck(node_ptr1->position, node_ptr2->position);
    ConnectPair cedge = ContourGraph::ReprojectEdge(node_ptr1, node_ptr2, FARUtil::kProjectDist, is_global_check);
    if (node_ptr1->is_odom) {
        cedge.start_p = cv::Point2f(FARUtil::free_odom_p.x, FARUtil::free_odom_p.y);
    } else if (node_ptr2->is_odom) {
        cedge.end_p = cv::Point2f(FARUtil::free_odom_p.x, FARUtil::free_odom_p.y);
    }
    ConnectPair bd_cedge = cedge;
    const HeightPair h_pair(node_ptr1->position, node_ptr2->position);
    if (!node_ptr1->is_boundary) bd_cedge.start_p = cv::Point2f(node_ptr1->position.x, node_ptr1->position.y);
    if (!node_ptr2->is_boundary) bd_cedge.end_p = cv::Point2f(node_ptr2->position.x, node_ptr2->position.y);
    return ContourGraph::IsPointsConnectFreePolygon(cedge, bd_cedge, h_pair, is_global_check);
}

2.4 IsPointsConnectFreePolygon（）

IsPointsConnectFreePolygon：当前节点与所匹配节点的连线与当前所有轮廓都没有交叉？

bool ContourGraph::IsPointsConnectFreePolygon(const ConnectPair& cedge,
                                              const ConnectPair& bd_cedge,
                                              const HeightPair h_pair,
                                              const bool& is_global_check)
{
    // check for boundaries edges 
    for (const auto& contour : ContourGraph::boundary_contour_) {
        if (!ContourGraph::IsEdgeOverlapInHeight(h_pair, HeightPair(contour.first, contour.second))) continue;
        if (ContourGraph::IsEdgeCollideSegment(contour, bd_cedge)) {
            return false;
        }
    }
    if (!is_global_check) {
        // check for local range polygons
        const Point3D center_p = Point3D((cedge.start_p.x + cedge.end_p.x) / 2.0f,
                                         (cedge.start_p.y + cedge.end_p.y) / 2.0f,
                                         0.0f);
        for (const auto& poly_ptr : ContourGraph::contour_polygons_) {
            if (poly_ptr->is_pillar) continue;
            // 节点本身所在的多边形是不是可能会包这个点呢？？？ 就是无论如何不能包？
            // 除非两者之间的距离一半小于投影距离 要不就CTNode所在的
            // 全局有膨胀？
            // 这跟两个节点的相对位置有关系呀 CTNode所在多边形包住NavNode的时候好像就不成立了
            // 这种为啥就不行呢？说明啥呀？
            if ((poly_ptr->is_robot_inside != FARUtil::PointInsideAPoly(poly_ptr->vertices, center_p)) || 
                ContourGraph::IsEdgeCollidePoly(poly_ptr->vertices, cedge)) 
            {
                return false;
            }
        }
        // check for unmatched local contours
        for (const auto& contour : ContourGraph::unmatched_contour_) {
            if (ContourGraph::IsEdgeCollideSegment(contour, cedge)) {
                return false;
            }
        }
        // check for any inactive local contours
        for (const auto& contour : ContourGraph::inactive_contour_) {
            if (ContourGraph::IsEdgeCollideSegment(contour, cedge)) {
                return false;
            }
        }
    } else {
        for (const auto& contour : ContourGraph::global_contour_) {
            if (!ContourGraph::IsEdgeOverlapInHeight(h_pair, HeightPair(contour.first, contour.second))) continue;
            if (ContourGraph::IsEdgeCollideSegment(contour, cedge)) {
                return false;
            }
        }
        for (const auto& poly_ptr : ContourGraph::contour_polygons_) {
            if (poly_ptr->is_pillar) continue;
            if (ContourGraph::IsEdgeCollidePoly(poly_ptr->vertices, cedge)) {
                return false;
            }
        }
    }
    return true;
}

2.5 ContourGraph::EnclosePolygonsCheck()

处理对象：ContourGraph::polys_ctnodes_：ctnode_ptr （对每一个多边形对象）
处理逻辑：

判断多边形轮廓中没有匹配到全局的点中是否有block_vertex点
若有block_vertex顶点，判断是否是is_contour_necessary（根据is_ground_associate）

问题：is_contour_necessary is_ground_associate 初始的时候都是false
is_ground_associate 啥时候才是true

void ContourGraph::EnclosePolygonsCheck() {
    for (const auto& ctnode_ptr : ContourGraph::polys_ctnodes_) { // loop each polygon
        if (ctnode_ptr->poly_ptr->is_pillar) continue;//若pillar不处理
        const CTNodePtr start_ctnode_ptr = FirstMatchedCTNode(ctnode_ptr);//找到多边形中第一个匹配到NavNode的节点
        if (start_ctnode_ptr == NULL) continue;//若没有匹配到的全局点 不处理
        /* 按照front方向取pre cur 节点*/
        CTNodePtr pre_ctnode_ptr = start_ctnode_ptr;
        CTNodePtr cur_ctnode_ptr = start_ctnode_ptr->front;
        /* 确保取到的pre cur节点都是global_match*/
        // 对多边形内所有匹配到的NavNode的相邻顶点对进行block_vertex判断
        // 并对找到的block_vertex判断是否is_contour_necessary（根据is_ground_associate ）
        
        while (cur_ctnode_ptr != start_ctnode_ptr) {
            if (!cur_ctnode_ptr->is_global_match) {
                cur_ctnode_ptr = cur_ctnode_ptr->front;
                continue;
            }
            CTNodePtr block_vertex = NULL;
            //参考方法：IsCTNodesConnectWithinOrder（）
            // 注意是判断两个顶点之间的多边形轮廓没有那种奇怪的形状（突出的夹角）
            //类似下面这种：1 2 3 4为多边形的顶点
            // 其中 1 4为匹配到全局的顶点
            // 2 3为待检验的点
            // 若2或者3 出现下面这种情况 则会返回false同时返回2或者3作为block_vertex
            // 这种形状的边不符合最短路径？
            
            //*(ctnode2)--------------------------------------------------*(ctnode3)
          
            
            //               o(ctnode1)-------------------o(ctnode4)
            // 若存在着种情况则会走下面的逻辑：
            // 若所返回的block_vertex顶点是is_ground_associate 且IsPointInMarginRange
            // 则block_vertex->is_contour_necessary属性为true（新创建的CTNode这些属性都是false）
            //?is_ground_associate啥时候是true的？是在terrain_grid_traverse_list_中匹配到该点就是true
            //表示地形可达的？？
            if (!IsCTNodesConnectWithinOrder(pre_ctnode_ptr, cur_ctnode_ptr, block_vertex) && block_vertex != NULL) {
                if (block_vertex->is_ground_associate && FARUtil::IsPointInMarginRange(block_vertex->position)) {
                    block_vertex->is_contour_necessary = true;
                }
            }
            pre_ctnode_ptr = cur_ctnode_ptr;
            cur_ctnode_ptr = cur_ctnode_ptr->front;
        }
    }
}

输入参数：ctnode1，ctnode2为一个多边形中front方向取到的与NavNode匹配到的邻近节点
处理逻辑：

若两节点相等或者不在同一个多边形内则返回false
取ctnode1的front节点作为next_ctnode
若next_ctnode不为空且ctnode2与next_ctnode不是同一个节点：
判断ctnode1的front节点是否在ctnode1 ctnode2的柱形区域内
1. 若不在，block_vertex = next_ctnode;返回false
2. 若在，取next_ctnode的front节点更新next_ctnode，继续下一次判断
若next_ctnode为空或者ctnode2与next_ctnode是同一个节点直接返回true
3中ctnode1与ctnode2中间所有节点都通过检测，返回ture
注意：返回true的时候block_vertex 是为null的

bool ContourGraph::IsCTNodesConnectWithinOrder(const CTNodePtr& ctnode1, const CTNodePtr& ctnode2, CTNodePtr& block_vertex) {
    block_vertex = NULL;
    if (ctnode1 == ctnode2 || ctnode1->poly_ptr != ctnode2->poly_ptr) return false;
    CTNodePtr next_ctnode = ctnode1->front; // forward search
    while (next_ctnode != NULL && next_ctnode != ctnode2) {
        if (!FARUtil::IsInCylinder(ctnode1->position, ctnode2->position, next_ctnode->position, FARUtil::kNearDist, true)) {
            block_vertex = next_ctnode;
            return false;
        }
        next_ctnode = next_ctnode->front;
    }
    return true;
}

3. ExtractGlobalContours()

轮廓是局部的
这里为啥整全局的轮廓呢？
全局的作用是啥？
—>通过调用关系来看这个是记录实际更新到全局的轮廓

UpdateNavGraph()--------> GraphCallBack()
TopTwoContourConnector()-----> FillContourConnect()
AddContourConnect()------>
AddContourToSets()---->

后面进行轮廓连接的进行有效检验的时候是基于这个的会首先判断 is_global_check是否是true

is_boundary 这个属性没看出啥时候会更新呢

根据global_contour_set_ 和boundary_contour_set_来更新下面几个集合？
这个两个集合是全局的一直记录到目前为止被匹配到全局并且产生连接的轮廓
并且global_contour_set_ 包含boundary_contour_set_

这几个集合每次也会被先清空然后根据当前最新的global_contour_set_ 和boundary_contour_set_来更新对应的集合
static std::vector global_contour_; // 每次也被清空？？
static std::vector inactive_contour_;
static std::vector unmatched_contour_; 这几个是在ExtractGlobalContours()
static std::vector boundary_contour_;
static std::vector local_boundary_;
并且每次都先clear() 所以也是个局部？
看着后面更新轮廓连线关系的时候也会同时更新 global_contour_ boundary_contour_
inactive_contour_ unmatched_contour_ 是怎么更新呢？？？

void ContourGraph::ExtractGlobalContours() {
    ContourGraph::global_contour_.clear();
    ContourGraph::inactive_contour_.clear();
    ContourGraph::unmatched_contour_.clear();
    ContourGraph::boundary_contour_.clear();
    ContourGraph::local_boundary_.clear();
    for (const auto& edge : ContourGraph::global_contour_set_) {
        ContourGraph::global_contour_.push_back({edge.first->position, edge.second->position});
        if (IsEdgeInLocalRange(edge.first, edge.second)) {
            if (!this->IsActiveEdge(edge.first, edge.second)) {
                ContourGraph::inactive_contour_.push_back({edge.first->position, edge.second->position});
            } else if (!edge.first->is_near_nodes || !edge.second->is_near_nodes) {
                PointPair unmatched_pair = std::make_pair(edge.first->position, edge.second->position);
                if (edge.first->is_contour_match) {
                    unmatched_pair.first = edge.first->ctnode->position;
                } else if (edge.second->is_contour_match) {
                    unmatched_pair.second = edge.second->ctnode->position;
                } 
                ContourGraph::unmatched_contour_.push_back(unmatched_pair);
            }
        }
    }
    for (const auto& edge : ContourGraph::boundary_contour_set_) {
        ContourGraph::boundary_contour_.push_back({edge.first->position, edge.second->position});
        if (IsEdgeInLocalRange(edge.first, edge.second)) {
            ContourGraph::local_boundary_.push_back({edge.first->position, edge.second->position});
            bool is_new_invalid = false;
            if (!IsValidBoundary(edge.first, edge.second, is_new_invalid) && is_new_invalid) {
                edge.first->invalid_boundary.insert(edge.second->id);
                edge.second->invalid_boundary.insert(edge.first->id);
            }
        }
    }
}