plan_env/grid_map.cpp
最近由于多个比较实验的原故，把ego-planner及Fast-planner中TOPO-REPLAN的代码都分析了一遍。EGO主要是为了看gridmap的使用。TOPO-Replan主要是分析了topo-path searching部分。现记录于下，以供交流。
1.欢迎参看关于kinoreplan部分的完整分析：Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
2.欢迎参看我关于topo-replan路径搜索部分的代码分析：Fast-planner代码阅读2-TopoReplan (path searching部分)
1.depthPoseCallback()
这一函数通过message_filter类接收同步后的最新的相机Pose 与 深度图，同时，如果相机的位置处于全局地图Map_size之外，则就会将md.occ_need_update这一flag置false，反之置为true。
2. updateOccupancyCallback（）
这是最终要的一个定时器回调函数，地图节点通过这一回调函数定时更新地图。
其中有两个重要的flag,一个是上文提到的，md.occ_need_update，只有接收到新图像且位于地图范围之内，才会进行接下来的projectDepthImage()及raycastProcess()这两个流程。
同样的，另外一个flag是md.local_updated。这一flag只在raycastProcess中判断深度图投影点数量不为零是才会置为true，这时才会进入clearAndInflateLocalMap（）这一流程，对局部地图进行膨胀和更新。
3. projectDepthImage();
这一函数比较直观，就是通过相机投影模型，将图像坐标系上的点投影至相机坐标系，再通过相机的位姿将相机坐标系上点投影至世界坐标系，最后将所有点存入md.proj_points_这一vector容器中。
4. raycastProcess()
这一函数会对md.proj_points中的每一个点进行raycast流程。首先判断每一个点是否超出地图范围，是否超出ray_length，如果超出，就将此点重新赋值为一个允许的射线上最远的点。
如果重新赋值，则利用setCacheOccupancy（）这一函数将md_.count_hit_and_miss_这一容器对应序列上的计数+1次。表示这一free空间被经过了一次。如果这一点是第一次被遍历，则应该把它加入到md_.cache_voxel这一容器中区
如果不需要重新赋值，说明当前点是障碍物，则利用setCacheOccupancy将这一点在md_.count_hit容器中对应序列的位置计数+1。需要说明的是，不管当前点是不是障碍物，md_.count_hit_and_miss_容器对应位置处的计数都会被+1.
当终点被setCache过后，就进入raycast环节，通过raycast.step函数从射线终点开始向相机点步进。并且将每一个中途点都利用setCacheOccupancy函数置一次free。需要注意的是，每一个中途点还利用md_.flag_traverse_容器进行了判断，如果对应序列处的值不是本轮raycast的num,则将其置为b本轮的racastnum.否则说明这一点及之后的点都已经被raycast过了，因此跳出当前射线终点的raycast循环。
当完成md.proj_points容器中所有点的raycast循环后，开始对md_.cache_voxel中的点进行循环判断。首先根据md_.count_hit及md_.count_hit_and_miss中对应位置的值判断当前voxel为障碍物的概率。并且如果当前点的log_odds_update是prob_hit_log，且md_.occupancy_buffer_中对应位置的概率值还没有超过最大值或当前点的log_odds_update是prob_miss_log，且md_.occupancy_buffer_中对应位置的概率值还没有低于最小值。且当前点是在局部地图范围内，则更新md_.occupancy_buffer_中的概率值。
5. clearAndInflateMap()
这一函数首先将局部范围外一圈的点的occupancy_buffer对应值置为：mp_.clamp_min_log_ – mp_.unknown_flag_。
然后将局部地图范围内的地图上一轮的occupancy_buffer_inflate值全部置为0；
紧接着，对局部地图的occupancy_buffer中所有点的值进行一一判断，判断是否超过为障碍物的最低概率mp_.min_occupancy_log_，如若判断，就对该点进行膨胀，并将所有膨胀点的occupancy_buffer_inflate值全部置为1；
6. publishmap() 和 publishInflateMap()
这两函数分别对occupancy_buffer及occupancy_buffer_inflate容器中局部地图范围内的所有点进行判断，若值分别超过障碍物最小概率及为1，且不超过高度范围，则将其从voxel序列还原成三维位置点，推入cloud容器中，最后一并发布。
镇楼图！
Original: https://blog.csdn.net/weixin_42284263/article/details/122283727
Author: 养生少年小余
Title: EGO-PLANNER代码阅读（地图部分）
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/520045/
转载文章受原作者版权保护。转载请注明原作者出处！