算法2 v-graph的动态更新（3）

        今天讲的部分是Contour里的ctnode和near nav nodes进行匹配的部分

MatchContourWithNavGraph（1）

contour_graph_.MatchContourWithNavGraph(nav_graph_, near_nav_graph_, new_ctnodes_)

        其中 nav_graph_是global层级的，near_nav_graph_是通过下面的函数获取到的，new_ctnodes是匹配函数完成后得到的。

    graph_manager_.UpdateGlobalNearNodes();    near_nav_graph_ = graph_manager_.GetExtendLocalNode();

        匹配函数 MatchContourWithNavGraph的作用就是，把刚扫描出来的current_contour和global_nav_graph进行匹配，但是 global_nav_graph 非常大 是全局的量，必须减少运算，就得从global_nav_graph里提取near_nav_graph_ 来进行匹配，这样运算的量会小非常多。

        如果ctnode有没有匹配到的点，且它的free_direct不为UNKNOW  那么我们根据具体的情况来把这些ctnode点push到一个new_convex_vertices里

        我们今天主要来看匹配函数部分 ，函数完成后，会从contour_graph_里得到new_convex_vertices(也就是new_ctnodes_)

void ContourGraph::MatchContourWithNavGraph(const NodePtrStack& global_nodes, const NodePtrStack& near_nodes, CTNodeStack& new_convex_vertices) {    //把每个global_nodes里的node里面的属性is_contour_match 和 ctnode 全初始化    for (const auto& node_ptr : global_nodes) { node_ptr->is_contour_match = false; node_ptr->ctnode = NULL;    }    // 修改contour_graph_里的ctnode_ptr的属性    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) {     // ctnode_ptr是本来就有的 从contour_graph_来的const NavNodePtr matched_node = this->NearestNavNodeForCTNode(ctnode_ptr, near_nodes);     //! NearestNavNodeForCTNode 从near_nodes里返回一个 nearest_node 或 NULL  属于navnode     //? 正常来说 每个ctnode应该都会得到一个nearerst_node（matched_node）     if (matched_node != NULL && IsCTMatchLineFreePolygon(ctnode_ptr, matched_node, false)) {  this->MatchCTNodeWithNavNode(ctnode_ptr, matched_node);  // 修改ctnode的部分属性 和 matched_node的部分属性  //! 应该完循环以后  很多 ctnode 里的 is_global_match 变成了 true     }    }    }    this->EnclosePolygonsCheck();   //可能有些ctnode的is_contour_necessary属性变成了true    //  这说明我的contour_graph是一个local层级的 才会从这里面 get new vertices    new_convex_vertices.clear();    //!  把contour_graph_里符合要求的 ctnode_ptr push 进 new_convex_vertices    for (const auto& ctnode_ptr : ContourGraph::contour_graph_) { // Get new vertices    // ctnode不是global_match 且 free_direct 不是UNKNOWN if (!ctnode_ptr->is_global_match && ctnode_ptr->free_direct != NodeFreeDirect::UNKNOW) {     if (ctnode_ptr->free_direct != NodeFreeDirect::PILLAR) { // check wall contour  // 因为dir里的点对都是归一化过的，所以点积直接就可以得到夹角的cos值  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); }    }}

        函数开头的一个循环，把nav_graph_里的nodes的is_contour_match、ctnode属性进行初始化，即每个nodes都没有完成contour_match 且对应的ctnode也是空的。

for (const auto& node_ptr : global_nodes) { node_ptr->is_contour_match = false; node_ptr->ctnode = NULL;    }

        接下来的循环 修改修改contour_graph_里的ctnode的属性 把is_global_match默认成false，nav_node_id = 0 然后接着判断NearestNavNodeForCTNode  ctnode和每一个near_nodes进行对比。

    // 修改contour_graph_里的ctnode_ptr的属性    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) {     // ctnode_ptr是本来就有的 从contour_graph_来的  算是global？     const NavNodePtr matched_node = this->NearestNavNodeForCTNode(ctnode_ptr, near_nodes);     //! NearestNavNodeForCTNode 从near_nodes里返回一个 nearest_node 或 NULL  属于navnode     //? 正常来说 每个ctnode应该都会得到一个nearerst_node（matched_node）     if (matched_node != NULL && IsCTMatchLineFreePolygon(ctnode_ptr, matched_node, false)) {  this->MatchCTNodeWithNavNode(ctnode_ptr, matched_node);  // 修改ctnode的部分属性 和 matched_node的部分属性  //! 应该完循环以后  很多 ctnode 里的 is_global_match 变成了 true     }    }    }

        我们来看一下NearestNavNodeForCTNode(ctnode_ptr, near_nodes)

//! 传入的ctnodes和每一个nearnodes的点进行配对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) { 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;  //  kMatchDist = 1.75 float dir_score = 0.0f;     //dir_score 是方向向量的得分？ 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)    {   //两个点的类型必须相同  const Point3D topo_dir1 = FARUtil::SurfTopoDirect(node_ptr->surf_dirs);  const Point3D topo_dir2 = FARUtil::SurfTopoDirect(ctnode_ptr->surf_dirs);  //计算两者的topo_dir  各返回一个归一化的向量  dir_score = (topo_dir1 * topo_dir2 - dir_thred) / (1.0f - dir_thred);   //! topo_dir1 * topo_dir2 从几何意义上说，两个向量点乘的结果表示了两个向量的相似度，结果越大则越相似  //! 从另一方面 因为两个向量已经归一化，点积代表夹角的cos值  //!  dir_score 被约束到 1到-3之间   1（完全重合 （1-0.5）/（1-0.5））  -1（dir1与dir2垂直  （0-0.5）/（1-0.5）） -3（完全相反 （-1-0.5）/（1-0.5））     } } else if (node_ptr->free_direct == NodeFreeDirect::PILLAR && ctnode_ptr->free_direct == NodeFreeDirect::PILLAR) {     dir_score = 0.5f; } dist_thred *= dir_score; const float edist = (node_ptr->position - ctnode_ptr->position).norm_flat(); //edist 相当于edge_dist 两点之间的距离 就是 边的距离 if (edist < dist_thred && edist 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;}

        其中定义了dir_thred = 0.5f 主要这个是弧度  0.5弧度 也就是60°，再根据near_nodes里提取每一个node_ptr进行挨个对比。

        对比过程中还定义了float dist_thred = FARUtil::kMatchDist 这个根据yaml的配置文件算出来是1.75

        定义了一个float dir_score = 0.0f来判断得分。

        传入的ctnode有一个topo_dir，near_nodes循环里的每个node也有一个topo_dir 两个dir可以计算出一个dir_score来判断向量的相似性。而dist_thred再迭代一次dist_thred *= dir_score

        现在有了dist_thred，我们还要计算ctnode和node之间的距离 记为edist

        最后edist < dist_thred的话，就取出nearest_node 并令 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;

        接着我们来看这一段，上面循环走完 得到nearest_node 和 min_edist 然后接下来要判断 nearest_node是不是已经匹配过。没有匹配过 也就是is_contour_match为false 那么直接返回nearest_node

        如果is_contour_match为true 就要计算这个nearest_node之前存过的ctnode与nearest_node的距离 记为pre_dist

        如果min_edist < pre_dist 我们就要把nearest_node之前存的ctnode给reset掉

static inline void RemoveMatchWithNavNode(const NavNodePtr& node_ptr) { if (!node_ptr->is_contour_match) return; node_ptr->ctnode->is_global_match = false; node_ptr->ctnode->nav_node_id = 0; node_ptr->ctnode = NULL; node_ptr->is_contour_match = false;    }

        is_contour_match为true 那么    !node_ptr->is_contour_match 为false 往下执行，

将nearest_node里的ctnode里的is_global_match 设为 false

将nearest_node里的ctnode里的nav_node_id 设为0

将nearest_node里的ctnode 指向 NULL

将nearest_node里的is_contour_match 设为 false

        如果min_edist > pre_dist 则nearest_node = NULL