部落格轉自：灰信網
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

示例程式碼分析

Github原始碼地址：simple_example.cpp

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

using namespace std;
using namespace octomap;


void print_query_info(point3d query, OcTreeNode* node) {
  if (node != NULL) {
    cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
  }
  else 
    cout << "occupancy probability at " << query << ":\t is unknown" << endl;    
}

int main(int argc, char** argv) {

  cout << endl;
  cout << "generating example map" << endl;

  OcTree tree (0.1);  // create empty tree with resolution 0.1


  // insert some measurements of occupied cells

  for (int x=-20; x<20; x++) {
    for (int y=-20; y<20; y++) {
      for (int z=-20; z<20; z++) {
        point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
        tree.updateNode(endpoint, true); // integrate 'occupied' measurement
      }
    }
  }

  // insert some measurements of free cells

  for (int x=-30; x<30; x++) {
    for (int y=-30; y<30; y++) {
      for (int z=-30; z<30; z++) {
        point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
        tree.updateNode(endpoint, false);  // integrate 'free' measurement
      }
    }
  }

  cout << endl;
  cout << "performing some queries:" << endl;
  
  point3d query (0., 0., 0.);
  OcTreeNode* result = tree.search (query);
  print_query_info(query, result);

  query = point3d(-1.,-1.,-1.);
  result = tree.search (query);
  print_query_info(query, result);

  query = point3d(1.,1.,1.);
  result = tree.search (query);
  print_query_info(query, result);


  cout << endl;
  tree.writeBinary("simple_tree.bt");
  cout << "wrote example file simple_tree.bt" << endl << endl;
  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;  

}
 

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

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

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

OcTree tree(0.1)

建立OcTree地圖物件,並設定八叉樹解析度為0.1米，即訪問地圖的最小單位為0.1米；。

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()

OcTreeNode* result = tree.search (query)

節點資訊查詢函式；函式宣告是這樣的

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

cout << "occupancy probability at " << query<< ":\t " << node->getOccupancy() << endl；

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

Occupancy probability

關於這個佔用概率值，高博在一篇部落格中做出了和通俗易懂的解釋，我就不再複製貼上了，下面附上連線半閒居士
我要說的是，對於一個最小單位的柵格，如何判斷updateNode()函式對其做出了貢獻或者說更新。就比如說

point3d endpoint( 4.05f, 4.05f, 4.05f );
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，這也驗證了高博那篇博文中提到的最大值和最小值限制。

ColorOcTree

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

int main(int argc, char** argv)
{
 //解析度
double res = 0.05;  // create empty tree with resolution 0.05(different from default 0.1 for test)
 
//建立彩色地圖物件
 ColorOcTree tree (res);
 // insert some measurements of occupied cells
for (int x=-20; x<20; x++)
{
for (int y=-20; y<20; y++)
{
 for (int z=-20; z<20; z++)
 { 
point3d endpoint ((float)x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
  ColorOcTreeNode* n =tree.updateNode(endpoint, true);
  //設定節點顏色資訊的函式，每個地圖節點差五個畫素大小，漸變
  n->setColor(z*5+100,x*5+100,y*5+100);
  }
  }
 }
  // insert some measurements of free cells
  for (int x=-30; x<30; x++)
 {  
for (int y=-30; y<30; y++)
  {
  for (int z=-30; z<30; z++)
  {
  point3d endpoint ((float) x*0.02f+2.0f,(float) y*0.02f+2.0f, (float) z*0.02f+2.0f);
  ColorOcTreeNode* n =tree.updateNode(endpoint, false);
  //不被佔用的節點設定為黃色
 n->setColor(255,255,0); // set colorto yellow
  }
  }
 }
// set inner node colors
 tree.updateInnerOccupancy();
 }

  cout << endl;
  tree.writeBinary("simple_tree.bt");
  cout << "wrote example file simple_tree.bt" << endl << endl;
  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;  

}

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

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