Far planner 代码系列（22）

引言：

在动态图数据的实时更新与处理中，算法的精确性和实时性是关键。特别是在自动驾驶和机器人导航等地方，对周围环境的快速准确感知是确保安全运行的基础。本文通过分析“DynamicGraph::UpdateGlobalNearNodes()”算法的实现细节，探讨了如何通过优化数据结构和算法逻辑来提高对节点信息的更新效率。

相关问题：

1. 如何有效管理和更新图中的节点信息？
2. 在实时动态环境中，如何快速定位对当前位置影响最大的节点？
3. 如何确定哪些节点是“桥梁”节点，它们在路径规划中的作用是什么？

相关答案：

1. 通过构建适当的动态数据结构和使用高效的搜索算法，可以有效地管理和更新图中的节点信息。 例如，使用二叉搜索树或红黑树等数据结构可以加快搜索和插入操作的速度。
2. 算法中使用了一些策略来快速定位对当前位置影响最大的节点。 通过设置不同的筛选条件，如根据节点的重要性（使用节点的重要性参数，如fgscore）或距离当前位置的地理位置和高度等，可以快速缩小搜索范围。
3. 在算法中，使用“is_bridge_internav_”标识节点是否为桥梁节点。 这些节点在某些情况下可以用来连接不同的导航路径或者在某个路径不可行时作为备选路径。

实质性帮助：

通过以上分析，我们可以得出，实现实时动态环境下的有效导航和路径规划，不仅需要高效的算法，还需要精确的数据结构来支持。例如，可以开发一个基于地理信息的图数据库，它能够实时更新节点信息，并支持快速查询。这种数据库可以利用缓存策略和索引技术来提高查询效率。同时，算法中使用到的“桥梁”节点概念可以用于设计路径选择机制，确保在复杂环境中依然能找到最优路径。

总之，通过深入理解和优化数据结构和算法，我们可以显著提高动态环境中导航和路径规划的效率和准确性。

DynamicGraph::UpdateGlobalNearNodes( ) （二）

        经过之前的过程，我们得到了extend_match_nodes_、wide_near_nodes_、near_nav_nodes_、internav_near_nodes_、surround_internav_nodes_和margin_near_nodes_

        我们要接着完善 wide_near_nodes_，这里也很好理解，把odom里连接到的每个node都取出来，如果这个node中的is_wide_near为false，那么我们就把它加入到wide_near_nodes_中。再来一个循环，把和node连接的其他contour的node也取出来，同样通过is_wide_near判断该点要不要放进wide_near_nodes_

        说白了，之前从globalGraphNodes_里得到一部分，现在要通过odom把漏掉的一部分也补进去，就这么简单。

        接着，如果internav_near_nodes_不为空，那就从里面找一个连接到odom的最短的inter_nav node

         首先，若internav_near_nodes_不为空，我们按intensity从小到大给里面的node进行排序，所以intensity小的就可能是最接近odom的点。

        当然我们还要通过循环每一个node来判断它到底是不是最接近odom的点，我们可以通过取出每一个点，令其为temp_internav_ptr。

        我们知道每一个odom都有一组potential_edges(这个之后再说，先知道有就行了)来存放可能的点，或者说离odom一段范围内的点。那我们就先要判断temp_internav_ptr在不在这个potential_edges里，如果不在，那直接排除掉，换下一个temp_internav_ptr。

        除了满足上面的要求，这个temp_internav_ptr还要满足它在InternavInRange的范围里。

        这个函数给了一个TRAJ_DIST 也就是trajectory的距离为5，高度范围的阈值为1.3，它判断当前传入的node的fgscore 和TRAJ_DIST的关系与该node 的position和height的关系，只要一个不满足要求就返回false。（fgscore我还没看到，感觉可能是和路径规划有关的内容？）

        如果上面两个要求都满足，那接下来继续看

        这个判断蛮长的，其中的 cur_internav_ptr_ 在初始化的时候为NULL，所以一开始是满足条件的，那么他就去更新cur_internav_ptr_

        这个更新也蛮好理解的，先把temp_internav_ptr赋给cur_internav_ptr_

        然后last_internav_ptr_ 初始是NULL ，开始设置last_internav_ptr_，更新terrain_grid的中心点，重置grid范围内的点，重置is_occupied，更新last_internav_ptr_ = cur_internav_ptr_

        接着，若last_internav_ptr_ 不为null了，（第一次更新让两者相等了，第二次更新 cur_internav_ptr就变了，而last_internav_ptr_还是之前的cur_internav_ptr），那这时候last_internav_ptr_ 和 cur_internav_ptr_两者就不相等了。这里同样更新terrain_grid的中心点，重置grid范围内的点，不同的是，增加了一个AddTrajectoryConnect的函数。

        最后依然还是把当前cur_internav_ptr的点赋给last_internav_ptr

        通过UpdateCurInterNavNode更新完，我们就要把函数给break了，跳出循环，那么整个UpdateGlobalNearNodes()就结束了。

        如果我们在循环internav_near_nodes的时候，存在一个node，它之前的条件都满足，但是到了UpdateCurInterNavNode的时候不满足条件，我们就认为该node它是一个bridge，将这个node的is_bridge_internav_设置为true（判断是否需要设置inter_nav_point的时候会用到），然后将函数break，跳出循环。

        到这里整个UpdateGlobalNearNodes()就结束了。