简介一套基于ORB-SLAM的三维密集点云生成与OctoMap室内导航地图构建项目面向计算机视觉、机器人导航方向的在校学生与开发者也可作为毕设或课程设计参考。项目包含完整C源码与说明文档并额外提供八叉树地图转换工具可帮助读者理解从视觉SLAM到八叉树占据栅格地图的完整流程。资源包共199个文件以头文件、C源文件、CMake构建脚本为主辅以配置文件、Python脚本与可执行工具整体约33MB目录结构清晰。目前已有1091人学习下载代码经运行验证且答辩评分较高可直接在ORB-SLAM框架上二次开发用于室内定位、导航地图构建等实验场景。1. 从ORB-SLAM稀疏特征点到能导航的八叉树地图ORB-SLAM 本身不会给你一张能直接用于室内导航的地图。它输出的是稀疏特征点和相机轨迹特征点在墙上、桌角、天花板边缘到处分布密度不均而且没有“这一块空间到底能不能过人”的语义回答。规划器需要的是占据、空闲、未知三种状态的分区而不是一堆好看的点云。常见的落地做法是用 ORB-SLAM 输出的关键帧位姿把每一帧 RGB-D 深度图回投影成三维密集点云再用 OctoMap 库把点云累积成八叉树地图最后导成 move_base 能加载的 2D 占用地图。标题里说的“添加八叉树地图转换工具”就是这条链路里最实用的 C 实现适合正在做 RGB-D 视觉导航、室内三维重建以及想把 ORB-SLAM 结果真正喂给导航栈的开发者。2. 坐标变换与点云拼接先吃透 ORB-SLAM 轨迹和深度回投影2.1 ORB-SLAM 轨迹文件里存了什么用 ORB-SLAM2 或 ORB-SLAM3 跑完 RGB-D 数据后KeyFrameTrajectory.txt里每行是 TUM 格式timestamp tx ty tz qx qy qz qw前四个数字是相机在世界坐标系下的平移后四个是四元数表示旋转。注意这个位姿是T_wc也就是把相机坐标变换到世界坐标的外参而不是反过来的T_cw。很多人在这一步会写反结果整个房间的地图被镜像或堆在一角。读取轨迹时我一般直接用 Eigen 构造变换矩阵#include Eigen/Core #include Eigen/Geometry #include fstream #include vector struct KeyFrame { double timestamp; Eigen::Isometry3d T_wc; }; std::vectorKeyFrame loadTrajectory(const std::string path) { std::vectorKeyFrame frames; std::ifstream ifs(path); if (!ifs.is_open()) { throw std::runtime_error(cannot open trajectory file: path); } std::string line; while (std::getline(ifs, line)) { if (line.empty() || line[0] #) continue; std::istringstream iss(line); KeyFrame kf; double tx, ty, tz, qx, qy, qz, qw; if (!(iss kf.timestamp tx ty tz qx qy qz qw)) { continue; } // 四元数构造参数顺序是 w, x, y, z Eigen::Quaterniond q(qw, qx, qy, qz); q.normalize(); kf.T_wc Eigen::Isometry3d::Identity(); kf.T_wc.linear() q.toRotationMatrix(); kf.T_wc.translation() Eigen::Vector3d(tx, ty, tz); frames.push_back(kf); } return frames; }这段代码做的事就是把文本行解析成Eigen::Isometry3d。对四元数做normalize()是为了防止轨迹文件里出现数值误差后续所有点云变换都要依赖这个旋转矩阵哪怕差 0.001 的模长都会让几百帧拼接后的点云出现肉眼可见的“重影”。2.2 深度图到三维点的回投影公式拿到相机位姿后下一步是把每一帧的深度图转换成相机坐标系下的三维点。RGB-D 相机给出的深度图像素值通常不是米而是整数需要除以一个尺度因子。例如 TUM 数据集里用 5000Kinect 驱动里可能是 1000。转换公式是z depth(v, u) / depth_scale x (u - cx) * z / fx y (v - cy) * z / fy其中fx、fy、cx、cy来自相机内参标定。实际操作里我会把这一步封装成一个函数并且把无效深度一起过滤掉#include opencv2/imgproc.hpp #include pcl/point_types.h #include pcl/point_cloud.h #include vector pcl::PointCloudpcl::PointXYZ::Ptr depthToPointCloud( const cv::Mat depth, double fx, double fy, double cx, double cy, double depthScale, double maxDepth) { auto cloud std::make_sharedpcl::PointCloudpcl::PointXYZ(); cloud-reserve(depth.rows * depth.cols / 4); const float inv_fx 1.0f / static_castfloat(fx); const float inv_fy 1.0f / static_castfloat(fy); for (int v 0; v depth.rows; v) { const uint16_t* row depth.ptruint16_t(v); for (int u 0; u depth.cols; u) { uint16_t raw row[u]; if (raw 0) continue; double z static_castdouble(raw) / depthScale; if (z 0.0 || z maxDepth) continue; pcl::PointXYZ p; p.x (u - cx) * z * inv_fx; p.y (v - cy) * z * inv_fy; p.z z; cloud-push_back(p); } } return cloud; }这里有两个容易踩的细节。第一raw 0在很多深度相机里表示“测量不到”必须丢掉否则会在地图原点附近堆出一片假墙。第二maxDepth要按场景设置室内一般给 4 到 6 米超出范围的深度值噪声极大。对不同数据集深度尺度可从这两个地方确认数据集的说明文档或者标定文件的depth_scale字段。2.3 关键帧点云拼接与体素滤波单帧点云是在相机坐标系下的要拼到地图里必须左乘相机位姿T_wcpcl::PointCloudpcl::PointXYZ::Ptr worldCloud( new pcl::PointCloudpcl::PointXYZ()); for (const auto kf : frames) { // 假设 depthImages 与 frames 按 timestamp 对齐 cv::Mat depth loadDepthImage(kf.timestamp); auto camCloud depthToPointCloud(depth, fx, fy, cx, cy, kf); for (const auto pt : camCloud-points) { Eigen::Vector3d p_cam(pt.x, pt.y, pt.z); Eigen::Vector3d p_w kf.T_wc * p_cam; worldCloud-push_back(pcl::PointXYZ(p_w.x(), p_w.y(), p_w.z())); } }T_wc * p_cam这个顺序不能反因为Isometry3d的乘法规则是矩阵左乘列向量。拼接完的点云往往有海量冗余点相邻关键帧会重复扫到同一面墙一个 100 平米的屋子可能拼出上千万个点直接送给 OctoMap 会让构建时间和内存都失控。在进入八叉树之前用 PCL 的体素滤波把每个小立方格里的点合并成一个是标准做法#include pcl/filters/voxel_grid.h pcl::VoxelGridpcl::PointXYZ vg; vg.setInputCloud(worldCloud); vg.setLeafSize(0.04f, 0.04f, 0.04f); vg.filter(*worldCloud);体素边长一般和八叉树分辨率相同或比八叉树略小。不要大于八叉树分辨率否则滤波本身把信息量抹掉了。体素滤波的作用不是让点云变漂亮而是让同一体积内只保留一个点避免 OctoMap 的射线更新在同一区域来回冲撞导致概率翻来覆去地跳。3. 八叉树地图的核心机制占据概率更新与 C 转换工具骨架3.1 为什么点云不能直接拿来导航三维密集点云在可视化上很直观但导航算法没法直接用。原因有三个。第一点云是离散点不能回答“以某个坐标为中心、半径 0.3 米的球内是否被占据”这种体积查询第二点云没有自由空间信息导航规划器需要知道哪些区域是“确定空的”点云只能告诉你墙的位置不能告诉你两面墙之间的通道是否可通行第三单帧点云有大量噪声同一个位置在不同帧里可能都被测到却谁真谁假说不清。八叉树地图把空间递归切分成正方体每个叶子节点存一个占据概率。这个结构天然支持体查询、支持自由/占据/未知三态还能在构建完成后压缩成稀疏的.bt文件。3.2 log-odds 概率更新与 OctoMap 默认参数OctoMap 不用简单累加“看到了多少次”而是用 log-odds 形式避免概率接近 0 或 1 时数值不稳定。设当前节点对数几率为L(n)每次观测后用下面公式更新L(n) L(n-1) l_free // 射线穿过该节点是空闲 L(n) L(n-1) l_occ // 射线终点该节点被占据然后通过p 1 - 1 / (1 exp(L(n)))转回概率。OctoMap 库里几个核心参数会直接决定地图质量参数默认值作用resolution0.05叶子节点边长单位米probHit0.7命中一次时更新的占据概率probMiss0.4射线穿过时更新的空闲概率occupancyThres0.5概率超过该值判为占据clampingThresMin约 0.12概率下限防止振荡clampingThresMax约 0.97概率上限防止反复翻转probHit和probMiss差得越小地图对噪声越鲁棒但收敛越慢。机器人导航场景下通常保持库默认值即可只有当你发现墙体“空心”或者地面全是噪点时再去动这两个值。3.3 转换工具的核心实现优先使用 insertPointCloud把点云写进八叉树最容易犯错的做法是对每个点调用updateNode(point, true)。这样做只更新了测量端点等于告诉地图“这些点是墙”却没有告诉地图“相机的光路经过的那些空间是空着的”。结果就是空白区域全部变成 unknownmove_base 在空旷走廊里也规划不出路径。正确做法是用insertPointCloud它内部会对传感器原点到每个点做一次射线穿越沿途节点按probMiss更新终点按probHit更新。核心骨架如下#include octomap/octomap.h #include octomap/Pointcloud.h void buildOctoMap(const pcl::PointCloudpcl::PointXYZ::Ptr cloud, const Eigen::Vector3d sensorOrigin, double resolution, const std::string outputPath) { octomap::OcTree tree(resolution); // 把 PCL 点云转成 octomap 自己的 Pointcloud octomap::Pointcloud octoCloud; for (const auto pt : cloud-points) { octoCloud.push_back(pt.x, pt.y, pt.z); } octomap::point3d origin(sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z()); // maxrange 传 -1 表示不限制射线长度 tree.insertPointCloud(octoCloud, origin, -1.0); // 更新内部节点概率并剪枝压缩 tree.updateInnerOccupancy(); tree.prune(); tree.writeBinary(outputPath); }这里的sensorOrigin不是当前关键帧的相机原点而是这一批点云中每一帧各自的光心。严格来说应该对每一帧分别调用一次insertPointCloud而不是把整段关键帧点云合并后只用一个原点。合并后只用一个原点会让远离处理帧的墙面被错误地当点云遮挡产生空洞。常见工程写法是外层循环遍历每个关键帧对单帧点云调用一次更新。updateInnerOccupancy()这一句容易被漏。它根据子节点的概率决定父节点的状态让墙面下方的大块空闲区域被表示成一个大节点从而显著压缩内存。如果省略树里全是细碎叶子地图能建出来但文件体积和内存都会大几倍。4. 编译运行与可视化把地图转换工具接到自己的数据集上4.1 依赖、CMake 与主流程设计整个工具依赖 PCL、OpenCV、Eigen 和 OctoMap。在 Ubuntu 上常见安装方式足够这里只看 CMake 配置里容易出问题的部分cmake_minimum_required(VERSION 3.16) project(octomap_tools) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(PCL REQUIRED COMPONENTS io filters) find_package(OpenCV REQUIRED) find_package(octomap REQUIRED) find_package(Eigen3 REQUIRED) add_executable(build_map src/build_map.cpp) target_link_libraries(build_map ${PCL_LIBRARIES} ${OpenCV_LIBS} octomap::octomap Eigen3::Eigen)主流程按“轨迹读取 → 深度图回投影 → T_wc 变换 → 单帧 insertPointCloud → 落盘”的顺序写不要把滤波和八叉树更新混在一起。PCL 和 OctoMap 都定义了自己的Pointcloud类型命名会冲突我习惯在头文件里用using OctoCloud octomap::Pointcloud;明确区分。4.2 命令行参数与一个最小运行示例工具的参数设计可以很简单够用就行。下面的命令行格式我觉得最适合做数据集批处理./build_map \ --depth-dir ./rgbd \ --traj ./KeyFrameTrajectory.txt \ --assoc ./associations.txt \ --fx 525.0 --fy 525.0 --cx 319.5 --cy 239.5 \ --depth-scale 5000 \ --resolution 0.05 \ --maxrange 5.0 \ --output map.bt每个参数的含义参数必填说明--depth-dir是存放全部深度图的目录文件名按时间戳命名--traj是ORB-SLAM 输出的轨迹文件--assoc否TUM 风格时间戳关联文件缺失时按最近邻匹配--fx/--fy/--cx/--cy是相机内参--depth-scale是深度像素值换算为米的除数--resolution否八叉树分辨率默认 0.05--maxrange否最大测量距离默认 4.0 米--output是输出 .bt 路径4.3 在 RViz 和 octovis 里验证八叉树地图构建完成后先用 OctoMap 自带的octovis快速看一眼这是最快发现尺度错误的方式octovis map.bt如果墙的厚度和真实偏差在一个分辨率以内再进 RViz。最常见做法是写一个 30 行左右的发布节点把OcTree转成octomap_msgs::Octomap消息发出来#include octomap_msgs/conversions.h #include octomap_msgs/Octomap.h octomap::OcTree tree(map.bt); octomap_msgs::Octomap msg; msg.binary true; msg.id OcTree; msg.resolution tree.getResolution(); if (octomap_msgs::binaryMapToMsg(tree, msg)) { pub.publish(msg); }RViz 里添加MarkerArray或Map话题后选择OccupancyGrid显示类型。此时如果地图里出现大量漂浮的点先回第 2 章检查坐标变换如果地图周围全是 unknown 而不是 free回第 3 章检查是不是把insertPointCloud换成了updateNode。5. 分辨率、maxrange 与回环构建可用地图的四个关键参数5.1 分辨率选不对后面全白做八叉树分辨率是第一个要定的参数它直接决定导航可用性和资源消耗。室内导航场景里常见的几档选择分辨率适用场景效果0.02精细重建、避障仿真墙面细节清晰节点数量大构建慢0.05室内导航地图平衡点墙厚约 0.05 米机器人体积明确0.10大范围仓库、巡检速度快窄通道可能被抹平对服务机器人室内导航0.05 是绝大多数项目的默认值。小于 0.03 后深度噪声会让墙面出现大量“毛刺”反而不利于导航。分辨率每缩小一半理论节点数膨胀约 8 倍构建时间也非线性上升。5.2 maxrange 要匹配走廊深度insertPointCloud的 maxrange 参数控制射线投多远。短走廊里如果你把 maxrange 放得很大比如 10 米那么一帧点云里的几百个稀疏点会在它们所在的整条射线上标记 free把走廊尽头的未扫描区域错误地标成空闲导航规划器可能直接规划穿过一堵还没扫描到的墙。我一般先把深度图的最大有效值统计出来再设置 maxrange 为略小于这个值的数。OpenCV 统计一行就能做完double minRaw, maxRaw; cv::minMaxLoc(depth, minRaw, maxRaw); double maxRange maxRaw / depthScale * 0.9;这样设置比拍脑袋定 4 米更符合具体传感器。5.3 回环漂移、深度尺度与时间戳对齐ORB-SLAM 只在检测到回环并跑完全局优化后位姿才会收敛到一致。如果你在线边跑边建图回环前的墙和回环后的墙会错开形成双层墙。离线构建时一定要用回环优化后的KeyFrameTrajectory.txt或FrameTrajectory_TUM_Format.txt。时间戳对齐也是一个隐蔽问题。RGB 图和深度图往往不是同一时刻拍摄的TUM 数据集官方提供了associate.py把彩色图和深度图按时间差小于 0.02 秒配对。配对错误会让点云和位姿错位地图里出现厚度夸张的墙面。深度尺度则要看数据集说明TUM 是除以 5000有的传感器驱动是除以 1000写死任何值都会让房间尺寸明显不对。最后还有一个容易忽略的点构建地图时不要把整段序列的所有帧都丢进八叉树。关键帧再加相邻间隔 3 到 5 帧已经能覆盖完整场景。帧数翻倍并不会让墙更厚更清晰只会让构建时间变长。6. 从 map.bt 到 move_base 代价地图最后一个可用性技巧八叉树地图本身是三维的move_base 传统二维导航栈需要 2D 占用栅格。常见技巧是从.bt里提取一个高度片层投影成 PGM 地图。实现的关键是只统计机器人底盘高度到略高于车体的区间内被占据的体素避免把桌面、吊灯投影成障碍#include octomap/octomap.h #include opencv2/imgproc.hpp void exportPGM(const std::string btPath, double zMin, double zMax, double resolution, const std::string pgmPath) { octomap::OcTree tree(btPath); double minX, minY, maxX, maxY; tree.getMetricMin(minX, minY, maxX, maxY); int cols static_castint(std::ceil((maxX - minX) / resolution)); int rows static_castint(std::ceil((maxY - minY) / resolution)); cv::Mat grid(rows, cols, CV_8UC1, cv::Scalar(205)); // 205 表示 unknown for (octomap::OcTree::leaf_iterator it tree.begin_leafs(), end tree.end_leafs(); it ! end; it) { if (!tree.isNodeOccupied(*it)) continue; double z it.getZ(); if (z zMin || z zMax) continue; int c static_castint((it.getX() - minX) / resolution); int r static_castint((it.getY() - minY) / resolution); if (c 0 c cols r 0 r rows) { grid.atuchar(rows - 1 - r, c) 0; // 0 表示占据 } } cv::imwrite(pgmPath, grid); }对机器人导航来说zMin 取 0.1 米zMax 取机器人高度的一半比较合理。导出后写一个配套的map.yaml把origin设为地图左下角坐标resolution设为投影用的分辨率。用 map_server 加载这张 PGM再配合 move_base 的 local costmap 和 global costmap室内导航建图链路就算真正闭环了。本文还有配套的精品资源点击获取