octomap 2. 原理以及原始碼分析
部落格轉自：DJ_Dreamaker
原理介紹
Octomap本身的理論基礎並不複雜，概率更新只是一個簡單的二值貝葉斯濾波過程，Octomap原文也給出了貝葉斯濾波的最終結果即遞推的概率更新公式。對貝葉斯濾波的推導過程感興趣的同學可以參考弗萊堡大學的課件或者閱讀《概率機器人》ch4.2。
這裡簡單的提一下Octomap工程實現時與八叉樹節點概率更新相關的幾個重要引數設定函式。
setProHit/setProMiss這兩個函式決定了inverse sensor model的log-odd概率更新的具體引數，預設情況下佔據三維體元被擊中（Hit）的概率值為0.7對應的log-odd為0.85，空閒體元被穿越（traverse）的概率值為0.4對應的log-odd為-0.4。可以呼叫getProHit/getProHitLog、getProMiss/getProMissLog檢視預設的引數設定。setClampingThresMax/setClampingThresMin這兩個函式決定了一個體元執行log-odd更新的閾值範圍。也就是說某一個佔據體元的概率值爬升到0.97（對應的log-odd為3.5）或者空閒體元的概率值下降到0.12（對應的log-odd為-2）便不再進行logodd更新計算。可以呼叫getClampingThresMax/getClampingThresMaxLog、getClampingThresMin/getClampingThresMinLog檢視預設的引數設定。setOccupancyThres這個函式定義了octomap判定某一個體元屬於佔據狀態的閾值（isNodeOccupied函式），預設是0.5，一般情況下我們將其設定為0.7。
由於log-odd與概率值之間可以相互轉化，因此在工程實現時octomap八叉樹節點類OcTreeNode儲存的數值是log-odd數值，並不是概率值。
Octomap基本資料型別解析
octomap::Vector3
octomap自定義的基本資料型別，API文件解釋如下：
This class represents a three-dimensional vector. The three-dimensional vector can be used to represent a translation in three-dimensional space or to represent the attitude of an object using Euler angle.
也就是說，這個資料結構給我們提供了octomap名稱空間下DIY的三維向量表達，它可以表達三維空間中的一個平移向量，也可以表達以尤拉角形式表示的三維物體。另外，這裡需要注意到octomath::Vector3的提供了多個與向量運算相關的函式，例如相對於座標系的變換實現函式、向量歸一化函式、向量點積函式等。
octomap::point3d
typedef octomath::Vector3 octomap::point3d
顯然，point3d是Octomap自定義的類型別名，原始的資料結構還得檢視octomath::Vector3
octomap::OcTreeNode
API文件解釋如下：
Nodes to be used in OcTree. They represent 3d occupancy grid cells. "value" stores their log-odds occupancy.
octomap::OcTreeKey
octomap自定義的基本資料型別，API文件解釋如下：
OcTreeKey is a container class for internal key addressing.The keys count the number of cells (voxels) from the origin as discrete address of a voxel.
也就是說，這個資料結構是一個關鍵字，它可以實現對八叉樹節點OcTreeNode的關鍵字查詢（也就是下面我們即將提到的無序關聯容器unordered_set，C++11標準）。
在2D鐳射SLAM中，我們通常可以生成一張二維柵格地圖，依據地圖解析度將一個二維空間點對映成一個二維柵格點，利用Occupancy grid mapping with known poses實現二維柵格地圖的概率更新。Octomap其實就是從二維空間擴充套件到三維空間中了，使用的是八叉樹資料結構（回顧下Octomap是什麼：採用八叉樹資料結構儲存三維環境的概率佔據地圖），OcTreeKey關鍵字查詢的就是離散的三維體元所處的空間地址，OcTreeKey定義的成員變數也說明了這一點。
key_type k [3]
其中key_type是一個類型別名
typedef uint16_t octomap::key_type
octomap::KeySet
typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> octomap::KeySet
API文件解釋如下：
Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
顯然，KeySet是Octomap自定義的類型別名！它使用一個無序關聯容器（std::unordered_set/boost::unordered_set）來儲存一組關鍵字集合，由雜湊函式來組織。雜湊函式也稱雜湊函式，直觀上，它是一個對映函式f，實現的功能為：記憶體中記錄的儲存位置=f（關鍵字），關於雜湊函式的更多介紹，可以自行百度。在Octomap中，關鍵字對應的雜湊值由KeyHash結構體實現。
// OcTreeKey::KeyHash: Provides a hash function on Keys
struct KeyHash
{
size_t operator()(const OcTreeKey& key) const{
// a simple hashing function
// explicit casts to size_t to operate on the complete range
// constanst will be promoted according to C++ standard
return size_t(key.k[0]) + 1447*size_t(key.k[1]) + 345637*size_t(key.k[2]);
}
};
octomap::KeyRay
typedef std::vector< OcTreeKey >::const_iterator const_iterator
typedef std::vector< OcTreeKey >::iterator iterator
顯然，KeyRay是octomap自定義的類型別名。
實際使用時，KeyRay用於儲存單條光束在三維空間中raytracing的結果，KeySet收納所有光束（也即點雲資料）raytracing的結果。
octomap::KeyBoolMap
typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> octomap::KeyBoolMap
KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysBegin( ) const
KeyBoolMap::const_iterator octomap::OccupancyOcTreeBase< NODE >::changedKeysEnd( )const
API文件解釋如下：
Data structrure to efficiently track changed nodes as a combination of OcTreeKeys and a bool flag (to denote newly created nodes)
KeyBoolMap是一個無序關聯容器，實現高效的關鍵字-值的查詢。它同時定義了兩個迭代器，方便對“changed nodes”
遍歷。這個資料結構的設計具有一定的應用背景，這裡先不做過多介紹。
octomap::PointCloud
官網的解釋如下：
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
在資料結構的設計上，它包括了兩個受保護的成員變數
pose6d current_inv_transform
point3d_collection points
其中
typedef std::vector<octomath::Vector3> octomap::point3d_collection
這裡，points變數又是一個自定義的類型別名，可以看出octomap::PointCloud使用了vector容器儲存點雲資料。於是，很自然的在程式中需要從PCL點雲資料中提取Octomap點雲資料結構時，可以使用push_back壓入資料。另外，octomap點雲類同時也定義了兩個類型別名，方便我們使用迭代器訪問vector中的點雲資料
typedef point3d_collection::const_iterator octomap::Pointcloud::const_iterator
typedef point3d_collection::iterator octomap::Pointcloud::iterator
在成員函式的實現上，我們使用到了getPoint函式和rotate函式
point3d octomap::Pointcloud::getPoint(unsigned int i) const
void octomap::Pointcloud::rotate (double roll,double pitch,double yaw)
八叉樹工程實現解析
八叉樹基本型別
到這裡，我們已經在上面將Octomap最基本的資料型別全部列出並加以解釋，卻唯獨還沒有解釋八叉樹型別OcTree的資料結構。其實，可以認為OcTree八叉樹類是建立在以上所有資料結構基礎上的頂層類，並不是指其它所有型別都派生於它，而是它提供了操作以上所有資料結構的的方法（函式），也就是核心的函式都由八叉樹型別OcTree中提供。
通過閱讀octomap的github原始碼，發現核心程式碼的實現在octomap::OccupancyOcTreeBase、octomap::OcTreeBase兩個類中。
其中insertPointCloud函式由OccupancyOcTreeBase類實現。在目前的PC機上，github提供的原始碼實現是通過OpenMP平行計算完成的。當然，Octomap建圖還經常使用在無人機上，受限於計算裝置，機載處理器無法像在PC機上實現平行計算，因此ETH ASL實驗室的工程人員專門為此開發了適合於無人機應用的Octomap建圖程式包volumetric_mapping。
點雲插入函式insertPointCloud函式解析
從實現流程上來看，空間點雲raycasting到OcTree的過程大致分為三步
- 座標系轉換。需要使用pcl::transformPointCloud函式
- 光線追跡過程。需要使用computeRayKeys函式。強調一下，引數origin（光束起點）和引數end（感測器末端擊中點）都是世界座標系下的座標！
bool octomap::OcTreeBaseImpl< OcTreeNode , AbstractOccupancyOcTree >::computeRayKeys(const point3d & origin,const point3d & end,KeyRay & ray ) const
// Traces a ray from origin to end (excluding), returning an OcTreeKey of all nodes traversed by the beam.
- 概率更新。需要使用updateNode函式
virtual OcTreeNode * octomap::OccupancyOcTreeBase< OcTreeNode >::updateNode (const OcTreeKey & key,float log_odds_update,bool lazy_eval = false )
// Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
從工程實現來看，空間點雲八叉樹的實現基於C++11標準使用了大量C++ Primer中的資料結構，包括基本的順序容器如Vector以及關聯容器map、set（確切說是無序關聯容器unordered_map和unordered_set），對C++ Primer掌握程度要求比較高。
查詢相關函式解析
Octomap為我們提供了節點查詢函式search、iterator以及光線追跡查詢函式castRay，Octomap原作給出的原話：individual octree nodes can be accessed by searching for their coordinate. For efficient batch queries, our implementation provides iterators to traverse the octree analogous to a standard C++ container class. With these iterators, all nodes,leaf nodes, or leaf nodes in a certain bounding box can be queried or they can be filtered according to further criteria.
Ray intersection queries, i.e., casting a ray from an origin into a given direction until it hits an occupied volume,
are an important use-case for a 3D map in robotics. This kind of query is used for visibility checks or to localize with range sensors. Thus, we provide this functionality in the castRay(·) method.
- 批量查詢迭代器：leaf_iterator。迭代器批量查詢所有的葉子節點的狀態，跳過所有的Inner nodes節點。可以檢視官網給出的使用例程！
- 單節點查詢：search、isNodeOccupied。isNodeOccupied函式：queries whether a node is occupied according to the tree's parameter for "occupancy"
- 光線追跡查詢：castRay。castRay函式中引數origin（光束起點）是世界座標系下sensor（可以是RGBD感測器、也可以是三維鐳射雷達）的位置；引數direction也就是光束的方向向量，只需要給出sensor model光線的方向向量即可，且沒有必要對方向向量歸一化，castRay函式在內部會為我們完成這件事，返回的光束末端點end是世界座標系下的座標表達。再次強調一下，上面我們提到的光線追跡computeRayKeys函式的引數origin（光束起點）和引數end（感測器末端擊中點）都是世界座標系下的表達！
virtual bool octomap::OccupancyOcTreeBase< OcTreeNode >::castRay(const point3d & origin,
const point3d & direction,
point3d & end,
bool ignoreUnknownCells = false,
double maxRange = -1.0
)const
例項
最後，以volumetric_mapping為例，給出序列計算模式下insertPointCloud函式的實現過程程式碼解析（程式碼略有改動！）
// T_G_sensor：4*4的齊次變換矩陣，儲存相機相對於世界座標系的實時位姿態
// cloud ：PCL點雲資料
void NbvPlannerROS::insertPointcloudIntoMapImpl(const Eigen::Matrix4f& T_G_sensor,const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& cloud)
{
// Remove NaN values, if any.
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
pcl::transformPointCloud(*cloud, *cloud, T_G_sensor);
const octomap::point3d p_G_sensor = octomap::point3d( T_G_sensor (0,3), T_G_sensor (1,3), T_G_sensor (2,3));
// 關聯容器也可以認為是“關聯陣列”，只是它相比陣列可以使用關鍵字來查詢相應的值。
// 關於關聯容器的講述，見C++ Primer 374頁和376頁最重要！！！ 關於關聯容器與順序容器的區別，請見C++ Primer 381頁和388頁，注意到find函式和insert函式都是關聯容器所獨有的！！！關聯容器並不具備與位置操作相關的push_back等函式
// 我們這裡的是無序關聯容器，除了雜湊管理操作之外，無序容器提供了與有序容器相同的操作，例如insert、find等
octomap::KeySet free_cells, occupied_cells; // 使用雜湊函式組織的set API文件對該資料結構的定義：Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
int i = 0;
for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud->begin();it != cloud->end(); ++it)
{
const octomap::point3d p_G_point(it->x, it->y, it->z);
// 區別：首先檢查endpoint是否已經是occupied_cells的關鍵字成員了
octomap::OcTreeKey key = m_octomap.coordToKey(p_G_point);
if (occupied_cells.find(key) == occupied_cells.end()) // 如果給定關鍵字存在於set中則find返回的迭代器指向該關鍵字，否則返回尾後迭代器。find是關聯容器特有的函式
{
castRay(p_G_sensor, p_G_point, &free_cells, &occupied_cells);
i++;
}
}
cout<<"if find, time is: "<<i<<endl;
updateOccupancy(&free_cells, &occupied_cells);
for (pcl::PointCloud<pcl::PointXYZRGBA>::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
{
m_octomap.integrateNodeColor( it->x, it->y, it->z, it->r, it->g, it->b );
}
m_octomap.updateInnerOccupancy();
}
void NbvPlannerROS::castRay(const octomap::point3d& sensor_origin,
const octomap::point3d& point,
octomap::KeySet* free_cells,
octomap::KeySet* occupied_cells) const {
// CHECK_NOTNULL(free_cells);
// CHECK_NOTNULL(occupied_cells);
if ( (point - sensor_origin).norm() <= 4.0 )
{
octomap::KeyRay key_ray;
if (m_octomap.computeRayKeys(sensor_origin, point, key_ray))
{
free_cells->insert(key_ray.begin(), key_ray.end()); // key_ray.begin(), key_ray.end()體現的是順序容器迭代器的基本用法;
}
octomap::OcTreeKey key;
if (m_octomap.coordToKeyChecked(point, key))
{
occupied_cells->insert(key); //同樣還是insert操作，只不過是insert操作的不同實現方式，見C++ Primer 384頁
}
}
else
{
octomap::point3d new_end = sensor_origin + (point - sensor_origin).normalized() * 4.0;
octomap::KeyRay key_ray;
if (m_octomap.computeRayKeys(sensor_origin, new_end, key_ray))
{
free_cells->insert(key_ray.begin(), key_ray.end());
}
}
}
void NbvPlannerROS::updateOccupancy(octomap::KeySet* free_cells, octomap::KeySet* occupied_cells)
{
// CHECK_NOTNULL(free_cells);
// CHECK_NOTNULL(occupied_cells);
// Mark occupied cells.
for (octomap::KeySet::iterator it = occupied_cells->begin(),end = occupied_cells->end();it != end; it++) // （無序）關聯容器迭代器的使用，見C++ Primer 382頁
{
m_octomap.updateNode(*it, true);
// Remove any occupied cells from free cells - assume there are far fewer
// occupied cells than free cells, so this is much faster than checking on
// every free cell.
if (free_cells->find(*it) != free_cells->end()) // find操作，它是（無序）關聯容器所特有的函式，訪問容器中的指定元素。如果給定關鍵字存在於set中則find返回的迭代器指向該關鍵字，否則返回尾後迭代器
{
free_cells->erase(*it); // erase操作，刪除容器中的指定元素。
}
}
// Mark free cells.
for (octomap::KeySet::iterator it = free_cells->begin(),end = free_cells->end();it != end; ++it)
{
m_octomap.updateNode(*it, false);
}
// m_octomap.updateInnerOccupancy();
}