SLAM 灰灰restudy及查漏補缺—octomap

因為階段學習需要，本篇先了解一個建圖工具octomap的相關內容。

1.octomap是什麼

    RGBD SLAM的目的有兩個：估計機器人的軌跡，並建立正確的地圖。octomap以八叉樹（octotree）的形式儲存地圖，相比點雲，能夠省下極大空間。點雲地圖提供了太多不必要的細節，佔據大量儲存空間，且在導航時無法提供“能否通過”的判斷資訊。

     octomap建立的地圖大概是這樣子的：（從左到右是不同的解析度）

                                   

      它的地影像是很多個小方塊組成的。解析度較高時，方塊很小；解析度較低時，方塊很大。每個方塊表示該格被佔據的概率。因此可以查詢某個方塊或點“是否可以通過”，從而實現不同層次的導航。簡而言之，環境較大時採用較低解析度，而較精細的導航可採用較高解析度。

---

2.octomap的原理（數學表達）

2.1八叉樹的表達

八叉樹即是有八個子節點的樹，八塊示意圖如下圖所示。實際的資料結構就是一個樹根不斷往下擴充套件，每分8個小枝，直到葉子為止。葉子節點代表了解析度最高的情況。如解析度為0.01cm，則每個葉子就是1cm見方的小方塊。

                                                                    

每個小方塊都有一個描述它是否被佔據的數，通常用0-1之間的浮點數表示它被佔據的概率，0.5表示未確定，越大表示被佔據的可能性越高。下圖是一棵八叉樹：

                                                               

用樹結構的好處是：當某個節點的子結點都“佔據”或“不佔據”或“未確定”時，就可以把它（節點）給剪掉！換句話說，如果沒必要進一步描述更精細的結構（子節點）時，我們只要一個粗方塊（父節點）的資訊就夠了。這可以省去很多的儲存空間。不用存一個“全八叉樹”！

2.2八叉樹的更新

八叉樹用概率來表達葉子是否被佔據，而不直接用0/1來表示，因為環境本身有動態特性，另外還有噪聲，導致對節點的狀態觀測會出現不同的佔據情況。

                           

先介紹logit變換，它是把一個概率p變換到全實數空間R上：

                                                            

其逆變換為：

                                                        

 

α叫做log-odds。

每新來一個就直接加到原來的上面。此外還要加一個最大最小值的限制。最後轉換回原來的概率即可。

八叉樹中的父親節點佔據概率，可以根據孩子節點的數值進行計算。比較簡單的是取平均值或最大值。如果把八叉樹按照佔據概率進行渲染，不確定的方塊渲染成透明的，確定佔據的渲染成不透明的，就能看到我們平時見到的那種東西了！它的視覺化做的比較好。

---

3.程式碼及安裝

這一塊不去講怎麼安裝octomap,只介紹octomap的程式碼主要含有2個模組本身的octomap和視覺化工具octovis.且主要只記錄高博介紹的3個轉換例程。

3.1:轉換pcd到octomap

首先將一個pcd格式的點雲地圖轉換為octomap地圖。

原始碼地址：https://github.com/gaoxiang12/octomap_tutor

3.2：加入色彩資訊

3.3：更好的拼接與轉換

 

以上均整理自高博的部落格：https://www.cnblogs.com/gaoxiang12/p/5041142.html