TerrainCallBack详解（2）

        上回讲到更新temp_obs_ptr和temp_free_ptr ，我们接着往下讲。接着就用到了ExtractNewObsPointCloud函数去更新抽取NewObsPointCloud

         函数把temp_obs_ptr_和surround_obs_cloud_输入，把surround_obs_cloud_作为一个参考点列，把其intensity设置为255。temp_obs_ptr_的intensity设为0 然后把这两个都并到一起，得到一个temp_new_cloud 。对temp_new_cloud进行下采样，减少点云的数量，其中intensity也会变化。具体咋变，我也不是很清楚。

        根据采样完的temp_new_cloud，我们把每个点提取出来进行判断，判断它的intensity是否小于2，如果小于2，那就往cur_new_cloud里push这个点。

        完事以后得到cur_new_cloud

        这个结束以后，接着往下走。函数GetSurroundFreeCloud执行，把之前的surround_free_cloud_清空，然后从neighbor_free_indices_重新更新surround_free_cloud_

         更新完成后，还要更新TerrainHeightGrid

         这个TerrainHeightGrid保存的是一个固定大小的可通行区域。它会随着机器人的移动而改变。但是尺寸一直都是固定的。应该是89*89*1 也就是范围为89*89 高度为1.

        而surround_obs_grid和surround_free_grid它们的范围都是200*200*125。如下图展示的分别为free和obs

         言归正传，图看够了，我们就来看UpdateTerrainHeightGrid的代码部分。

        上半部分的代码主要就是把 surround_free_cloud拷贝一份，复制到copy_free_ptr里。接着把copy_free_ptr通过一次滤波。其滤波的filter大小为terrain_height_grid_->GetResolution()，根据yaml文件可以换算一下就是（0.8，0.8，0.15）。然后往terrain_grid_occupy_list_填0 这个list的大小为7921。

        接着就是一顿循环。我们把copy_free_ptr里的点挨个取出来，注意到它变成csub的时候，只用到了点的x和y坐标，z坐标为0。然后作者用了一个Expansion2D函数把csub扩大了一圈。当INFLATE_N为1的时候，从一个点扩展到九个点。

         然后我们再把这subs里的每一个sub都取出来，如果当前sub不在terrain_height_grid_的range里，那就什么都不做，取下一个sub继续判断。

        如果sub在terrain_height_grid_里，那就把它转成terrain_height_grid_下的ind，如果这些点对应的ind在terrain_grid_occupy_list_[ind]里是0的话，那就是首次扫描到这个点，把这个ind对应的cell给resize成1，并把当前的z坐标push进cell的第一个位置。

        如果这些点对应的ind在terrain_grid_occupy_list_[ind]里是1的话，那就接着往cell里push它的z坐标。注意 这个是原始的point点的z坐标。它不是0，只有csub才是拿z=0生成的。最后terrain_grid_occupy_list_[ind]里把0变为1 ind对应的地方为0：不能通行（有障碍物），为1：可以通行（无障碍物）【因为传入的点都是从surround_free_cloud来的】

        接着，根据terrain_grid_occupy_list_​来得到terrain_grid_traverse_list_​​​​​​

        这个函数的内容非常长。我们一步步来看

        首先根据robot_pos的x和y坐标（z坐标设为0了）得到一个terrain_height_grid_下的robot_sub点。然后把terrain_height_ptr_清空。并判断robot_sub在不在范围内。

        给terrain_grid_traverse_list_填充0，这个terrain_grid_traverse_list_的大小和terrain_grid_occupy_list_​的大小是一致的。

         auto IsTraversableNeighbor = [&] (const int& cur_id, const int& ref_id) 这是一个lambda匿名函数。待会会用到。它的作用就是：

        放入ref_id，判断terrain_grid_occupy_list_[ref_id]是否为0，如果为0就代表ref_id这个点是障碍物，就不处理。如果不是障碍物，就把terrain_height_grid_里的cur_id对应cell存的第一个z值读出来给cur_h赋值。然后我们把terrain_height_grid_里的ref_id对应cell存的每一个z值都取出来，做一个小循环，把ref_id对应的每个z值与cur_h差值小于H_THRED的值都累加起来，当然counter也++。

        如果counter大于0，我们就把ref_id对应的cell给resize成1，并且把它的均值放到里面。并且返回true，如果counter为0，那说明ref_id里的每个z值与cur_h的值的差值都超过了H_THRED，就返回false。

        我是这么理解的：cur代表现在的点的位置，ref代表参考点的位置，如果两者的高度差的绝对值在H_THRED内，那说明这两个ind对应的没有很大的高度差，那机器人就可以通行过去，如果两者的高度差大于H_THRED 那说明两个ind直接可能是个楼梯，或者矮墙之类的，让高度有骤然变化的点。

        从这个角度出发，也就不难理解terrain_height_grid_的作用。它是映射到平面z=0的点，而我们把x和y相同的，z值不同的点，都放到了terrain_height_grid_里的cell，那么这样，一个ind可以对应terrain_height_grid_的点，而这个ind里对应的cell，就是这个x、y确定以后的一系列的空间点的高度值z。可以用图来表示：

        上面的匿名函数完事以后，还有匿名函数auto AddTraversePoint = [&] (const int& idx)

        这个匿名函数就没有那么复杂了，就是把传入的ind转成terrain_height_grid_下的坐标点cpos，然后用第一次发现该点的z值（cell里的第一个点的z值）去替换掉cpos的z值。接着把它从point3d类型转成pclpoint类型，并存到terrain_height_ptr_里，这个terrain_height_ptr_也是我们这个函数主要输出的东西。最后最后，把terrain_grid_traverse_list_[idx] = 1 代表可以通行。

        两个匿名函数讲完，我们接着讲：

         这个robot_idx我感觉很奇怪，因为robot_sub一直是固定的（44，44，0）

        我们来分析一下，因为在执行这个函数之前都会执行一次UpdateRobotPosition，在这里面，除了更新obs_indices和free_indices以外，还会更新TerrainHeightGridOrigin，也就是更新小车的odom在terrain_height_grid上的位置信息，那么再把它还原回robot_idx的时候，会用到origin信息，所以每次origin信息更新，再用origin信息反解sub，得到的sub都是44，44，0，因此用sub去解ind 得到的都是固定的3960。

        下面设置两个局部变量  q  和  visited_set，然后有意思的部分就来了：

        这个while循环是非常有意思的。初始的q一定不为零，因为它存了robot_idx，每次执行都会把q里前面的元素给pop掉。

        首先，判断这个从q来的cur_id在terrain_grid_occupy_list_[cur_id]的值是否为0，如果为零，那就判断is_robot_terrain_init是否为true，如果为true，就重新走while循环，不执行下面的步骤。

        若terrain_grid_occupy_list_[cur_id]为1，我们同样要判断is_robot_terrain_init，首次应该为fasle，那么!is_robot_terrain_init就为true。他就开始循环对应cur_id里的cell的z值，然后我们把terrain_height_grid_里的ref_id对应cell存的每一个z值都取出来，做一个小循环，把ref_id对应的每个z值与cur_h差值小于H_THRED的值都累加起来，当然counter也++。如果counter大于0，取均值，然后把cur_id对应的cell长度重置为1，把均值填进去，这个部分看过前面内容的应该都理解。接着我们就把cur_id放到匿名函数AddTraversePoint里，也就是把cur_id变成point并push进terrainHeightOut，并设置terrain_grid_traverse_list_里的cur_id位置为1  代表可以通行。接着把标志位is_robot_terrain_init = true，并清空q。如果is_robot_terrain_init为true，那么!is_robot_terrain_init就为false，就只执行AddTraversePoint(cur_id)。

        我们分析一下第一次走这个程序的时候，当我们的robot_idx也就是3960输入的时候，它对应的terrain_grid_occupy_list_[3960]为0，且第一次输入，is_robot_terrain_init也为false。

        那么它就会往下执行：

         这个部分就是把cur_id转换成terrain_height_grid_下的csub，然后对csub进行扩展，循环四次拓展四个ref_sub，再把符合要求的ref_sub的ref_ind给它push进q，然后接着进行while循环。

        比如 初始sub为（44，44，0），对应ind为3960。第一次循环完了为（43，44，0），对应的ind为3959；第二次循环为（44，45，0），对应4049，以此类推。

        以ref_sub为（43，44，0）为例，那么就要先判断ref_sub在不在terrain_height_grid_的范围内，如果在，就算出terrain_height_grid_下对应的ind。之前说了，是3959，然后我们要判断这个ind有没有在visited_set里出现过，并且is_robot_terrain_init为false或IsTraversableNeighbor(cur_id, ref_id)为true，首次来说，is_robot_terrain_init肯定为false，所以我们就把这个3959给push进q里。然后在visited_set里加入这个3539。

        visited_set就是用来怕我们重复包含同一个ref_id的，如果之后满足is_robot_terrain_init，那么就只能判断IsTraversableNeighbor(cur_id, ref_id)是否为true了。

        举一个输出的terrain_height_ptr_点为例(-0.8,0,0.239169 - 0) pos2sub以后为（43，44，0），sub2ind以后为3959

         到目前位置，UpdateTerrainHeightGrid的部分结束了。我们回到TerrainCallBack继续说：

        这个红框是我们刚讲完的部分，接着往下就是更新obscloud，这部分也很简单，就是根据neighbour_obs_indices来更新，往surround_obs_cloud_对应(neighbor_ind）的地方里追加cloud就完事了。

        动态障碍物的部分暂时不讲了。我们跳过去接着看。

        StackCloudByTime这部分的函数把原有的stack_new_cloud_和之前刚刚生成的cur_new_cloud_进行合并，并获取系统当前时间，把合并后的stack_new_cloud_里与当前系统时间差距1s以上的点全部去除掉。输出新的stack_new_cloud_。