OctoMap 八叉樹核心原理：OctoKey 生成與點雲體素映射詳解

OctoMap 與八叉樹基礎

OctoMap 是一款基於八叉樹的 3D 佔據圖映射框架，廣泛應用於機器人導航、自動駕駛等領域。其核心在於將空間遞歸劃分為八個子體素，實現高效的空間索引與查詢。

OctoKey 生成原理

Morton 編碼（Z-Order Curve）

OctoKey 本質上是將 3D 座標 (x, y, z) 通過 Morton 編碼 映射為一個 64 位整數。具體步驟：

1. 將座標量化為整數索引（根據樹深度和分辨率）。
2. 將三個整數的二進制位交織：xyzxyzxyz...。
3. 得到的二進制串即為 OctoKey，唯一標識一個體素節點。

這種編碼保證了空間鄰近性：鄰近體素的 Key 數值也接近，利於緩存與範圍查詢。

Parent Key 與 Child Key 的關係

• Parent Key：父節點的 OctoKey，對應較大體素。
• Child Key：子節點 Key，由 Parent Key 左移 3 位並加上子節點索引（0-7）生成。
• center_offset_key：用於計算子節點中心偏移的輔助常量，實際是 1 << (depth - 1) 的變體。

遞歸公式：child_key = (parent_key << 3) | child_index。

點雲到體素的映射過程

空間劃分邏輯

1. 確定根節點邊界框（包含所有點雲）。
2. 遞歸劃分：每個非空節點分裂為 8 子節點。
3. 停止條件：體素內無點、達到最大深度、或滿足佔據概率閾值。

座標轉體素索引

// 假設分辨率 resolution，原點 origin
int ix = floor((pt.x - origin.x) / resolution);
int iy = floor((pt.y - origin.y) / resolution);
int iz = floor((pt.z - origin.z) / resolution);
// 再將 (ix, iy, iz) 經 Morton 編碼得 OctoKey

實戰建議

• 閱讀源碼時重點關注 OcTreeKey、OcTreeNode 結構及 coordToKey、keyToCoord 方法。
• 調試時可打印 Key 的二進制，觀察空間層級關係。
• 大規模點雲建議啟用壓縮存儲（OcTreeCompressed）節省內存。

掌握 Morton 編碼與遞歸劃分邏輯，即可透徹理解 OctoMap 的核心機制。