很多做机器人导航、三维重建或者移动抓取的同学应该都遇到过类似的问题点云数据量很大但真正有信息量的物理边界却很少相邻几帧点云稍微带一点噪声地图上就会出现“一团雾”一样的假障碍等到环境长距离变化后又希望地图能根据新观测自动更新。这些问题背后其实都指向一个共同点——地图的数据结构和概率更新策略决定了建图系统能否长期稳定运行。这篇文章要系统讲清楚的就是基于八叉树Octrees的概率三维建图框架 OctoMap。文章会覆盖它的设计动机、概率更新原理、核心数据表示、安装配置、可直接运行的 C 示例以及如何把它接到 ROS 导航中生成二维占据栅格地图。无论你是刚接触三维建图的学生还是已经在做机器人项目落地的开发者都能从里面找到可以直接复用的思路和代码。1. 为什么需要 OctoMap从占据栅格地图说起1.1 三维地图中的“体素爆炸”问题接触过二维激光 SLAM 的同学一定很熟悉占据栅格地图Occupancy Grid Map的概念。它把环境划分成一个个等大小的方格每个格子记录三种状态空闲free、占据occupied、未知unknown。这样一张二维栅格地图下游的成本地图、路径规划都可以直接使用。但把这种思路直接扩展到三维马上会遇到问题如果使用固定大小的三维体素Voxel表示环境内存增长速度会非常恐怖。举一个粗糙但直观的例子假设我们要建图的范围是 100 米 × 100 米 × 20 米如果每个体素边长是 0.1 米那么体素数量是[ 1000 \times 1000 \times 200 200,000,000 ]也就是 2 亿个体素。哪怕每个体素只保存 1 个字节的状态也需要约 200 MB 内存。如果再保存概率值、时间戳或颜色内存会成倍增长。如果想把分辨率提高到 0.05 米体素数量会膨胀到 16 亿普通机器会直接卡死。更浪费的是绝大多数三维环境中真正被障碍物占据的空间其实是稀疏的。空旷的走廊、大片的室内地面、室外开放区域都很少产生“被占据”的体素。因此为整个空间均匀分配体素本身就不是一种经济、优雅的做法。1.2 OctoMap 提供了什么思路OctoMap 并没有放弃占据栅格地图的思想而是把它的存储结构从“均匀网格”换成了“八叉树”。八叉树是一种递归分割的空间数据结构。三维空间可以用一个大的立方体表示如果这个立方体内还没有足够的信息它可以被等分成 8 个子立方体每个子立方体又可以继续分割直到达到预设的分辨率。这样环境中的大面积空闲区域可以用较大的节点表示只有物体表面附近的区域才会被细分成小体素。结果就是稀疏区域占用极少的内存物体边界区域仍能保持分辨率级别的精度。另外OctoMap 是一个概率建图框架。它不只是简单地把一帧点云标记为“占据”或“空闲”而是利用贝叶斯更新思想把传感器噪声、多次观测信息融合到体素的占据概率中。因此即使在激光雷达有噪声、或者环境中存在动态物体的情况下OctoMap 也能维护一个相对稳定的概率地图。1.3 这篇文章的应用场景OctoMap 的典型使用场景包括移动机器人的三维环境感知与避障无人机、自动导引车的全局三维建图室内三维重建、点云后处理机器人导航之前的地图预处理在线更新环境地图用于动态避障。需要说明的是OctoMap 本身并不负责 SLAM 中的位姿估计。它通常作为建图后端或地图表示模块接收已经配准好的点云不断把点云更新到地图中。理解了这一点就不会把 OctoMap 和 LOAM、ORB-SLAM 这类完整 SLAM 系统搞混。2. OctoMap 核心原理拆解2.1 八叉树的节点和空间划分在 OctoMap 中每个节点对应空间中的一个立方体。根节点表示整个建图空间。根节点可以被均匀分割成 8 个子节点每个子节点代表一个更小的立方体。这个过程可以递归进行直到子节点对应的立方体边长小于或等于用户设定的分辨率resolution。这里的分辨率是 OctoMap 最重要的参数之一。如果你设置分辨率为 0.1 米那么地图中最小的叶子节点就是边长 0.1 米的立方体。想要更精细的地图可以调小分辨率但内存和计算开销会显著增加想要更轻量的地图可以调大分辨率但地图可能会丢失细小物体结构。八叉树的优势不只是节省内存还体现在节点剪枝pruning上。当一个内部节点的 8 个子节点状态一致时可以把这些子节点合并到父节点中删除子节点从而进一步降低存储开销。在一面平整的墙壁上远离边缘的大片区域往往会被合并成几个较大的父节点而不是成千上万个小叶子节点。2.2 概率更新模型如果只保存“占据/空闲”二值状态传感器噪声很容易导致误判。OctoMap 在每个叶子节点上保存的不是二值状态而是一个对数几率log-odds值。假设某个叶子节点为占据的概率是 (p)那么对应的对数几率是[ l \log \frac{p}{1-p} ]当传感器给出一帧观测后OctoMap 会根据这个观测更新节点上的对数几率。由于对数几率可以避免大量概率乘法更新过程变得非常快。实际实现中更新通常带有截断机制不让概率无限接近 0 或 1也避免概率值溢出。当我们判定一个体素为“空闲”或者“占据”时并不是直接把它设置为 0 或 1而是按照观测模型提高或降低对应的对数几率。一帧观测可能只让概率从 0.5 变成 0.6但连续多帧观测都在同一位置这个节点的占据概率就会逐步接近一个较高值。这种概率累计方式让 OctoMap 对传感器噪声有天然的忍耐力。第一次观测某个节点时它通常保持“未知”状态概率为默认的 0.5。随着射线穿过空间射线经过的节点会被标记为“空闲”射线终点对应的节点会被标记为“占据”。如果环境中的某个障碍物消失了相同区域的后续射线会把它更新为“空闲”概率逐渐降低从而让地图能够适应环境动态变化。2.3 空闲、占据和未知的处理OctoMap 地图中并不是所有节点都有保存值。一个从未被观测到的区域通常不存在对应节点。遍历地图时你只能看到已经被更新过的区域。这样可以有效区分“未知区域”和“空闲区域”如果地图中不存在某个位置的节点视为未知如果节点存在且占据概率较低视为空闲如果节点存在且占据概率较高视为占据。在导航中未知和空闲的处理方式完全不同。未知区域往往会被规划算法视为不可通行而空闲区域可以通过。这也是 OctoMap 在很多机器人系统中能被当作独立地图模块使用的重要原因。3. 环境准备与安装方式3.1 Ubuntu 环境下的安装OctoMap 官方提供了比较完整的 C 库同时也包含了简单的可视化工具 octovis。如果你使用的是 Ubuntu最常见的安装方式是直接使用系统包管理工具sudo apt update sudo apt install liboctomap-dev octomap-tools octovis上面的命令会安装 OctoMap 开发库、命令行工具和可视化工具。开发库包含头文件和 CMake 配置后续我们可以在自己的项目中通过 CMake 找到 OctoMap。如果你的发行版软件源中找不到这些包或者需要修改源码也可以从源码编译git clone https://github.com/OctoMap/octomap.git cd octomap mkdir build cd build cmake .. make -j$(nproc) sudo make install源码编译需要确保系统已经安装 cmake 和 g 等编译工具。不同版本的 OctoMap 可能对 C 标准有不同要求如果编译过程中出现 C 标准相关错误建议检查 cmake 中的编译选项。3.2 验证安装是否成功安装完成后可以在终端中确认库文件是否存在ls /usr/local/include/octomap 2/dev/null || ls /usr/include/octomap ldconfig -p | grep octomap如果你能查看到octomap相关的头文件和动态库说明安装基本成功。如果使用源码安装头文件通常位于/usr/local/include/octomap如果使用 apt 安装头文件可能在/usr/include/octomap。3.3 创建一个小型工程为了方便演示我们创建一个最简单的工程。工程结构如下octomap_demo/ ├── CMakeLists.txt └── main.cpp其中CMakeLists.txt是构建配置main.cpp是我们的代码文件。CMakeLists.txt可以参考这样写cmake_minimum_required(VERSION 3.10) project(octomap_demo) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(octomap REQUIRED) add_executable(octomap_demo main.cpp) target_include_directories(octomap_demo PRIVATE ${OCTOMAP_INCLUDE_DIRS}) target_link_libraries(octomap_demo PRIVATE ${OCTOMAP_LIBRARIES})需要注意不同版本安装出来的 CMake 包名可能有区别。如果上述find_package(octomap REQUIRED)找不到可以检查安装目录下的 cmake 文件名称或者用 pkg-config 的方式编译g main.cpp -o octomap_demo $(pkg-config --cflags --libs octomap)实际项目中具体用哪种方式取决于你的操作系统、安装路径以及 OctoMap 版本。核心思路是先确认本地是否已经有可用的库再看 CMake 是否能自动发现它。4. 实战一创建并更新第一张三维概率地图4.1 用插入射线的方式模拟传感器观测OctoMap 提供了一个非常方便的函数insertRay。它接收传感器原点和射线终点自动把这条射线穿过的空间标记为 free把射线终点所在的体素标记为 occupied。这正好符合激光雷达的物理模型激光从传感器出发没有碰到障碍物时路径上是空无一物的一旦打到物体上返回的点就是物体表面。下面是一段完整可运行的 C 代码。// 文件路径octomap_demo/main.cpp #include octomap/octomap.h #include iostream #include vector int main() { // 1. 创建分辨率为 0.1 米的八叉树地图 octomap::OcTree tree(0.1); // 2. 假设传感器位于世界坐标系原点 octomap::point3d sensor_origin(0.0f, 0.0f, 0.0f); // 3. 模拟几组激光端点 std::vectoroctomap::point3d endpoints; endpoints.emplace_back(2.0f, 0.0f, 0.0f); endpoints.emplace_back(2.0f, 0.2f, 0.0f); endpoints.emplace_back(2.0f, -0.2f, 0.0f); endpoints.emplace_back(0.0f, 2.0f, 0.2f); endpoints.emplace_back(-1.5f, -1.0f, 0.5f); // 4. 将每组端点作为一帧观测插入地图 for (const auto endpoint : endpoints) { tree.insertRay(sensor_origin, endpoint); } // 5. 更新内部节点概率保证父节点信息准确 tree.updateInnerOccupancy(); // 6. 统计当前叶子节点数量和占据叶子数量 size_t occupied_leaves 0; for (auto it tree.begin_leafs(); it ! tree.end_leafs(); it) { double p it-getOccupancy(); if (p tree.getOccupancyThres()) { occupied_leaves; } } std::cout leaf nodes: tree.getNumLeafNodes() std::endl; std::cout occupied leaves: occupied_leaves std::endl; std::cout memory size: tree.memoryUsage() bytes std::endl; // 7. 写入二进制地图文件 tree.writeBinary(simple_map.bt); std::cout written simple_map.bt std::endl; return 0; }4.2 关键代码解释octomap::OcTree tree(0.1)表示创建一张八叉树地图叶子节点大小为 0.1 米。分辨率可以在后续读取地图时确认但创建对象时指定它是必要的。tree.insertRay(sensor_origin, endpoint)是演示中最重要的一步。它并不是简单地把 endpoint 节点设置为占据而是会沿着传感器原点到终点执行一次射线遍历。射线经过的每一个体素都会被更新为空闲概率终点会被更新为占据概率。这样构建的地图在空间逻辑上更接近真实物理世界。tree.updateInnerOccupancy()用于在更新完叶子节点后重新计算内部节点的占据概率。我们通常只对叶子节点做插入操作但内部节点的状态会影响可视化和部分查询速度。插入完一批点后调用它能保证整棵树的一致性。在统计占据叶子节点时我们使用getOccupancy()获取该节点的占据概率再和阈值tree.getOccupancyThres()比较。阈值默认通常是 0.5但不同版本可能不同直接使用接口获取更稳妥。4.3 编译和运行在octomap_demo目录中执行cmake -B build cmake --build build ./build/octomap_demo如果 CMake 配置成功并且 OctoMap 库已经正确安装屏幕上会看到类似下面的输出leaf nodes: 1024 occupied leaves: 8 memory size: 812 bytes written simple_map.bt实际数值取决于地图中创建了多少节点。可以看到OctoMap 使用非常少的字节数就能表示一张包含若干点的小地图。如果使用等尺寸体素网格即使体素数量很少也需要为每个体素分配固定空间。运行结束后工程目录下会生成一个simple_map.bt文件。这个文件是 OctoMap 的二进制格式地图文件。可以用 octovis 打开它查看地图效果octovis simple_map.bt4.4 多帧观测后的概率变化单次插入射线后占据概率可能还不够高。为了让模型更加稳定我们通常会在一段时间内连续累积多帧观测。修改上面 main 函数中的插入循环让相同测量重复多次可以看到概率逐渐升高// 模拟多次观测累积 for (int frame 0; frame 10; frame) { for (const auto endpoint : endpoints) { tree.insertRay(sensor_origin, endpoint); } } tree.updateInnerOccupancy(); octomap::OcTreeNode* node tree.search(2.0f, 0.0f, 0.0f); if (node ! nullptr) { std::cout endpoint occupancy after 10 frames: node-getOccupancy() std::endl; }在这里tree.search(2.0f, 0.0f, 0.0f)会返回坐标(2.0, 0.0, 0.0)对应的叶子节点指针。如果该位置的节点从未被创建函数返回nullptr。多次观测能明显提高占据概率这也是 OctoMap 被称为概率三维建图框架的核心原因。5. 实战二节点遍历、查询与地图文件读写在实际工程中我们不只是把点云插入地图还需要对地图做查询、统计、裁剪和读写。下面这些能力几乎每个项目都会用到。5.1 遍历叶子节点并统计信息遍历叶子节点是 OctoMap 中最常用的操作之一。以第 4 节产生的tree为例size_t free_leaves 0; size_t occupied_leaves 0; double min_x 1e9, min_y 1e9, min_z 1e9; double max_x -1e9, max_y -1e9, max_z -1e9; for (octomap::OcTree::leaf_iterator it tree.begin_leafs(), end tree.end_leafs(); it ! end; it) { double x it.getX(); double y it.getY(); double z it.getZ(); // 更新坐标范围 min_x std::min(min_x, x); min_y std::min(min_y, y); min_z std::min(min_z, z); max_x std::max(max_x, x); max_y std::max(max_y, y); max_z std::max(max_z, z); double p it-getOccupancy(); if (p tree.getOccupancyThres()) { occupied_leaves; } else { free_leaves; } } std::cout free leaves: free_leaves std::endl; std::cout occupied leaves: occupied_leaves std::endl; std::cout bounding box: [ min_x , max_x ] x [ min_y , max_y ] x [ min_z , max_z ] std::endl;在遍历过程中it.getX()、it.getY()、it.getZ()返回叶子节点的坐标it.getSize()可以返回该叶子节点的边长it-getOccupancy()返回该节点的占据概率begin_leafs()和end_leafs()构成遍历区间。如果叶子节点是被剪枝后的大节点it.getSize()可能大于我们最初设定的分辨率。因此统计包围盒或做碰撞检测时要充分考虑叶子节点的大小。5.2 查询指定位置的占据状态在地图构建完成后我们经常需要回答“某个位置是否有障碍物”。最直接的方式是查询坐标对应的节点octomap::point3d query_point(1.0f, 1.0f, 0.0f); octomap::OcTreeNode* result tree.search(query_point); if (result nullptr) { std::cout unknown area std::endl; } else { double p result-getOccupancy(); if (p tree.getOccupancyThres()) { std::cout occupied, probability p std::endl; } else { std::cout free, probability p std::endl; } }需要注意如果查询坐标落在从未被观测过的未知区域search返回空指针如果查询坐标所在区域被剪枝成了更大的父节点返回的可能是父节点而不是原始分辨率的小叶子节点如果对地图做过多帧累积观测占据概率可能很接近饱和值。在机器人路径规划中这种按点查询的方式可以快速判断某个采样点是否可行。5.3 二进制地图文件的读取OctoMap 支持将地图保存为二进制文件也支持从二进制文件恢复地图。第 4 节已经用writeBinary写入了simple_map.bt。读取的代码如下octomap::OcTree loaded_tree(0.1); if (!loaded_tree.readBinary(simple_map.bt)) { std::cerr failed to read map file std::endl; return 1; } std::cout loaded resolution: loaded_tree.getResolution() std::endl; std::cout loaded leaf nodes: loaded_tree.getNumLeafNodes() std::endl;读取时传入的分辨率最好和写入时保持一致。如果地图文件来自其他设备或传感器建议先打印getResolution()查看实际分辨率避免后续因分辨率不一致导致坐标换算错误。5.4 将地图转成可视化点云虽然 octovis 可以直接显示.bt文件但在实际项目中我们往往需要把 OctoMap 地图转成点云再用 PCL 或 Open3D 做后续处理。以下代码可以提取所有被占据的叶子节点坐标for (auto it tree.begin_leafs(); it ! tree.end_leafs(); it) { double p it-getOccupancy(); if (p tree.getOccupancyThres()) { std::cout it.getX() it.getY() it.getZ() std::endl; } }在工程中这里可以替换成向点云容器中写入坐标的代码。如果还需要颜色、法线等信息可以另外使用ColorOcTree等扩展地图类型。6. 把 OctoMap 用在二维导航中6.1 为什么三维地图要和二维占据栅格地图转换很多移动机器人底盘是只能在平面内运动的差速底盘或全向底盘。虽然上层规划可以使用三维 OctoMap但 move_base、costmap 等导航组件通常只认识二维 OccupancyGrid。因此一个常见需求是把维护好的 OctoMap 三维地图投影成一张二维占据栅格地图。这也是很多开发者搜索“octomap二维占据栅格地图”时真正想解决的问题。OctoMap 本身不直接输出二维地图但 ROS 生态中有成熟的octomap_server包可以完成这件事。它的内部会维护一张 OctoMap同时发布二维的OccupancyGrid和三维的Octomap话题。6.2 使用 octomap_server 发布二维栅格地图在 ROS 环境里如果还没安装 octomap_server可以执行sudo apt install ros-${ROS_DISTRO}-octomap-server其中${ROS_DISTRO}要替换成你的 ROS 发行版名称比如 noetic、humble 等。安装完成后写一个 launch 文件即可启动launch node pkgoctomap_server typeoctomap_server_node nameoctomap_server param nameresolution value0.05/ param nameframe_id valuemap/ remap fromcloud_in to/points_raw/ /node /launch各参数含义如下resolutionOctoMap 的分辨率。0.05 米适合室内精细建图0.1 米更节省计算资源frame_id地图坐标系名称。一般和 TF 树中的 map 坐标系保持一致cloud_in输入点云话题。如果机器人发布的话题名不是cloud_in需要通过 remap 映射。启动成功后octomap_server 会订阅点云数据不断更新 OctoMap并向外发布一个二维占据栅格地图话题通常叫/projected_map。你可以通过 RViz 添加 Map 显示话题选择/projected_map查看。6.3 不依赖 ROS 的投影思路如果你的项目没有使用 ROS也可以自己写一段降维投影代码。核心思想是遍历 OctoMap 的所有叶子节点把占据节点对应到二维栅格数组上。示例思路如下// 伪代码需要根据自己的二维栅格类调整 double grid_resolution 0.1; int width 100; int height 100; std::vectorint grid(width * height, 0); for (auto it tree.begin_leafs(); it ! tree.end_leafs(); it) { double p it-getOccupancy(); if (p tree.getOccupancyThres()) { continue; } int px static_castint((it.getX() - origin_x) / grid_resolution); int py static_castint((it.getY() - origin_y) / grid_resolution); if (px 0 px width py 0 py height) { grid[py * width px] 100; // 标记为 occupied } }这个投影思路忽略高度、只取占据投影。遇到多层货架、桥梁这类层叠结构时单纯投影会在二维层面产生假障碍。更严谨的做法是先把整个空间按高度切分成多个层每层单独投影或者使用机器人可通过高度过滤。在平面移动机器人场景下把高于底盘的障碍物投影到二维地图通常是比较合理的。7. 常见问题与排查思路OctoMap 的 API 本身不算复杂但不同版本、不同使用方式下仍然有一些容易踩坑的地方。下面整理了几个常见问题。问题现象常见原因解决思路内存占用依然很大分辨率设置过小或者点云没有做体素降采样增大分辨率对输入点云做降采样、滤波地图出现“假墙”或动态物体拖影只用单帧信息直接 updateNode缺少射线 free 更新使用 insertRay或延长观测时间累计概率占据概率一直停留在 0.5 左右节点创建后没有执行 updateInnerOccupancy或查询了父节点插入后调用 updateInnerOccupancy用叶子节点查询读取地图后分辨率与预期不符写入和读取时构造参数不一致读取后打印 getResolution再按需处理CMake 编译找不到 octomap没有安装 dev 包或 CMake 包名不匹配安装 liboctomap-dev查看本地 cmake 文件名称点云插入到地图后位置偏移很严重输入点云没有转换到地图坐标系或外参标定不准确保先统一到 map/world 坐标系再插入地图逐条看下可能的原因。第一类问题是性能问题。分辨率 0.01 米和 0.05 米的差别不只是精度存储开销差的不是线性倍数而是立方倍增长。建图前先确认最终用途如果只是为了导航避障不需要过分追求毫米级地图。另外激光雷达一帧点云如果包含几十万个点并且不做降采样地图更新耗时也会明显增加。可以先用体素滤波器把点云降采样到合适密度再插入 OctoMap。第二类问题很常见。当你直接把一个点云中的每个点调用updateNode(point, true)时你只是把这些点标记为占据却没有告诉 OctoMap 这些点和传感器之间的空间是空闲的。结果就是地图中同一位置如果前后帧噪声变化点云会形成一道很厚的“雾状墙”。更合理的做法是知道每个点的传感器原点用insertRay完成 free 和 occupied 的同时更新。第三类问题通常和“内部节点”有关。OctoMap 的叶子节点会被剪枝合并到父节点如果遍历的是父节点概率可能不代表真实叶子。查询时先确认返回的节点指针对应的是叶子还是内部节点。插入点云后如果不调用updateInnerOccupancy()内部节点的概率也不会更新。第六类问题在机器人上非常普遍。OctoMap 只负责把点云丢进地图它不知道点云是在雷达坐标系、相机坐标系还是 map 坐标系。如果你没有把点云变换到机器人 base_link 或 map 坐标系就插入地图那么地图中的障碍物位置必然和真实环境不一致。8. 工程落地与使用建议8.1 分辨率不是越小越好分辨率的选择要和传感器精度、机器人尺寸、下游算法匹配。一个常见经验是导航避障0.1 米到 0.2 米足够机械臂抓取或精细重建0.01 米到 0.05 米超大范围室外地图0.2 米以上并配合多地图分块管理。如果某段区域需要特别精细可以考虑多张 OctoMap 分层管理而不是