octomap庫的一點總結

      前些天突然想起高博《視覺SLAM十四講》提到的octomap八叉樹地圖，在網上百度又只有高博寫的那麼一片博文，於是就想自己看看這個庫，瞭解其幾個基本的類和介面，以下是我這幾天來的總結，如果有錯誤請及時指出，或者聯絡我，我也會及時改正。（[email protected]）

一，官方的 Introduction部分翻譯(http://octomap.github.io/octomap/doc/index.html)

      Octomap庫實現了一種填充3D柵格的構圖方式，並提供了相應的資料結構和構圖演算法。整個地圖利用一個Octree的類來實現。

      直接檢視本庫主要的類octomap::OcTree，以及其示例程式src/octomap/simple_example.cpp。如果你想要將單組測量資料整合到3D地圖中，請檢視OcTree::insertRay(…)函式；如果你想要將整個3D掃描資料（點雲）整合到地圖中，請檢視OcTree::insertPointCloud(…)函式。同時可以通過OcTree::search(...) 和OcTree::castRay(...)函式來進行地圖空間佔用的查詢。推薦使用迭代器的方式對Octree的節點等進行查詢，例如leaf_iterator，tree_iterator和leaf_bbx_iterator。

二，示例程式碼分析(Github原始碼地址：https://github.com/OctoMap/octomap)

      在使用該庫時，主要使用了基本的OcTree類和帶有顏色資訊的ColorOcTree類，所以只簡單分析了這兩個類的示例程式碼。

1，octomap/octomap/src/simple_example.cpp

1.	#include <octomap/octomap.h>
2.	#include <octomap/OcTree.h>
3.	using namespace std;
4.	using namespace octomap;
5.	
6.	//查詢資訊輸出函式，輸入為octomap名稱空間下的3D點位置和用OcTree::search(...)得到的返回值
7.	void print_query_info(point3d query, OcTreeNode* node) 
8.	{
9.	    if (node != NULL) 
10.	    {
11.	       cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
12.	    }
13.	    else 
14.	      cout << "occupancy probability at " << query << ":\t is unknown" << endl;    
15.	}
16.	 // 主函式入口
17.	int main(int argc, char** argv) 
18.	{
19.	  cout << endl;
20.	  cout << "generating example map" << endl;
21.	  // 建立一個空的OcTree物件地圖，解析度為0.1
22.	  OcTree tree (0.1);  // create empty tree with resolution 0.1
23.	
24.	  // insert some measurements of occupied cells
25.	  // 向地圖中插入一些測量資料，這些資料點在地圖中表現為已佔用狀態
26.	  for (int x=-20; x<20; x++) 
27.	  {
28.	    for (int y=-20; y<20; y++) 
29.	    {
30.	      for (int z=-20; z<20; z++) 
31.	      {
32.	        //建立空間點物件，並向地圖中更新節點
33.	        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
34.	        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
35.	      }
36.	    }
37.	  }
38.	
39.	  // insert some measurements of free cells
40.	// 向地圖中插入一些測量資料，這些資料點在地圖中表現為已未佔用狀態
41.	  for (int x=-30; x<30; x++) 
42.	  {
43.	    for (int y=-30; y<30; y++) 
44.	    {
45.	      for (int z=-30; z<30; z++) 
46.	      {
47.	        //建立空間點物件，並向地圖中更新節點
48.	        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
49.	        tree.updateNode(endpoint, false);  // integrate 'free' measurement
50.	      }
51.	    }
52.	  }
53.	//上面已經完成了對地圖的建立，之後的程式是對地圖節點資料的訪問
54.	  cout << endl;
55.	  cout << "performing some queries:" << endl;
56.	 // 節點（0,0,0）
57.	  point3d query (0., 0., 0.);
58.	  OcTreeNode* result = tree.search (query);
59.	  print_query_info(query, result);
60.	// 節點（-1,-1,-1）
61.	  query = point3d(-1.,-1.,-1.);
62.	  result = tree.search (query);
63.	  print_query_info(query, result);
64.	// 節點（1,1,1）
65.	  query = point3d(1.,1.,1.);
66.	  result = tree.search (query);
67.	  print_query_info(query, result);
68.	  cout << endl;
69.	// 將建好的地圖儲存
70.	  tree.writeBinary("simple_tree.bt");
71.	  cout << "wrote example file simple_tree.bt" << endl << endl;
72.	  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
73.	  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;  
74.	}
 

      這是執行該程式碼後的執行結果：

圖表 1

      這是用octovis檢視所生成的地圖simple_tree.bt：

圖表 2

下面針對一些關鍵語句進行分析：

(1)OcTree tree(0.1);建立OcTree地圖物件。這個函式的宣告是這樣的：

resolution譯為“解析度”，即訪問地圖的最小單位；

(2)  point3d endpoint( (float)x*0.02f-1.0f, (float)y*0.02f-1.0f, (float)z*0.02f-1.0f);

tree.updateNode( endpoint, false );

建立一個三維空間點，並將其新增到地圖當中；point3d建構函式的三個引數分別是三維點的x/y/z座標；使用.updateNode()函式將空間點資訊更新到地圖當中，第二個引數表示該空間點對應的節點未被佔用(false);

(3)  OcTreeNode* result = tree.search (query);節點資訊查詢函式；函式宣告是這樣的

傳入引數為一個三維點物件，如果在這個地圖tree中，這個三維點對應的位置有節點（不管佔用與否），那麼將返回該位置的節點指標，否則將返回一個空指標；

(4)  cout << "occupancy probability at " << query<< ":\t " << node->getOccupancy() << endl；如果查詢到有該位置上節點的資訊，則使用getOccupancy()函式輸出該節點佔用情況，那為什麼這個Occupancy是個小數呢？這是因為Octomap在描述一個柵格是否被佔用時，並不是單一的只描述為佔用和被佔用，而是用一個概率（Occupancy probability）來描述它，即這個柵格被佔用的概率是多少，通過這個概率來確定這個柵格被佔用的可能性。

2，Occupancy probability

      關於這個佔用概率值，高博在一篇部落格中做出了和通俗易懂的解釋，我就不再複製貼上了，下面附上連線(https://www.cnblogs.com/gaoxiang12/p/5041142.html)。

      我要說的是，對於一個最小單位的柵格，如何判斷updateNode()函式對其做出了貢獻或者說更新。就比如說

1.           point3d endpoint( 4.05f, 4.05f, 4.05f );

2.           tree.updateNode( endpoint, true );

這兩行程式碼，由於解析度是0.1，並不能精確到0.01，所以要把這個（4.05,4.05,4.05）歸到（4.0,4.0,4.0）這個節點呢，還是（4.1,4.1,4.1）這個節點或者其他節點呢？經過我的多次測試，應當歸到（4.0,4.0,4.0）節點當中，也就是說，對於節點（4.0,4.0,4.0）來說，凡是同時滿足x=[4.0,4.1)，y=[4.0,4.1)，z=[4.0,4.1)的point，如果使用updateNode()函式將這個point更新到地圖當中，那麼必然會影響到節點（4.0,4.0,4.0）的Occupancy probability。

      總結就是，對於一個點point(x,y,z)，使用updateNode()函式將其更新到地圖當中，那麼Occupancy probability受到影響的節點將是

即小於point座標值的最大節點。同時當我們用search ()函式對point進行查詢時，返回的資訊也將是小於point座標值的最大節點資訊。

      另外，在對某個節點進行不同次數的更新之後，發現Occupancy probability最大值為0.971，最小值為0.1192，這也驗證了高博那篇博文中提到的最大值和最小值限制。

3，octomap/src/testing/test_color_tree.cpp

      帶有色彩的八叉樹地圖ColorOcTree與基本的OcTree類似，需要知道如何向節點當中新增顏色資訊就好了。下面是test_color_tree.cpp的部分程式碼：

1.   int main(int argc, char** argv)

2.   {

3.     //解析度

4.     double res = 0.05;  // create empty tree with resolution 0.05(different from default 0.1 for test)

5.     //建立彩色地圖物件

6.     ColorOcTree tree (res);

7.     // insert some measurements of occupied cells

8.     for (int x=-20; x<20; x++)

9.     {

10.      for (int y=-20; y<20; y++)

11.      {

12.        for (int z=-20; z<20; z++)

13.        {

14.          point3d endpoint ((float)x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);

15.          ColorOcTreeNode* n =tree.updateNode(endpoint, true);

16.          //設定節點顏色資訊的函式，每個地圖節點差五個畫素大小，漸變

17.          n->setColor(z*5+100,x*5+100,y*5+100);

18.        }

19.      }

20.    }

21.   

22.    // insert some measurements of free cells

23.    for (int x=-30; x<30; x++)

24.    {

25.      for (int y=-30; y<30; y++)

26.      {

27.        for (int z=-30; z<30; z++)

28.        {

29.          point3d endpoint ((float) x*0.02f+2.0f,(float) y*0.02f+2.0f, (float) z*0.02f+2.0f);

30.          ColorOcTreeNode* n =tree.updateNode(endpoint, false);

31.          //不被佔用的節點設定為黃色

32.          n->setColor(255,255,0); // set colorto yellow

33.        }

34.      }

35.    }

36.    // set inner node colors

37.    tree.updateInnerOccupancy();

38.  }

      這是執行該程式碼後的執行結果：

      這是用octovis檢視所生成的地圖simple_color_tree.ot：

三，安裝和編譯octomap庫時的一個小問題

      第一次編譯安裝octomap庫時，直接按照網上的教程進行的操作：

1.     git clone https://github.com/OctoMap/octomap

2.     cd octomap

3.     mkdir build

4.     cd build

5.     cmake ..

6.     make

7.     sudo make install

可是在執行make指令時卻出現了undefined reference to的錯誤

在網上百度了好久，找到了關於這個問題的帖子（https://github.com/OctoMap/octomap/issues/171）

1，        我首先用的方法是 sudo make，確實，編譯通過，也安裝成功了，但是我在編譯自己寫的octomap_test程式時，仍然會出現類似的undefined reference to的錯誤，於是在編譯時還是需要sudo；

2，        後來我覺得有一個對方法1的評價特別對：

於是我嘗試了第二種做法：

開啟查詢了這個/opt/ros/indigo/share/octpmap/octomap-config-version檔案，我的版本是1.6.9，在重新下載了octomap原始碼之後，校對了版本，具體指令如下：

1.     git clone https://github.com/OctoMap/octomap

2.     cd octomap

3.     git tag   //列出所有版本，檢視是否有自己的版本

4.     git checkout v1.6.9   //校驗為自己的版本

5.     mkdir build

6.     cd build

7.     cmake ..

8.     make

9.     sudo make instal

感覺第二種做法才是真正的解決方案。