简介：面向室内导航与三维重建场景，基于ORB-SLAM生成三维密集点云，并利用OctoMap构建八叉树导航地图，项目配套完整C++源码与文档说明，适合SLAM、机器人导航、计算机视觉方向的在校生、研究者或开发者学习与二次开发。压缩包共199个文件，大小约32.95MB，以C++源码为主（含.h/.cpp/.cc头文件与实现文件），同时包括CMake构建脚本、Shell/Python辅助脚本、yaml/xml配置说明以及pcd2octomap、text2binary等转换工具，覆盖特征提取、跟踪、局部建图、回环检测、全局优化等ORB-SLAM核心模块。目前已有1091人浏览学习。可直接在已有工程上编译运行，结合文档和转换工具理解从相机位姿估计、稀疏地图到密集点云、八叉树地图的完整链路；代码结构清晰、模块分离，便于在此基础上做算法替换或功能扩展，也可作为课程设计或毕业设计的参考实现。
1. 基于ORB-SLAM三维密集点云的室内导航地图：八叉树地图转换工具是关键
ORB-SLAM输出的是特征点稀疏地图，拿去做导航第一步就卡住。常见做法是先用RGB-D或双目数据补出密集点云，再把点云转成OctoMap八叉树占据地图，交给move_base做路径规划。这一步看着简单，实际卡点多：坐标帧没对齐、点云噪点把自由空间填死、分辨率选错导致代价地图抖动，任何一个问题都会让导航在走廊里反复横跳。这篇博文从数据链路说起，把PCL点云到octomap::OccupancyOcTree的转换工具完整拆开，给出C++源码级实现、参数对照表和高发性报错处理。八叉树地图转换工具决定导航成败，适合正在做室内移动机器人导航、或想把手头ORB-SLAM结果转成可导航地图的工程师，读完可以直接照着一套最小工具落地。
2. 从ORB-SLAM位姿到三维密集点云：位姿链路与点云密度补全
2.1 ORB-SLAM稀疏地图为什么不能直接用于OctoMap构建
ORB-SLAM的全局地图里只有路标点（MapPoint），每个点只代表纹理角点，不包含物体表面连续信息。导航需要的不是“有特征的位置”，而是“障碍物占据的体素”和“可通行的自由空间”。稀疏点云转占据栅格后，墙面会变成稀疏的离散块，路径规划器几乎每帧都要重新搜索路径，严重时直接规划出一条穿墙路线。
另一个问题是ORB-SLAM的地图点缺少“射线末端”语义。OctoMap的更新模型需要从传感器原点发出一条射线，射线穿过的区域标记为free，末端标记为occupied。稀疏地图没有射线起点与轨迹信息，直接灌入八叉树会导致节点概率混乱。我一般拿到ORB-SLAM结果后，先做一次密集重建，再转OctoMap。这个顺序不要倒过来：OctoMap内部只保存占据概率，不保存颜色纹理，一旦转走再想补细节就得重新生成整张点云。
2.2 RGB-D与双目点云的密集化两条路线
室内场景最常用的密集化路线是RGB-D。ORB-SLAM2/3的RGB-D模式会对每一帧深度图像用相机内参反投影，生成当前帧点云，再按关键帧位姿拼接。这样出来的点云密度高，缺点是深度图在反光瓷砖、白墙和远距离上会大量掉数据，拼接后出现条状空洞。
双目密集化用视差图反投影，计算量明显更大，室内近距离精度不如RGB-D，但对环境光不敏感。如果手上的ORB-SLAM是单目版本，密集化要走多视角立体匹配或离线MVS，整体工作量会成倍增加。三条路线的取舍如下：
| 路线 | 传感器输入 | 室内近距离精度 | 实时性 | 适配场景 |
|---|---|---|---|---|
| RGB-D反投影 | 深度图+相机位姿 | 高 | 高 | 室内近距离、有深度传感器 |
| 双目视差 | 左右目图像+位姿 | 中 | 中 | 光照变化大、室外半开放 |
| 离线MVS/Neural | 多视图图像 | 中高 | 低 | 离线建图，环境长期不变 |
2.3 点云坐标帧与尺度对齐：转换前的硬性检查
无论哪条路线生成的密集点云，都必须统一到世界坐标系，并且尺度一致。ORB-SLAM单目模式存在尺度漂移，跑一圈回来轨迹可能缩了或放大了。最直接的校验方法是拿两个已知物理距离的物体（比如门宽）在点云里量一下，偏差超过5%就先做Sim3对齐。
代码里一般用Eigen把当前帧点从相机系转到世界系：
#include <Eigen/Core> #include <Eigen/Geometry> // Tcw 是ORB-SLAM给出的当前帧位姿，点云在相机系下为 p_cam Eigen::Matrix4d Tcw = keyframe->GetPose(); // 4x4变换矩阵 Eigen::Matrix4d Twc = Tcw.inverse(); // 求逆得到世界系到相机系 for (auto& p : cloud->points) { Eigen::Vector4d p_cam(p.x, p.y, p.z, 1.0); Eigen::Vector4d p_world = Twc * p_cam; // SE3逆变换 p.x = p_world.x(); p.y = p_world.y(); p.z = p_world.z(); }这段代码先把相机系坐标齐次化，再做一次SE3逆变换到世界系。注意Twc的平移分量是相机光心在世界系的位置；如果是单目ORB-SLAM，这个位置的单位不是米，需要把Sim3求解出的尺度因子乘到坐标上，否则后面OctoMap的分辨率参数会完全失效。PCL里更省事的是用pcl::transformPointCloud配合Eigen::Affine3f，但千万确认位姿矩阵是行主序还是列主序：OpenCV的cv::Mat默认行存储，Eigen默认列存储，直接memcpy会出现转置，点云当场炸开。
3. OctoMap八叉树地图原理：概率占据、分辨率与内存结构
3.1 八叉树的剪枝结构与占据概率更新
OctoMap的核心是八叉树（Octree），根节点代表整个包围盒，递归分裂成8个子节点，直到叶子节点对应最小体素。与普通三维栅格数组不同，八叉树只对“有信息”的节点继续分裂，空白大区域停在高层节点上，因此内存占用远小于长×宽×高的密集数组。
每个叶子节点保存一个占据概率。传感器读数到来时，OctoMap用对数优势比（log-odds）更新：
L(n) = clamp(L(n) + log(p_occ / (1 - p_occ)), l_min, l_max)激光打在障碍物表面，末端节点被更新为occupied；射线穿过的中间体素更新为free。这个机制天然适合ORB-SLAM密集点云：把每个点当作“表面命中”，用传感器光心位置做射线起点，两点之间做一次castRay，沿途体素全部标记为free。这样建出的地图同时包含障碍物表面和可通行空间，路径规划器才能正常工作。
3.2 分辨率选择直接影响导航通过性
分辨率是转换工具里最先要定的参数，没有之一。室内导航常见三档：
| 分辨率 | 体素对应物理尺寸 | 典型场景 |
|---|---|---|
| 0.02m | 2cm | 精细避障，细节丰富但内存大 |
| 0.05m | 5cm | 常规室内导航，内存与精度平衡 |
| 0.10m | 10cm | 厂房、仓储AGV，规划快但细节丢失 |
我一般先按0.05跑通，再试0.02。0.05的含义是每个叶子节点代表5cm立方体，一个10m×10m×3m的房间，满分辨率有2400万个体素；虽然八叉树剪枝后远少于此，但点云密集区域的节点数依然可观。选型时还要看机器人底盘尺寸：直径40cm的机器人用0.02能得到细腻边界，但代价地图膨胀半径没设好反而把狭窄通道堵死；0.05配合合理膨胀半径是最稳的组合。
3.3 内存占用与.bt/.ot文件格式
OctoMap提供两种保存格式：.bt（binary）和.ot（带颜色）。导航只需要占据信息，存.bt就够。文件体积和分辨率强相关，同一片区域，0.02分辨率的导出文件可能是0.05的3到5倍。加载时内存开销同样不容忽视，有人把0.01分辨率的室内地图直接加载，节点数过亿，RViz打开就卡死。转换工具的出发点应该是“够用就好”，不是“越细越好”。想保留颜色做可视化可以生成.ot，但导航栈根本不读颜色字段，属于白缴内存。
4. 八叉树地图转换工具C++实现：PCL点云到OccupancyOcTree完整代码
4.1 工具依赖与CMake工程配置
转换工具本身不复杂，我一般拆成三个文件：main.cpp做参数解析，PointCloudToOctomap.cpp做转换，export_map.cpp做地图导出。后续要接增量建图或ROS服务化，只改入口就行。
依赖只有PCL和OctoMap两个库。OctoMap用系统包管理装liboctomap-dev；PCL如果只做PCD读写，不引入可视化模块，编译会快很多。CMake配置如下：
cmake_minimum_required(VERSION 3.10) project(pcd_to_octomap) find_package(PCL REQUIRED COMPONENTS io filters) find_package(OctoMap REQUIRED) add_executable(pcd_to_octomap src/main.cpp src/PointCloudToOctomap.cpp) target_include_directories(pcd_to_octomap PRIVATE include) target_link_libraries(pcd_to_octomap ${PCL_LIBRARIES} octomap)find_package(OctoMap REQUIRED)要求OctoMap安装路径已写入CMAKE_PREFIX_PATH。编译期最常见的坑是PCL和OctoMap各自引入的Boost版本冲突，链接时出现Boost::filesystem版本不一致的报错。解决办法是把两个库统一到系统默认Boost，不要在CMAKE_PREFIX_PATH里混排多套第三方Boost。
4.2 核心转换代码：逐点插入与射线标记free
从PCL点云到OctoMap，最直接的方式是把每个点当作occupied端点，调用insertPointCloud替代手写raycast。insertPointCloud接收传感器原点与点云，内部自动对每个点做射线更新，效率比逐个updateNode高一个数量级。
#include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <octomap/octomap.h> #include <octomap/OcTree.h> using namespace octomap; // 输入: 世界系下的PCL点云、传感器原点、分辨率、射线最大距离 std::shared_ptr<OcTree> BuildOctomapFromCloud( const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const octomap::point3d& sensor_origin, double resolution, double max_range, double occupancy_thresh, double prob_hit, double prob_miss) { auto tree = std::make_shared<OcTree>(resolution); tree->setOccupancyThres(occupancy_thresh); // 低于该概率不算占据 tree->setProbHit(prob_hit); // 命中端点的概率提升 tree->setProbMiss(prob_miss); // 射线穿过时概率衰减 octomap::Pointcloud octo_cloud; for (const auto& pt : cloud->points) { if (std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z)) { octo_cloud.push_back(pt.x, pt.y, pt.z); } } // 射线末端标记occupied，射线中间体素标记free tree->insertPointCloud(octo_cloud, sensor_origin, max_range); tree->prune(); // 概率一致的子节点合并到父节点，降低内存 return tree; }关键逻辑在insertPointCloud内部：每条从origin到点云点的射线都会被遍历，沿途体素执行free更新，端点执行occupied更新。prob_hit=0.7意味着单次命中后log-odds增加约0.85，prob_miss=0.4表示射线穿过使log-odds减少约0.41。多帧观测下，真实障碍物概率持续逼近1.0，误检噪声因为位置不稳定很难累积到阈值，这就是八叉树地图转换工具抗噪的根本原因。
max_range直接控制射线多长距离内算free。室内一般给8到15米。给得太大，远端点云稀疏区域的体素被大量标记free，真实墙壁被“穿透”；给得太小，点云密集区域的自由空间标记不充分，路径规划把大量可通行区域判成未知。
4.3 命令行参数与转换工具操作步骤
实际使用中参数不应该写死在代码里，全部走命令行，同一份编译产物才能适配不同传感器和设备。
./pcd_to_octomap --input scan.pcd --output map.bt \ --resolution 0.05 --origin 0 0 0 --max-range 12.0 \ --occ-thresh 0.5 --prob-hit 0.7 --prob-miss 0.4| 参数 | 默认值 | 说明 |
|---|---|---|
| --input | 无 | 输入PCD/PLY文件路径 |
| --output | map.bt | 输出文件，.bt二进制或.ot带颜色 |
| --resolution | 0.05 | 八叉树叶子分辨率，单位米 |
| --origin | 0 0 0 | 传感器原点，世界系坐标 |
| --max-range | 12.0 | 射线最大长度，超过不标记free |
| --occ-thresh | 0.5 | 占据概率判定阈值 |
| --prob-hit | 0.7 | 命中概率，建议0.65~0.85 |
| --prob-miss | 0.4 | 未命中概率，建议0.3~0.5 |
--origin必须单独拿出来。很多点云直接从关键帧拼接，传感器原点不在(0,0,0)，如果忽略这个参数，insertPointCloud会从世界原点向每个点发射线，相机真实扫过的空间被错误保留为free，点云背后的区域反而没被标记，整张地图的占据关系完全扭曲。
4.4 编译与运行阶段的高频报错
第一类报错集中在Eigen头文件冲突。PCL 1.10以上自带较新Eigen，OctoMap源码里又有内部向量实现，两者混编时会出现EIGEN_MAKE_ALIGNED_OPERATOR_NEW相关的对齐错误。解决方法是先包含PCL头，再包含OctoMap头，让PCL的Eigen版本先占据符号表。
第二类报错是PCD格式不兼容。ORB-SLAM跑出来的点云如果包含RGB字段，用pcl::PointCloud<pcl::PointXYZ>直接loadPCDFile会报找不到字段。这时要看文件头的FIELDS声明，或改用pcl::PointCloud<pcl::PointXYZRGB>读取后再丢弃颜色字段。
第三类不是编译错误，而是输出地图“全黑全绿”的语义性错误。全黑代表全部未知，通常是--origin给错，射线起点落在点云内部；全绿代表全部free，一般是--prob-miss太高或--max-range过大。先用降采样后的小数据量点云测试参数，收敛之后再跑全量，能省下大量排错时间。
5. 八叉树地图转换后的导航验证与增量更新实战技巧
5.1 转换后地图在RViz里的三看检查
转换工具输出.bt文件后，先加载进RViz做三看：墙体是否连续，地面是否被判为障碍，天花板和反光表面是否出现悬空块。悬空块在室内极常见，ORB-SLAM拼接点云时，高光地砖和玻璃门会产生大量离群点。我习惯在转换工具里加一个高度范围过滤，只保留z从0.05到1.8米的点再进OctoMap。这个参数做成--height-min和--height-max，不同机型只改命令行，不用重新编译。
5.2 costmap参数与octomap话题对接
move_base侧订阅OctoMap话题时，costmap配置里数据源要声明为Octomap类型。obstacle_range建议比转换工具里的--max-range小1到2米，这样代价地图看到的障碍物边界与八叉树占据体素一致，不会出现地图里有墙、代价地图却看不到的情况。这里有个容易踩的坑：octomap_server发布的/octomap_full是完整概率图，navigation栈只需要binary类型，接错话题会导致地图无法加载。
5.3 增量更新与动态避障的双层地图
静态建图完成后，OctoMap的增量更新优势很实用。新帧点云只需要再调用insertPointCloud并保存，新障碍物就能合并进旧地图，不需要重跑全量。实际操作中要先把旧地图改动区域的节点概率重置为unknown，否则多帧累加会让更新响应变迟钝。
运行期避障建议用独立的局部OctoMap，--prob-hit调到0.8左右，动态障碍物出现即标记、离开即清除，不影响全局静态图；全局图保持0.7命中概率，长期稳定性更好。验证时可以用octomap自带的compare命令做新旧地图节点级对比，观察体积异常变化。做地图转换，稳定可复现永远比单次效果重要。
本文还有配套的精品资源，点击获取
