點雲Octomap 及八叉樹解析

什麼是Octomap

Octomap採用八叉樹數據結構存儲三維環境的概率佔位地圖。在機器人領域的點雲地圖地圖一般都是通過軌跡優化,點雲拼接,最後構成地圖,這種做法簡單直接,也有一些缺陷:

1.地圖形式不緊湊。

點雲地圖通常規模很大,所以一個pcd文件也會很大。一張640480的圖像,會產生30萬個空間點,需要大量的存儲空間。即使經過一些濾波之後,pcd文件也是很大的。而且討厭之處在於,它的“大”並不是必需的。點雲地圖提供了很多不必要的細節。對於地毯上的褶皺、陰暗處的影子,我們並不特別關心這些東西。把它們放在地圖裡是浪費空間。

2.處理重疊的方式不夠好。

在構建點雲時,我們直接按照估計位姿拼在了一起。在位姿存在誤差時,會導致地圖出現明顯的重疊。例如一個電腦屏變成了兩個,原本方的邊界變成了多邊形。對重疊地區的處理方式應該更好一些。

3.難以用於導航

說起地圖的用處,第一就是導航啦!有了地圖,就可以指揮機器人從A點到B點運動,豈不是很方便的事?但是,給你一張點雲地圖,是否有些傻眼了呢?我至少得知道哪些地方可通過,哪些地方不可通過,才能完成導航呀!光有點是不夠的!

octomap就是為此而設計的!它可以優雅地壓縮、更新地圖,並且分辨率可調!它以八叉樹的形式存儲地圖,相比點雲,能夠省下大把的空間。octomap建立的地圖大概是這樣子的(從左到右是不同的分辨率)

Octomap的原理

八叉樹的表達

八叉樹可以理解為一個立方體切分為八塊,重複切分,直至不可切分為止。

每個小方塊都有一個數描述它是否被佔據。在最簡單的情況下,可以用0-1兩個數表示(太簡單了所以沒什麼用)。通常還是用0~1之間的浮點數表示它被佔據的概率。0.5表示未確定,越大則表示被佔據的可能性越高,反之亦然。由於它是八叉樹,那麼一個節點的八個孩子都有一定的概率被佔據或不被佔據啦!(下圖是一棵八叉樹)

Octomap的更新

八叉樹中的父親節點佔據概率,可以根據孩子節點的數值進行計算。比較簡單的是取平均值或最大值。