Far planner代码系列（30）TopTwoContourConnector

我们知道，nav_node里的potential_contours中可能有许多个nodes，而TopTwoContourConnector函数主要是把nav_node和potential_contours里得分最高的两个nodes进行关联。

        这个是graph_manager_.UpdateNavGraph里的一个小函数，我们先讲一下，会对大体的流程有一定的了解。

        假设我们有两个ctnode 分别叫做ctnodeA和ctnodeB，他们再经过一次更新后变成了navnodeA和navnodeB，并加入了global_graph（也可以叫他们contour_nodeA和contour_nodeB）。而且他们互相在对方的potential_contours和contours_votes里。如下图我们用nodeA和nodeB表示

         那么：

        A的potential_contours就有（B,*,*,*) 

        B的potential_contours就有（A,*,*,*)

        注：*号代表和该node有潜在连接关系的node

        A的contour_votes里就会有(,,....)

        B的contour_votes里就会有(,,....)

        注：node_id(B)代表是B点的node_id，后面的111101是每次判断连接的一个数字，1代表连接，0代表不连接，判断连接是否有效 则取sum(1,1,1,1,0,1)= 5, 看看5占了这个deque的size大小的多少，比如现在deque的size为6 我们就可以确定他们是一个validconnect

        回到函数里，我们循环NodeA所有的contour_votes，如果是有效连接（即deque里面1的总和与deque的size的比例达到一定，我们就认为nodeA到nodeX的连接有效），我们就计算他们各自的votes分数，并存到votesc容器里，在对votesc进行一个从大到小的排序。

         接着我们进行一个循环，把nodeA的潜在contour_node挨个取出来，比如我们取到了B点，

那么:

        const auto it  = node_ptr->contour_votes.find(cnode_ptr->id)

        就是node_ptr->contour_votes.find(nodeB_id) 

        因为nodeB是nodeA的potential_contour之一，那么肯定能找到它的id，不然就报错。

        找到了以后我们就计算nodeB的vote的得分，记为itc，如果itc的分数在votesc里是最高的两个之一，且nodeB到nodeA的连接是有效的：

        1、我们把它们互相加到对方的contour_connect里，并把两个node组成NavEdge点对，加入到global_contour_set_中。

        2、把它们互相加到对方的connect_nodes里