Elevation_map是anybotics發(fā)布的針對多足機(jī)器人構(gòu)建局部地圖的算法。但在計(jì)算速度上，elevation_map速度較慢，難以符合融合更新的實(shí)時(shí)性要求。因此在算法上進(jìn)行改進(jìn)磺樱。
ray tracing
slam框架下的點(diǎn)云更新對于新增的點(diǎn)可以輕易添加到地圖中，而對消失的點(diǎn)無法輕易判斷婆咸。因此主要通過光線追蹤的方法來判斷當(dāng)前時(shí)刻點(diǎn)云地圖中的某些點(diǎn)是否消失竹捉，其原理主要是當(dāng)前幀中傳感器與地面上的每一點(diǎn)都會(huì)形成光路，這條光路上所有點(diǎn)都會(huì)有高度限制擅耽，若上一幀地面點(diǎn)的高度超過該高度限制活孩，則表示該點(diǎn)物體消失。
? 方法一：對于新采樣地圖中所有的高度點(diǎn)乖仇，與傳感器所在的位置行成光路憾儒，計(jì)算光路上每個(gè)點(diǎn)具有的高度限制。最后迭代地圖中所有點(diǎn)乃沙，與上一幀高度圖比較起趾，移除與視覺約束矛盾的點(diǎn)。
? 方法二：對于新采樣地圖中所有的高度點(diǎn)警儒，與傳感器所在的位置行成光路训裆，以光線在二維平面映射的角度為Key，以（x,y,z）射線形式存儲(chǔ)蜀铲。遍歷上一幀高度圖边琉，計(jì)算每一點(diǎn)與傳感器所在位置在二維平面行成的角度(每1度為最小單位)，將已存儲(chǔ)擁有相同角度的射線與之比較记劝，計(jì)算是否有視覺約束矛盾变姨。
? 方法三：將multimap存儲(chǔ)形式改成哈希表。
源碼使用了方法1厌丑，但存在計(jì)算冗余情況定欧，因此提出方法2，同時(shí)在方法2的基礎(chǔ)上改進(jìn)數(shù)據(jù)存儲(chǔ)方式怒竿，使用哈希表而提速砍鸠。統(tǒng)計(jì)對地圖進(jìn)行一次ray tracing所使用的時(shí)間，統(tǒng)計(jì)結(jié)果如下（單位s）：
通過統(tǒng)計(jì)計(jì)算時(shí)間耕驰，發(fā)現(xiàn)桎梏Elevation map速度的并不是ray tracing計(jì)算時(shí)間爷辱，因此要統(tǒng)計(jì)各部分運(yùn)算時(shí)間。
運(yùn)行時(shí)間分析
Elevation map運(yùn)行的流程如下：
? 實(shí)時(shí)傳入點(diǎn)云處理
-1.更新地圖位置
-2.更新機(jī)器人動(dòng)作變化中的地圖方差
-3.處理點(diǎn)云（結(jié)合機(jī)器人運(yùn)動(dòng)方差和傳感器測量方差計(jì)算每個(gè)點(diǎn)方差）
-4.將點(diǎn)云加入地圖，并更新
-5.發(fā)布原始地圖
? 地圖融合（fuse）
? 光線跟蹤（ray tracing)
地圖融合和光線追蹤部分是由定時(shí)器觸發(fā)饭弓，但其中因?yàn)楦鞑襟E對于Raw_map的使用巩检，需要進(jìn)行進(jìn)行線程的堵塞，保證該步驟在使用Raw_map數(shù)據(jù)時(shí)示启，其他步驟不會(huì)對該數(shù)據(jù)進(jìn)行更新。
統(tǒng)計(jì)計(jì)算時(shí)間發(fā)現(xiàn)领舰，處理點(diǎn)云中需要結(jié)合機(jī)器人移動(dòng)方差和傳感器測量方差計(jì)算每個(gè)點(diǎn)方差夫嗓，因?yàn)樯婕熬仃囘\(yùn)算且運(yùn)算點(diǎn)云數(shù)量多，計(jì)算時(shí)間大概需要0.4s左右冲秽。因此該部分使用GPU加速舍咖。
GPU加速
GPU加速需要編寫.cu文件，并有如下步驟锉桑。
a. 使用cuda并行處理排霉，開辟和點(diǎn)云數(shù)相同數(shù)量的線程。
b. 將計(jì)算常量拷貝到GPU緩存民轴。
c. 每個(gè)線程并行計(jì)算攻柠。
通過實(shí)驗(yàn)發(fā)現(xiàn)，GPU大大提升了運(yùn)行速度后裸，可是elevation的實(shí)時(shí)傳入點(diǎn)云處理部分達(dá)到8HZ左右（加速前2HZ）瑰钮，使用光線追蹤的visibilitityCleanup達(dá)到4-5hz，基本可以滿足實(shí)時(shí)的要求微驶。
