EGO-PLANNER代码阅读（地图部分）

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容器中，最后一并发布。

镇楼图！