Elevation_map是anybotics发布的针对多足机器人构建局部地图的算法。但在计算速度上，elevation_map速度较慢，难以符合融合更新的实时性要求。因此在算法上进行改进。

ray tracing

slam框架下的点云更新对于新增的点可以轻易添加到地图中，而对消失的点无法轻易判断。因此主要通过光线追踪的方法来判断当前时刻点云地图中的某些点是否消失，其原理主要是当前帧中传感器与地面上的每一点都会形成光路，这条光路上所有点都会有高度限制，若上一帧地面点的高度超过该高度限制，则表示该点物体消失。

ray tracing

• 方法一：对于新采样地图中所有的高度点，与传感器所在的位置行成光路，计算光路上每个点具有的高度限制。最后迭代地图中所有点，与上一帧高度图比较，移除与视觉约束矛盾的点。

• 方法二：对于新采样地图中所有的高度点，与传感器所在的位置行成光路，以光线在二维平面映射的角度为Key，以（x,y,z）射线形式存储。遍历上一帧高度图，计算每一点与传感器所在位置在二维平面行成的角度(每1度为最小单位)，将已存储拥有相同角度的射线与之比较，计算是否有视觉约束矛盾。

• 方法三：将multimap存储形式改成哈希表。

源码使用了方法1，但存在计算冗余情况，因此提出方法2，同时在方法2的基础上改进数据存储方式，使用哈希表而提速。统计对地图进行一次ray tracing所使用的时间，统计结果如下（单位s）：

ray tracing不同方法耗时结果

通过统计计算时间，发现桎梏Elevation map速度的并不是ray tracing计算时间，因此要统计各部分运算时间。

运行时间分析

Elevation map运行的流程如下：
• 实时传入点云处理
-1.更新地图位置
-2.更新机器人动作变化中的地图方差
-3.处理点云（结合机器人运动方差和传感器测量方差计算每个点方差）
-4.将点云加入地图，并更新
-5.发布原始地图

• 地图融合（fuse）

• 光线跟踪（ray tracing)

地图融合和光线追踪部分是由定时器触发，但其中因为各步骤对于Raw_map的使用，需要进行进行线程的堵塞，保证该步骤在使用Raw_map数据时，其他步骤不会对该数据进行更新。
统计计算时间发现，处理点云中需要结合机器人移动方差和传感器测量方差计算每个点方差，因为涉及矩阵运算且运算点云数量多，计算时间大概需要0.4s左右。因此该部分使用GPU加速。

GPU加速

GPU加速需要编写.cu文件，并有如下步骤。
a. 使用cuda并行处理，开辟和点云数相同数量的线程。
b. 将计算常量拷贝到GPU缓存。
c. 每个线程并行计算。

GPU加速效果

通过实验发现，GPU大大提升了运行速度，可是elevation的实时传入点云处理部分达到8HZ左右（加速前2HZ），使用光线追踪的visibilitityCleanup达到4-5hz，基本可以满足实时的要求。