跑通 ORB-SLAM2 的稀疏地图之后,我们都会面临同一个尴尬:定位精度看着挺漂亮,可把点云导出来给朋友看,对方一句“这哪像房间,不就是一堆散点吗”直接把你噎住。那个屏幕上飘着的稀疏特征点地图,作为 SLAM 的“中间产物”完全合格,但要拿去渲染、做三维重建、给机械臂做避障,根本不够用。于是我决定动手给 ORB-SLAM2 加一个在线稠密点云构建模块,把相机实时看到的深度信息转换成真正“实心”的场景点云。这篇文章就是这个系列的第一篇,重点讲清楚整个方案的原理、选型依据和核心实现思路,包括从深度图到三维点云的数学变换、关键帧点云的全局融合逻辑,以及我在实际操作中踩过的一系列坑,适合已经跑通过 ORB-SLAM2 基础 Demo、想在它上面扩展功能的朋友。
1. 从稀疏地图说起——为什么 ORB-SLAM2 默认只给一堆散点
1.1 稀疏地图不是缺陷,而是特征点法的必然
很多刚接触 SLAM 的人都会困惑:ORB-SLAM2 作为视觉 SLAM 的标杆实现,为什么输出的地图那么“寒酸”?其实这要从它的核心算法讲起。ORB-SLAM2 是典型的特征点法 SLAM,整个前端和后端都建立在 ORB 特征之上——先对图像提取角点和描述子,然后通过描述子匹配建立帧间数据关联,再通过三角化恢复出三维空间点。整个过程里,地图点就是那些被多次观测、经过 BA 优化后仍然稳定的角点对应的三维坐标,数量通常只有几千到几万个。
这些稀疏点对于“定位”来说绰绰有余,因为 SLAM 系统需要的只是稳定的几何约束,而不是完整的表面信息。但一旦涉及到三维重建、体积测量、路径规划、可视化展示,稀疏点就完全不够用了,你需要覆盖整个物体表面的稠密点云,每个像素最好都能对应一个三维点。
1.2 “在线构建稠密点云”到底要解决什么问题
所谓“在线构建”,是指在运行 ORB-SLAM2 的同时,实时地完成点云的生成和融合,而不是跑完整个序列之后离线处理。这个“在线”二字,正是整个项目的难度所在。
第一个难点是实时性。SLAM 系统本身就有跟踪、局部建图、回环检测三个线程在跑,如果点云构建再占用大量 CPU,跟踪线程很容易掉帧,最终导致定位精度崩盘。第二个难点是数据量。一张 640×480 的深度图,即使过滤掉无效像素,也往往能产生十几万个三维点,按每秒处理 10 个关键帧计算,一秒钟就是上百万个点。如果不做降采样和管理,内存和显示都会成为瓶颈。第三个难点是数据一致性。回环检测触发之后,ORB-SLAM2 会对关键帧位姿做图优化,位姿一变,已经生成的全局点云如果不跟着变,整个地图就会“散架”。
把这些问题拆开看,就能明白为什么这个扩展任务不是简单地在每个关键帧里生成一个 PCL 点云对象然后丢进全局容器里就完事的——它需要一套经过设计的同步、变换和滤波策略。
1.3 数据量先算一笔账
在我动手写第一行代码之前,先做了一个简单的数据量估算,事实证明这一步非常值得。
假设深度图分辨率是 640×480,即 307200 个像素。深度相机在室内环境下,有效深度值的比例通常在 60% 到 80% 之间,也就是说每帧能产生 18 万到 24 万个有效点。ORB-SLAM2 在一般场景下每秒大约选择 0.5 到 2 个关键帧,取中间值每秒 1 个关键帧,一分钟产生 60 个关键帧,每个关键帧按 20 万个点计算,一分钟就是 1200 万个点。PCL 的PointXYZRGB类型因为有内存对齐,每个点占 32 字节,一分钟的原始点云数据量是 1200 万 × 32 字节 ≈ 384MB。
这个数字直接吓了我一跳。如果不做体素滤波降采样,跑五分钟的场景就能吃下接近 2GB 内存,任何普通机器都扛不住。所以整个系统的第一个设计决策就明确了:全局点云必须做体素滤波,把密度压低到一个可控范围。比如 leaf size 取 0.02 米时,一个 20 万点的关键帧通常能压到 8 万到 10 万点,大幅缓解内存压力,同时重建表面依然平滑。
2. 稠密化方案选型:RGB-D 直接投影是当下最稳的路径
2.1 三条路线对比
把稀疏地图稠密化,业内大概有三条路线,我逐一分析过之后才做的决定,这里也分享给大家做参考:
路线一:RGB-D 直接投影。这是最朴素也最直接的思路——既然深度相机已经给了你每个像素的深度值,那就直接把深度图反投影到三维空间,生成点云,然后用 SLAM 输出的位姿把所有关键帧点云变换到全局坐标系。优点是逻辑清晰、误差可控、实时性好,几乎没有“猜”的成分;缺点是必须依赖深度相机,硬件上有限制。
路线二:多视图立体匹配。用双目相机或单目相机的多帧图像,通过极线搜索和块匹配,在像素级别恢复深度。代表实现有 ELAS、SGM、REMODE 等。这套方案不依赖深度相机,但计算量感人,尤其在 CPU 上做全图匹配,帧率很难保证,而且对纹理缺乏的区域(白墙、光滑地板)表现很差。ORB-SLAM2 的单目模式本身还存在尺度不确定问题,稠密化之前还得先把尺度拉到一致,工程复杂度陡增。
路线三:神经隐式表达。也就是最近很火的 NeRF-SLAM、3D Gaussian Splatting 这类方向。它们确实能构建质量极高的场景表示,视觉效果惊艳。但这类方案对 GPU 算力要求极高,在线运行的稳定性远不如传统方案,而且和 ORB-SLAM2 结合要做大量改造,不适合作为系列第一篇的切入点。
三者的对比如下表所示:
| 维度 | RGB-D 直接投影 | 多视图立体匹配 | 神经隐式表达 |
|---|---|---|---|
| 输入设备 | RGB-D 相机 | 双目/单目 | 不限,但最好有深度 |
| 实时性 | 高,CPU 即可 | 中低,计算量大 | 低,依赖高性能 GPU |
| 实现复杂度 | 低 | 高 | 极高 |
| 精度 | 受深度噪声影响 | 受纹理影响 | 高,但需要训练 |
| 适合场景 | 室内、深度相机可用 | 室外/无深度传感器 | 离线或近线重建 |
2.2 为什么选 RGB-D 直接投影
我最终选择了路线一,原因很实在:ORB-SLAM2 原生支持 RGB-D 相机输入模式,rgbd_tum这个 demo 就是专门为 RGB-D 方案设计的,系统在跟踪阶段已经会利用深度图辅助特征点深度估计。这意味着深度图和彩色图在 ORB-SLAM2 内部已经做了时间对齐和配准,我只需要在关键帧生成的时候把深度图“烤”一遍即可,不需要额外处理图像对齐问题,大幅降低了改造工作量。
另外一个原因是误差可控。RGB-D 直接投影的误差主要来自深度传感器本身的噪声和位姿估计误差,都是高斯性质为主的误差,方便用滤波手段处理。而多视图立体匹配的误差来源复杂,匹配错误会在点云中形成完全不相关的小面片,清理起来非常痛苦。
2.3 环境准备与依赖项
这套方案的基础依赖是 ORB-SLAM2 编译环境加 PCL 点云库。我在 Ubuntu 18.04 + OpenCV 3.4.1 + Pangolin 0.5 的环境下完成编译,下面列出关键依赖项:
- ORB-SLAM2 本体:包含 DBoW2、g2o、Sophus,按官方 README 编译即可
- Eigen3:
sudo apt install libeigen3-dev,注意 Eigen 默认装在/usr/include/eigen3 - PCL:
sudo apt install libpcl-dev pcl-tools,版本 1.8 以上即可 - Pangolin:用于显示窗口和轨迹可视化,编译时注意 0.5 和 0.6 版本之间有 API 差异
编译上有几个容易卡住的点。第一,Pangolin 0.6 把Viewport相关接口改了,如果 ORB-SLAM2 的源码是按旧版写的,直接编高级版本编译会报错。最稳妥的做法是直接下载 0.5 版本编译。第二,OpenCV 4 移除了cv::CV_LOAD_IMAGE_GRAYSCALE这类常量,ORB-SLAM2 的旧代码需要全局替换成cv::IMREAD_GRAYSCALE才能编过,所以如果机器上已经装了 OpenCV 4,要么自己改源码,要么用 Anaconda 或 Docker 搞一个 OpenCV 3 的环境。第三,PCL 和 ORB-SLAM2 都要用 C++11 编译,确保 CMakeLists 里加了-std=c++11,有些旧版本 g2o 在 C++17 模式下会编译失败。
注意:如果你用的是一台新机器,建议先单独编译一个示例 PCL 程序确认环境没问题,再把它加进 ORB-SLAM2 的 CMakeLists。千万不要一上来就大改项目工程,否则编译报错时根本分不清是 ORB-SLAM2 自身的问题还是 PCL 引入的问题。
3. 关键帧点云生成:从深度图到三维坐标的数学变换
3.1 针孔相机模型与内参矩阵 K
把深度图转换成三维点云,本质上是针孔相机模型的逆过程。先回忆一下相机内参矩阵:
K = [fx 0 cx] [ 0 fy cy] [ 0 0 1]其中fx、fy是焦距(像素单位),cx、cy是光心坐标。深度图上的一个像素(u, v),其深度值为d,根据针孔模型,这个像素对应的三维点在相机坐标系下的坐标为:
Z = d / depth_scale X = (u - cx) * Z / fx Y = (v - cy) * Z / fydepth_scale是深度值的单位换算系数,这是最容易被忽视的细节。TUM 数据集里的深度图是 16 位 PNG,单位是毫米,depth_scale取 1000.0;而很多 RealSense 驱动输出的深度单位是毫米,Kinect 的某些 SDK 又会输出浮点米,必须确认清楚。
有个来自我实际项目的经验:无论深度相机输出什么深度格式,先把图像 read 成 16 位单通道,用depth.ptr<unsigned short>访问像素,然后统一除以depth_scale换算成米,是最安全的处理方式。千万不要想当然地认为深度图一定是 float 或一定是毫米。
3.2 像素到三维点的代码实现
下面是核心的点云生成函数,输入是彩色图、深度图和相机内参,输出是pcl::PointCloud<pcl::PointXYZRGB>的局部点云:
void generatePointCloud(cv::Mat& rgb, cv::Mat& depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud, float fx, float fy, float cx, float cy, float depth_scale = 1000.0f, float min_depth = 0.1f, float max_depth = 6.0f) { cloud->clear(); int rows = depth.rows; int cols = depth.cols; // 预分配内存能明显提高效率 cloud->reserve(rows * cols * 0.7); for (int v = 0; v < rows; v++) { for (int u = 0; u < cols; u++) { // TUM数据集的深度图是16位单通道,单位为毫米 unsigned short d_raw = depth.ptr<unsigned short>(v)[u]; if (d_raw == 0) continue; float d = d_raw / depth_scale; if (d < min_depth || d > max_depth) continue; pcl::PointXYZRGB p; p.z = d; p.x = (u - cx) * p.z / fx; p.y = (v - cy) * p.z / fy; // 颜色直接从RGB图对应的像素读取 cv::Vec3b color = rgb.at<cv::Vec3b>(v, u); p.b = color[0]; p.g = color[1]; p.r = color[2]; cloud->points.push_back(p); } } cloud->width = cloud->points.size(); cloud->height = 1; cloud->is_dense = false; }这个函数的min_depth和max_depth两个阈值我强烈建议保留。深度相机在近距离(小于 0.1 米)时噪声极大,远距离(大于 6 米)时精度急剧下降,限定范围能从源头过滤掉大量无效点,比依赖后续滤波更省计算量。
3.3 为什么用关键帧而不是每一帧
沿着时间轴看,相邻两帧的位姿变化非常小,如果把每一帧都反投影生成点云并融合到全局地图,结果就是同一块表面会被重复采样几十次,形成巨大冗余。ORB-SLAM2 内部已经有一套通过熵、共视程度、空间分布等指标筛选出的关键帧机制,它选出的关键帧在视角和空间位置上都足够多样化,而且数量可控,非常适合直接复用。
我在实现里做的就是在LocalMapping线程插入新关键帧的地方加一个回调,每当有新关键帧产生,就取出它的彩色图、深度图和位姿,然后把点云生成任务投递到专门的点云处理线程。这样 SLAM 主流程完全不受影响,点云构建在另一个线程里并行跑,真正做到了“在线”。
还有一点想提醒大家:直接用关键帧有个隐藏好处——ORB-SLAM2 的关键帧都保存了经过 BA 优化的高精度位姿,这比直接用实时跟踪帧的位姿更稳,点云的拼接误差会更小。这也从侧面印证了“复用关键帧”这套设计在精度和性能上是双赢的。
3.4 深度图预处理:一个随便处理就能毁掉整个地图的环节
如果你以为拿到深度图直接反投影就完事了,那很快就会被实际效果教育。以 RealSense 和 Kinect 为代表的消费级深度相机,在以下场景中会产生大量错误深度值:
- 反光表面、黑色物体、镜面:红外图案无法正确投影,深度值要么为 0,要么成片错误
- 物体边缘:深度传感器在边缘处会“穿透”背景,形成俗称的飞点
- 透明物体:光线直接穿透,深度值完全没有意义
针对这些问题,除了前面提到的深度阈值,我还会对深度图做一次中值滤波。不要小看这一步,cv::medianBlur(depth, depth, 5)的代价很小,却能明显压掉孤立噪声点。另外,如果你的深度图和彩色图不是像素级对齐的,还需要先做配准——ORB-SLAM2 的 RGB-D demo 默认认为两者已经对齐,但这个假设换到你自己采集的数据上不成立,当面面上出现“颜色长在点云外面”的情况时,八成就是对齐出了问题。
4. 点云全局融合:位姿、坐标变换与滤波
4.1 把局部点云变换到世界坐标系
单个关键帧生成的局部点云是在该关键帧的相机坐标系下的,要拼接到全局地图,需要把它乘上该关键帧的位姿变换矩阵。ORB-SLAM2 中KeyFrame::GetPose()返回的是Tcw,即从世界坐标系到相机坐标系的变换矩阵。而我们从相机坐标系到世界坐标系需要的是Twc,即Tcw的逆矩阵。
因为Tcw是刚体变换,求逆可以直接用旋转矩阵转置 + 平移矩阵相乘的方式快速完成,无需调用昂贵的通用求逆函数。具体实现如下:
cv::Mat Tcw = kf->GetPose(); cv::Mat Twc = cv::Mat::eye(4, 4, CV_32F); // 旋转部分:R_wc = R_cw^T cv::Mat Rcw = Tcw(cv::Rect(0, 0, 3, 3)); cv::Mat Rwc = Rcw.t(); Rwc.copyTo(Twc(cv::Rect(0, 0, 3, 3))); // 平移部分:t_wc = -R_wc * t_cw cv::Mat tcw = Tcw(cv::Rect(3, 0, 1, 3)); cv::Mat twc = -Rwc * tcw; twc.copyTo(Twc(cv::Rect(3, 0, 1, 3))); // 使用PCL的变换接口把局部点云变换到世界坐标系 pcl::transformPointCloud(*local_cloud, *global_cloud, Twc);注意pcl::transformPointCloud的变换矩阵类型是Eigen::Matrix4f,ORB-SLAM2 返回的是cv::Mat,两者之间记得做一次类型转换。我就在这里翻了车,直接传入cv::Mat导致编译报错,最后统一封装了一个cvMat2Eigen工具函数才解决。
4.2 融合策略:累加、降采样、滤波三步走
全局点云的融合不是一个“把新点云 push 进去”就结束的操作,需要按以下三步走:
第一步,累加。把新关键帧变换后的点云追加到全局点云容器里。这个操作要加锁,因为点云处理线程和可视化线程可能同时访问全局点云。
第二步,体素滤波降采样。这是控制点云规模的核心手段。体素滤波的原理非常直观:把三维空间划分成固定大小的立方体网格(体素),每个体素内所有的点用它们的重心点代替。这样既保留了表面的基本形貌,又大幅减少了点的数量。
pcl::VoxelGrid<pcl::PointXYZRGB> voxel; voxel.setInputCloud(global_cloud); voxel.setLeafSize(0.02f, 0.02f, 0.02f); voxel.filter(*filtered_cloud);leaf size 的选择直接影响重建质量和资源消耗。以 TUM fr1 桌面场景(工作距离约 1 米)为例,实测下来 0.01 米能保留细节但数据量偏大,0.02 米在视觉质量和资源消耗之间最均衡,0.05 米则明显丢失物体轮廓,只适合预览和快速建图。
第三步,统计滤波去除离群点。即使做了深度阈值过滤,点云中仍然会残存一些孤立飞点。统计滤波的思想是:计算每个点与其 k 个最近邻的平均距离,如果这个平均距离超过全局标准差的一定倍数,就认为该点是离群点。
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(filtered_cloud); sor.setMeanK(50); sor.setStddevMulThresh(1.0); sor.filter(*clean_cloud);setMeanK取 50、setStddevMulThresh取 1.0 是我在室内场景下反复尝试得出的组合,能有效清除飞点又不会误删真实的表面点。如果场景比较杂乱或者噪声特别大,可以适当提高setMeanK,但stddevMulThresh不建议超过 2.0,否则会把点云磨得没有任何细节。
4.3 回环检测对已有点云的冲击——容易被忽略的大坑
这一节是整篇文章最值得仔细读的部分。ORB-SLAM2 的回环检测线程在发现回环后会对关键帧位姿进行图优化,优化之后部分关键帧的位姿会发生明显变化。如果你只是简单地把每帧点云按当时的位姿拼到全局地图里,那么回环优化一触发,新来的点云和之前的点云就会出现错位,原本闭合的场景变得七零八落。
第一次遇到这个问题时我一度怀疑是自己的坐标系搞错了,排查了很久。后来在 ORB-SLAM2 的LoopClosing::Run()逻辑里找到线索:回环矫正后所有关键帧的位姿都被更新了,但已经生成的点云并不会自动跟着变。
解决方案有两种,我在项目里是组合使用的:
方案一:全局重建。把所有关键帧的 ID 和对应的局部点云缓存下来,当回环优化完成后,遍历所有关键帧,用更新后的位姿重新生成全局点云。优点是干净彻底,缺点是需要保留所有关键帧的点云副本,内存开销大,回环后重算耗时明显。
方案二:增量修正。比较回环前后每个关键帧的位姿变化,把变换矩阵直接作用到该关键帧之前生成的全局点云上。这个方法高效,但需要自己维护关键帧与点云子集的映射关系,实现复杂。
考虑到这是系列第一版,我选择了方案一的简化版——回环后不立即重建,而是给系统设置一个标记,在每次可视化刷新时检测到标记后清空全局点云,重新累加后续关键帧。短期看会让地图闪一下子,但逻辑简单、不会引入额外的 bug。后续版本再按需优化。
4.4 线程设计:让点云构建不拖累 SLAM
在线构建最怕的就是点云处理阻塞了 SLAM 主流程。我最终采用的架构是一个典型的生产者-消费者模型:
LocalMapping线程发现新关键帧后,把关键帧指针丢进一个线程安全队列,然后立刻返回,不等待点云处理结果- 点云处理线程循环从队列里取出关键帧,执行深度图预处理、点云生成、坐标变换和全局融合
- 可视化线程用独立频率刷新全局点云,和点云处理线程通过互斥锁共享数据
这里有一个很重要的判断:点云处理线程的耗时不能超过关键帧产生的平均间隔,否则队列会无限增长,内存最终被耗尽。我在代码里设置了一个队列最大长度(比如 30),超过后直接丢弃最旧的关键帧——视觉上损失一点细节,但保证了系统长期在线运行的稳定性。
5. 实测与调优:在 TUM 数据集上跑通并记录数据
5.1 数据集准备与运行方式
调试阶段我没有急着接相机,而是先用 TUM RGB-D 数据集验证整体流程。TUM 的fr1_desk和fr2_desk是经典的室内桌面试点集,包含 RGB 图像、深度图像和 Ground Truth 轨迹,非常适合做功能验证。
TUM 数据集的 RGB 图和深度图是分开存储的,时间戳不完全对齐,运行前需要用官方提供的associate.py脚本生成关联文件。标准命令如下:
python associate.py rgb.txt depth.txt > associations.txt ./rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml ./fr1_desk ./associations.txtTUM1.yaml、TUM2.yaml、TUM3.yaml分别对应 TUM 的三种相机参数,记得按数据集类型选择,否则内参错误会让点云几何形状严重变形。
5.2 不同体素参数下的性能表现
我在fr1_desk序列(约 600 帧、31 个关键帧)上做了一组对比测试,机器配置是 i5-8400 + 16GB 内存,结果如下表:
| Leaf Size (m) | 关键帧平均点数 | 全局点云总点数 | 内存占用 | 建图耗时 | 表面质量 |
|---|---|---|---|---|---|
| 0.01 | 约 18 万 | 约 160 万 | 约 51 MB | 逐帧累计明显卡顿 | 细节丰富,边缘清晰 |
| 0.02 | 约 8 万 | 约 95 万 | 约 30 MB | 流畅 | 表面平滑,细节可辨 |
| 0.05 | 约 2 万 | 约 40 万 | 约 13 MB | 流畅 | 表面过密,物体边缘模糊 |
结论非常明确:在室内桌面场景下,0.02 米的体素尺寸是最佳平衡点。如果你的场景更大(比如整个楼层),可以考虑把 leaf size 提高到 0.03 至 0.05 米,牺牲细节换取可控的内存。
5.3 常见异常形态与排查方案
整个开发过程里我遇到了一批非常典型的异常形态,这里整理成排查表,方便大家对照解决:
| 异常现象 | 根本原因 | 解决方案 |
|---|---|---|
| 点云出现大量悬浮“飞点”,尤其在物体边缘 | 深度传感器边缘测量不稳定 | 提高深度阈值下限;增加统计滤波 |
| 点云整体镜像或翻转,物体位置看起来不对 | 相机内参fx/fy/cx/cy标定错误,或彩色图/深度图对齐错误 | 重新标定相机;确认ORB-SLAM2的yaml文件内参正确 |
| 颜色错位,彩色纹理“糊”在错误的位置 | RGB 与深度图像时间戳不对齐 | 使用对齐后的数据;ROS下正确设置时间同步 |
| 回环优化后地图明显错位错层 | 回环后关键帧位姿更新,点云未同步更新 | 实现回环后全局重建或位姿增量修正 |
| 点云数据量爆炸,内存持续增长 | 未做体素滤波,或 leaf size 过小 | 定期执行 VoxelGrid 降采样 |
| 主线程卡顿,界面几乎不动 | 点云处理直接在 SLAM 主线程里执行 | 改为独立点云线程 + 队列异步处理 |
这其中的“点云整体镜像”是最坑的一个,因为表面看起来“好像挺正常”,但细看又哪里都不对。我当时是在实测中突然发现的:房间的墙明明在左边,点云却显示在右边,来回换了好几种内参组合才定位问题。后面整理了经验,每次换相机或者换数据集,第一件事就是输出一个棋盘格的稀疏点云,检查长宽比和坐标轴朝向,确认无误后再做大规模建图。
6. 写在系列第一部分的最后
到这里,ORB-SLAM2 在线构建稠密点云的基础框架已经完整跑通了:从深度图反投影生成局部点云,用关键帧位姿把局部点云融合到全局坐标系,配合体素滤波和统计滤波控制数据质量,再通过独立线程保证不拖累 SLAM 主流程。整个过程在 TUM 数据集上验证过,帧率稳定、地图完整,可以说达到了第一版可用的标准。
我个人在实际开发中的体会是,这个项目真正的难点不在“写代码把点云拼起来”,而在于理解 ORB-SLAM2 每个线程的运行节奏和数据结构之间的关系——关键帧机制、位姿更新时机、回环检测的副作用,任何一个环节没吃透,后期都会被隐蔽的 bug 折磨。建议想复现的朋友先不要急着接真实相机,用 TUM 数据集把流程跑通、把参数调顺,再切换到在线数据源。
系列的第二篇,我计划重点写这几个方向:一是把回环后的位姿修正做完整,真正实现回环后全局地图的自动拼接;二是把点云导出和持久化做好,支持保存为 PCD/PLY 文件,方便后续接三维重建管线;三是讨论一下从稠密点云到占据栅格地图的转换,让这套系统能直接用于机器人导航。做出来之后会第一时间更新,感兴趣的朋友可以先自己动手体验一下第一部分的代码,有问题随时交流。