Elevation_map是anybotics發(fā)布的針對多足機器人構建局部地圖的算法。但在計算速度上，elevation_map速度較慢，難以符合融合更新的實時性要求。因此在算法上進行改進。

ray tracing

slam框架下的點云更新對于新增的點可以輕易添加到地圖中，而對消失的點無法輕易判斷。因此主要通過光線追蹤的方法來判斷當前時刻點云地圖中的某些點是否消失，其原理主要是當前幀中傳感器與地面上的每一點都會形成光路，這條光路上所有點都會有高度限制，若上一幀地面點的高度超過該高度限制，則表示該點物體消失。

ray tracing

? 方法一：對于新采樣地圖中所有的高度點，與傳感器所在的位置行成光路，計算光路上每個點具有的高度限制。最后迭代地圖中所有點，與上一幀高度圖比較，移除與視覺約束矛盾的點。

? 方法二：對于新采樣地圖中所有的高度點，與傳感器所在的位置行成光路，以光線在二維平面映射的角度為Key，以（x,y,z）射線形式存儲。遍歷上一幀高度圖，計算每一點與傳感器所在位置在二維平面行成的角度(每1度為最小單位)，將已存儲擁有相同角度的射線與之比較，計算是否有視覺約束矛盾。

? 方法三：將multimap存儲形式改成哈希表。

源碼使用了方法1，但存在計算冗余情況，因此提出方法2，同時在方法2的基礎上改進數(shù)據存儲方式，使用哈希表而提速。統(tǒng)計對地圖進行一次ray tracing所使用的時間，統(tǒng)計結果如下（單位s）：

ray tracing不同方法耗時結果

通過統(tǒng)計計算時間，發(fā)現(xiàn)桎梏Elevation map速度的并不是ray tracing計算時間，因此要統(tǒng)計各部分運算時間。

運行時間分析

Elevation map運行的流程如下：
? 實時傳入點云處理
-1.更新地圖位置
-2.更新機器人動作變化中的地圖方差
-3.處理點云（結合機器人運動方差和傳感器測量方差計算每個點方差）
-4.將點云加入地圖，并更新
-5.發(fā)布原始地圖

? 地圖融合（fuse）

? 光線跟蹤（ray tracing)

地圖融合和光線追蹤部分是由定時器觸發(fā)，但其中因為各步驟對于Raw_map的使用，需要進行進行線程的堵塞，保證該步驟在使用Raw_map數(shù)據時，其他步驟不會對該數(shù)據進行更新。
統(tǒng)計計算時間發(fā)現(xiàn)，處理點云中需要結合機器人移動方差和傳感器測量方差計算每個點方差，因為涉及矩陣運算且運算點云數(shù)量多，計算時間大概需要0.4s左右。因此該部分使用GPU加速。

GPU加速

GPU加速需要編寫.cu文件，并有如下步驟。
a. 使用cuda并行處理，開辟和點云數(shù)相同數(shù)量的線程。
b. 將計算常量拷貝到GPU緩存。
c. 每個線程并行計算。

GPU加速效果

通過實驗發(fā)現(xiàn)，GPU大大提升了運行速度，可是elevation的實時傳入點云處理部分達到8HZ左右（加速前2HZ），使用光線追蹤的visibilitityCleanup達到4-5hz，基本可以滿足實時的要求。