还真有朋友在群里问过——ORB-SLAM2 跑出来的地图能不能拿来直接做导航或者避障？答案很遗憾，默认版本只能给你一份稀疏的特征点云，指望它还原墙体轮廓、障碍物边界，甚至给机械臂抓取做几何测量，基本没戏。但这件事本身完全可以解决，而且不必推到离线重建那一步去，改改框架、加上稠密深度恢复，ORB-SLAM2 就能做到在线构建稠密点云。

这个方向适合的人群主要有三类：一是刚跑通 ORB-SLAM2、想深入理解多视图几何与 SLAM 耦合的初学者；二是做移动机器人导航、三维视觉测量，手里只有单目或双目相机、不想引入额外深度传感器的朋友；三是准备在 ORB-SLAM2 基础上二次开发、给地图模块做扩展的工程师。这篇文章会从框架层面拆解为什么原版只有稀疏点云，然后给出在线稠密建图的技术路线选型，接着落到具体实现——包括线程如何改、关键帧怎么挑、深度图怎么融合、点云后处理怎么做，最后把我实操过程中踩过的坑和排查方法一并整理出来。整个项目可以当作一个系列的第一篇，先解决“稠密点云怎么从 ORB-SLAM2 里长出来”这根主线，后面再展开谈纹理映射、语义融合、动态场景处理这些延展话题。

1. 为什么原版 ORB-SLAM2 只有稀疏点云

1.1 从 ORB-SLAM2 的线程架构看地图承载形式

ORB-SLAM2 的整个系统由四个线程并行驱动：Tracking、LocalMapping、LoopClosing、Viewer。Tracking 每来一帧图像，就提取 ORB 特征点、与局部地图匹配、估计相机位姿；LocalMapping 负责把新关键帧送入局部地图，做三角化、局部 BA、冗余关键帧剔除；LoopClosing 则检测回环，一旦发现就做位姿图优化和全局 BA。Viewer 拿来可视化，默认画出来的 MapPoints 本质上是 ORB 特征点经过多视图三角化后的稀疏三维位置集合。

这套架构的固有属性决定了地图的稀疏性。ORB 特征点本身就是图像上角点、边缘响应突出的像素位置，数量再多也有限，一个 640x480 分辨率的图像，ORB 特征提取上限通常在 1000 到 2000 个。就算把金字塔层数调高、特征数量调到 3000，得到的 MapPoints 也还是离散的特征点云，而不是连贯的表面。所以从根源上讲，ORB-SLAM2 的地图是“为了位姿估计服务”的，不是为了“重建表面”设计的。

1.2 稀疏点云和稠密点云在工程落地的差距

稀疏点云在定位、回环检测上表现优秀，但工程里需要它的地方往往不是定位，而是测量、感知和交互。举个例子：你用 ORB-SLAM2 跑一段走廊，输出的稀疏点云能看出左右两侧墙面上有特征的位置，但墙面本身长什么样、中间没有纹理的区域在哪里、地上的障碍物轮廓是什么形状，完全无从知晓。移动机器人靠这种地图做避障，大概率会撞上白墙——因为墙面上如果没有特征，MapPoints 就不会在那块区域生成。

我在做室内机器人的时候就踩过这个坑。一开始天真地认为 ORB-SLAM2 输出的 MapPoints 能直接当障碍物地图用，结果机器人对着光滑柜门直接怼过去了。那段位姿估计倒是很稳，回环也正常，但地图里那一整面柜门区域是空的。后来老老实实去做稠密点云，这才把感知闭环补上。

2. 在线稠密重建的技术路线选型

2.1 离线重建与在线重建的本质差异

刚接触稠密重建的朋友很容易一上来就想用 COLMAP、OpenMVS 这类离线方案。它们确实能产出极其漂亮、带纹理的稠密网格，但问题是全离线流程，先做特征匹配再做全局优化，最后密集匹配，一栋室内场景跑下来几个小时都算快的。离线重建的范式是“先采集、后处理”，你得把数据全部录完，放到离线环境里慢慢算，这对导航、避障、实时交互这些场景毫无意义。

在线构建的思路则完全不同：相机每到一个新位置，系统要在几十毫秒内完成深度估计、点云融合、地图更新。这种实时性决定了我们不能做全局能量最小化这类大计算量的优化，而是要在“局部一致性”和“处理速度”之间做取舍。

2.2 基于关键帧的等距立体匹配为什么更合理

在线稠密重建当前无非几条路。一种是直接上 RGB-D 相机，拿到硬件测得的深度图投影成点云，ORB-SLAM2 原版其实就有 RGB-D 的接口。另一种是双目视觉，靠相机之间的固定基线求视差。问题是，不是所有人都愿意加硬件。单目加测距传感器倒是省事，但深度估计又受限于单目尺度模糊，工程麻烦不小。

我这套方案选择的是“基于关键帧的等距立体匹配”——说白了，就是在单目 ORB-SLAM2 的基础上，把每一对新旧关键帧当成一对基线的“伪双目”，通过对极几何约束做稠密匹配来估计深度。为什么选它？一是和 ORB-SLAM2 的线程结构天然兼容，关键帧本来就有精确的位姿和共视关系；二是计算量可控，等距立体匹配相比全局优化在速度上有数量级优势；三是代码改动不需要动特征提取和位姿估计的底层，任何改过 ORB-SLAM2 的工程师都能快速上手。

这里要提醒一点，单目方案恢复的深度存在尺度不确定性。ORB-SLAM2 在初始化后会把 MapPoints 的尺度归一化，整个地图有了相对尺度，但没有物理尺度。如果你只是做导航避障，相对尺度够用；但要做精确测量，必须引入某种绝对尺度信息——比如加一个单点激光测距、已知高度的相机安装角，或者一个已知尺寸的标定物，把整个点云缩放一下。

2.3 深度图融合与直接拼接的取舍

拿到每对关键帧的深度图之后，面临一个选择：是把每一帧深度图直接转换成局部点云拼上去，还是转成体积表示做融合？直接拼接的优点是简单，但缺点极其致命——不同关键帧估计出的深度残差会导致重叠区域点云“糊”成厚厚的一层，墙面像是被抹了一层石膏。体积融合（比如 TSDF）效果好得多，能把多帧深度观测真正融合出一个平均表面，但内存开销和实现复杂度都会上一个台阶。

我采用的折中方案是在线拼接加体素滤波，配合合理的权重策略。具体来说：每一帧新深度图在融合前做一次双向一致性检查，把其中一帧中匹配失败或深度跳变的像素直接剔除；融合时给不同关键帧设置不同的权重——新关键帧的权重高、旧关键帧的权重低，这样重叠区域的点云会偏向最晚看到的观测，墙面厚度问题得到了很大缓解。整套流程下来的效果，比真正做 TSDF 会差一些，但在单目 + CPU 实时这条约束下，它基本是性价比最优解。

3. 系统架构拆解：怎么把稠密模块“塞”进 ORB-SLAM2

3.1 稠密建图模块的线程与数据流设计

给 ORB-SLAM2 加稠密点云，最忌讳的就是在 Tracking 主线程里直接做稠密匹配。Tracking 线程是实时性的命脉，一帧耽误超过 30 毫秒，定位精度就会肉眼可见地下降。我的方案是新增一个独立的 DenseMapping 线程，负责消费 LocalMapping 输出的关键帧数据，执行立体匹配、深度恢复和点云融合。

数据流这样设计：Tracking 每次估计完当前帧位姿后，如果系统判定当前帧适合作为关键帧，就把它丢给 LocalMapping。LocalMapping 完成词袋计算、新增 MapPoints 三角化、局部 BA 之后，会往一个专门为稠密建图准备的队列里推送“已完成优化”的关键帧。这一步有讲究——必须等局部 BA 结束再推送，否则关键帧的位姿还会在后续优化中变化，你提前用未收敛的位姿去做立体匹配，深度图就作废了。DenseMapping 线程从队列里取出关键帧，和它的参考帧做等距立体匹配，生成深度图，再经过滤波和融合，把点云写入一个全局地图数据结构。

3.2 关键帧选择与参考帧匹配策略

哪两个关键帧之间做立体匹配，直接决定了深度图的质量。我的策略分两层：先选参考帧，再匹配。

参考帧的选取条件是：优先选取与当前关键帧共视程度最高、且基线长度适中（大于一定阈值但又不会大到视差超限）的上一关键帧。为什么强调“上一关键帧”？因为相邻关键帧之间的视角变化小，光照差异不大，匹配成功率最高。如果遇到大幅度旋转导致两者共视面积不足，就回退到时间上更早的关键帧，再不行就干脆丢弃这一关键帧的稠密恢复——宁缺毋滥，一张质量差的深度图带来的噪声，往往比不添加点云更糟糕。

基线的选择还需要量化。以室内场景为例，帧率在 30fps、相机行进速度在 0.3m/s 左右，相邻关键帧的基线大致在 5 到 15 厘米。这个基线配合 10 到 20 米的最大深度范围，视差能达到亚像素到几个像素，深度分辨率的性价比处在甜蜜点。基线太短了，深度误差呈平方级膨胀；太长了，则会因为视角差异过大导致遮挡和匹配歧义。

3.3 ORB-SLAM2 原框架的改动边界

改 ORB-SLAM2 有个原则：尽量做加法，不要做减法。我梳理了实际需要改动的文件，总共就三处。第一处是在 LocalMapping 线程里，新增一个关键帧发布器的回调函数，在新关键帧完成局部 BA 后触发；第二处是在 Tracking 线程保存当前帧对应的左目图像（如果本来就是单目或双目配置，保存的是主相机图像），因为后续 DenseMapping 需要原始灰度图去做像素级匹配；第三处是 Viewer 线程替换点云绘制方式，默认的 MapPoints 绘制换成我们融合出的稠密点云显示。

这样改动的边界非常干净。系统的特征提取、位姿估计、回环检测、全局 BA 这些核心逻辑完全不动，相当于在原有系统旁边挂了一个“外挂模块”。依赖关系上，DenseMapping 只读取关键帧位姿和图像数据，不反向修改任何地图信息，因此即便稠密模块崩溃或性能退化，原 SLAM 系统依然能稳定运行。这种模块解耦的思路，对后续迭代维护也特别重要——我后来在这个基础上加语义分割层、纹理映射层，都因为模块边界清晰而省了大量精力。

4. 核心实现细节与参数调优

4.1 等距立体匹配的完整流程

这里讲一下等距立体匹配的具体实现细节。输入是参考帧 Ir 和当前帧 Ic，以及它们的位姿 Tr、Tc。通过相对位姿变换可以计算出极线几何关系，然后把当前帧的图像沿极线方向做校正变换，使得对应点在两幅图像中位于同一水平扫描线上。这一步可以调用 OpenCV 的 cv::initUndistortRectifyMap 配合 cv::remap 完成，做完之后匹配就从二维搜索退化成一维搜索，计算量大大降低。

接下来用块匹配算法在极线上搜索每个像素的最佳视差。块大小我建议取 7x7 到 11x11 之间。取小了，低纹理区域的匹配噪声会明显增强；取大了，边缘会被磨平，深度不连续的位置误差很大。搜索范围则根据相机内参、基线和最大深度范围计算，比如基线 10 厘米、焦距 500 像素、最大深度 15 米，视差范围大约上下几百个像素。算法选型上，可以用简单的 SAD（绝对误差和）或 Census 变换加汉明距离，这两种在 CPU 上都能跑到近实时。SAD 的优势是直观好调，Census 的优势是对光照变化鲁棒性强，我实测下来室内场景两者差异不大。

4.2 深度图的滤波与后处理三板斧

立体匹配生成的初始深度图，噪声可以说是“漫天飞雪”，直接用必然是灾难。我的后处理流程有三板斧。第一板斧是左右一致性检查：对每一像素，从左图算到的视差 d1，再到右图对应位置算反向视差 d2，如果两者相差超过一个像素阈值（我一般取 1，偶尔取 2），就认为这个像素的匹配不可靠，直接剔除。这招对遮挡区域、无纹理区域的误匹配非常有效。

第二板斧是中值滤波：对剩余的有效深度像素做 5x5 窗口的中值滤波，能把孤立的离群深度点抹掉，同时尽量保留深度边缘。第三板斧是深度边界保持：需要额外计算图像梯度，在梯度大的地方降低滤波强度，避免把物体边缘“磨圆”。这三板斧处理完之后，深度图基本达到了可以融合进点云的质量。整个过程是纯像素操作，在 CPU 上帧耗时大约 5 到 10 毫秒，对系统整体压力可以接受。

4.3 全局点云融合与体素滤波的参数选择

深度图恢复之后，需要投影反算成三维点，再根据相机位姿变换到世界坐标系，插入全局点云地图。如果每一帧都往全局点云里插几万个点，内存很快就会爆炸。我的方案是每处理完一帧深度图，就对全局点云做一次体素滤波降采样。体素大小选多大，取决于你的使用场景。以室内机器人为例，体素设成 0.02 米（2 厘米）就能比较完整地保留墙面和障碍物轮廓，点密度也足够用于导航代价地图生成；如果场景特别大或者内存吃紧，可以放宽到 0.05 米。

体素滤波的机制很简单：把空间划分成大小相同的立方体格，每个格子里只保留一个点（通常取格内所有点的重心）。它同时起到了三个作用：降采样、去噪、均质化密度。我用 PCL 的 pcl::VoxelGrid 实现，实测 2 厘米体素下，一个 50 平米室内场景跑完大约产生 20 万到 50 万个点，内存占用在几百 MB 量级，完全可控。

4.4 稠密点云构建的关键参数速查表

这里把我在多组室内场景中调出来的“稳妥参数组合”整理成一张速查表，抛砖引玉，具体还得结合你自己的传感器和场景微调。

4.5 深度图与点云融合的代码骨架

为了让这套流程有可落地的感觉，我这里给出 DenseMapping 线程的核心代码骨架。这部分基于 C++、OpenCV，以及 ORB-SLAM2 的关键帧位姿接口——命名可能因版本稍有差异，思路可以完全照搬。

void DenseMapping::Run() { while (1) { // 从队列取关键帧，等待 10ms 防止忙等 if (!keyframeQueue_.empty()) { KeyFrame* kf = keyframeQueue_.pop(); // 选择参考帧：共视程度最高且距离适度 KeyFrame* ref = SelectReferenceKeyframe(kf); if (ref == nullptr) { continue; } // 灰度图直接从关键帧拿 cv::Mat I1 = ref->GetGrayImage(); cv::Mat I2 = kf->GetGrayImage(); // 计算两帧相对位姿 cv::Mat T12 = ref->GetPoseInverse() * kf->GetPose(); // 步骤 1：对极校正，让极线水平 cv::Mat R1, R2, P1, P2, Q; cv::Mat K = ref->GetCamera()->GetCameraMatrix(); cv::Size imgSize = I1.size(); cv::stereoRectify(K, cv::Mat(), K, cv::Mat(), imgSize, T12(cv::Rect(0,0,3,3)), T12.colRange(0,3).rowRange(2,3), R1, R2, P1, P2, Q); cv::Mat map1x, map1y, map2x, map2y; cv::initUndistortRectifyMap(K, cv::Mat(), R1, P1, imgSize, CV_32FC1, map1x, map1y); cv::initUndistortRectifyMap(K, cv::Mat(), R2, P2, imgSize, CV_32FC1, map2x, map2y); cv::Mat rectI1, rectI2; cv::remap(I1, rectI1, map1x, map1y, cv::INTER_LINEAR); cv::remap(I2, rectI2, map2x, map2y, cv::INTER_LINEAR); // 步骤 2：SAD 块匹配求视差 cv::Ptr<cv::StereoMatcher> matcher = cv::StereoSGBM::create(minDisp, numDisp, 9); cv::Mat disp; matcher->compute(rectI1, rectI2, disp); // 步骤 3：根据 Q 矩阵把视差图反投影为三维点 cv::Mat points3D; cv::reprojectImageTo3D(disp, points3D, Q, true); // 步骤 4：换成世界坐标系、生成 pcl::PointCloud ConvertToPointCloudAndFilter(points3D, kf->GetPose(), globalCloud_); // 步骤 5：体素滤波降采样，控制地图规模 pcl::VoxelGrid<pcl::PointXYZ> downSampler; downSampler.setInputCloud(globalCloud_); downSampler.setLeafSize(leafSize_, leafSize_, leafSize_); downSampler.filter(*globalCloud_); } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }

简单解释几个关键点。cv::stereoRectify 的输入是两帧的相对位姿，不需要标定双目相机——这说起来是整套方案里最妙的地方，等于把“时间上相邻的单目关键帧”强行当成了“空间上的双目相机”。这样求出来的 Q 矩阵含义其实不是传统双目里的基线尺度，而是时间基线的尺度，所以反投影出来的三维点会自动归一化到 ORB-SLAM2 的尺度空间中，不需要额外对齐。步骤 4 的位姿变换矩阵乘上 Q 反投影得到的相机坐标系坐标，就能得到世界坐标系下的点云了。注意最终还要把深度值不合法（NaN 或者无穷大）的点全抹掉，这一步在实际工程里比大多数细节都更影响点云质量。

5. 踩坑记录与问题排查实录

5.1 墙面怎么变成了“厚被子”

这是我第一次把深度图全量融合进点云之后遇到的第一个视觉灾难——一组墙面点云，厚度居然有 10 厘米。所有镜头扫过的地方都像是在墙上贴了一层棉被。排查之后发现是三个因素叠加：一是相邻关键帧位姿有估计误差，即便局部 BA 收敛之后，残余误差在三角化时会被放大；二是等距立体匹配在低纹理区域视差估计会整体偏移；三是我直接做了“加法融合”，旧点云权重和新点云权重一样，误差就不断累积。

解决思路分两步：首先在融合前加入双向一致性检查，把低置信度的匹配直接丢弃；其次采用带衰减的加权融合，新帧点云的权重是旧帧的 2 到 3 倍。实测墙面厚度从 10 厘米降到了 2 到 3 厘米，视觉上已经接近墙体本身的厚度。如果还想再薄，就得走 TSDF 或 Poisson 重建的路子，但那就是离线或半在线的范畴了。

5.2 内存持续增长导致系统崩溃

第二个坑是在长走廊场景中出现的。刚开始一跑长走廊，系统在几分钟内内存就飙升到好几个 GB，然后直接 OOM。原因在于关键帧队列没有设置容量上限，而全局点云的体素滤波尽管每次都在降采样，但如果场景一直在新增区域，点云总量必然持续增长。此外 ORB-SLAM2 自身的 MapPoints 管理和关键帧剔除机制只服务于稀疏地图，不会帮我管理稠密点云的内存。

解法是双管齐下：给关键帧队列设置上限（我取 200），超过上限就丢弃最旧的关键帧，避免 DenseMapping 永远在追赶新数据；同时把全局点云按照固定体素持续降采样，并定期用统计学滤波把周围邻居稀少、处于“孤立飘散”状态的点剔除。经过这两个手段，一个 50 平米室内场景跑完整段路径，内存稳定在 1GB 到 2GB 之间，长时间运行不再有崩溃风险。

5.3 纹理稀疏区域的空洞怎么处理

办公室白墙、纯色地板、无花纹天花板，这些纹理稀疏区域是立体匹配的重灾区——匹配程序找不到足够的灰度差异，输出基本要么是噪声要么是空白。我最早以为是自己匹配参数没调好，调来调去，白墙依旧白墙。后来想明白了：立体匹配的本质决定了它依赖图像纹理，没有纹理就没有可匹配的像素差异，再调参也变不出来。

面对这种情况，我做了三个层面的处理。第一是在线层面：如果当前关键帧统计出的有效匹配率低于某个阈值，就跳过这一帧的稠密恢复，绝不硬生成垃圾数据；第二是算法层面：对低纹理区域采用图像插值补全，比如从邻近有效深度像素做拉普拉斯插值，能稍微缓解空洞；第三是硬件层面：在有条件的情况下，可以考虑补一个结构光或散斑投射器来人为增加纹理，我身边有团队就是这么干的，效果立竿见影。说了这么多，真的想提醒大家：算法不是魔法，传感器层面的瓶颈有时候绕不过去，识别这个瓶颈并且有意识地避开它，比强行硬解更务实。

5.4 回环闭合后点云出现了“重影”

回环检测是 ORB-SLAM2 的看家本领，但它给稠密建图带来一个副作用。回环闭合后，全局 BA 会调整关键帧的位姿，而我早期实现的 DenseMapping 模块融合点云时，用的是关键帧在被优化之前的旧位姿。于是场景中同一面墙，在回环前和回环后分别被投影到了两个不同的位置，视觉上就是重影。

这种问题的根因在于位姿更新和数据融合的时序不一致。解决办法是：DenseMapping 消费关键帧时，先从 ORB-SLAM2 的地图对象中查询该关键帧的最新位姿；若发现位姿已经更新过，则丢弃先前基于旧位姿融合的那部分局部点云，用新位姿重新从深度图生成一次。这个“重融合”的机制虽然会带来一点计算开销，但能保证地图的一致性。我的经验是，每一百个关键帧里触发重融合的通常只有几个，代价可控。

5.5 点云坐标系与导航地图坐标系怎么对齐

最后这一个坑，属于集成阶段的问题。稠密点云构建出来了，但你是否发现点云坐标系跟机器人底盘坐标系的朝向有误差？很多 ROS 场景下，ORB-SLAM2 启动时的初始相机坐标系和 base_link 坐标系不是天然对齐的。如果直接把点云发布到 map 或 odom 话题下，导航模块收到的地图是斜的、歪的，代价地图分分钟失灵。

我踩过这个坑之后，现在的做法是：在启动 ORB-SLAM2 构建稠密点云之前，先让机器人原地做一次旋转，让 SLAM 系统完成初始化。然后把初始相机坐标系到 base_link 的静态变换通过标定方式写进 TF。更粗暴但有效的办法是：系统初始化完成后，让机器人在已知直线方向上走一段，通过比较 SLAM 轨迹和真实位移来反解初始偏航角。这个校准步骤千万不要省，一旦点云歪着你后面做导航定位就是连锁反应地踩坑。

6. 一套可复用的实验验证方法

6.1 用开源数据集做定量验证

自己录数据当然好，但问题是你不知道真值，遇到问题很难定位是算法问题还是数据问题。我建议先拿开源数据集跑通流程，再做真实场景测试。TUM RGB-D 数据集、EuRoC MAV 数据集都有提供真值轨迹和标准地图，可以用来做两件事。

第一件事是轨迹一致性验证：把 ORB-SLAM2 输出的相机轨迹和真值轨迹做对齐，看 ATE（绝对轨迹误差，Absolute Trajectory Error）和 RPE（相对位姿误差，Relative Pose Error）是不是在合理范围。如果轨迹本身漂移严重，稠密点云再怎么做也不会对。这个前置验证一定不能少——很多朋友跑来问我为什么点云模糊，结果一查是 ATE 已经超过 10 厘米了，这时候去调立体匹配参数纯属白费力气。第二件事是点云精度验证：在 TUM 的某个室内房间场景，用平面拟合法提取点云中的墙面和桌面平面，和数据集提供的标准平面模型做比对，看看点云平面拟合的 RMS 误差是多少。这是我目前觉得最直接也最不费劲的定量指标。

6.2 自采数据时的评估技巧

自己录制数据没有真值，怎么验证点云质量？我的土办法是“原路返回法”。手持相机或装在机器人平台上，沿一条路径走一遍，记住起点和终点。在起点放置一个平面标定板或贴几根反光标记，走完一圈回来，再看点云中标记点的三维位置和实际位置的偏差。这可以粗略估计累积漂移和点云精度的综合水平。

更精细一点的做法是：在场景里放几个已知尺寸的箱子、球体或标准平面，跑完离线处理点云，用这些物体的几何尺寸当参照物。比如一个边长为 30 厘米的立方体，点云中拟合出来的立方体边长如果是 33 厘米或 27 厘米，那误差水平就一目了然了。我实测在 5 米范围内、相机运动平稳的前提下，这套方案的点云尺寸误差能控制在 3% 到 5% 以内，足够做机器人避障和粗略体积测量。

7. 性能调优方向的硬核建议

7.1 CPU 实时性的瓶颈在哪

DenseMapping 线程在 CPU 上跑等距立体匹配，最耗时的瓶颈集中在块匹配那一步。默认的 cv::StereoSGBM 是参数全面的实现，优化的准确性不错，但速度不尽如人意，在 640x480 分辨率下可能耗掉 50 毫秒以上。如果追求实时性，推荐换用 OpenCV 里更轻量级的 cv::StereoBM，或者直接上 SIMD 优化过的自定义 Census 匹配核。我自己实测，在同样的参数下，StereoBM 比 SGBM 快 3 到 5 倍，质量会略差一些，但经过滤波融合后差值可以接受。

若分辨率可以压到 320x240，耗时会进一步降到 10 毫秒左右。如果你的机器人平台用的是 NVIDIA Jetson，那更简单，把块匹配扔到 CUDA 上并行处理，DenseMapping 线程甚至能跑在比 Tracking 线程还低的延迟时间范围内。汇总一下数据：纯 CPU 下 StereoBM 320x240 大约 10 到 15 毫秒，SGBM 640x480 大约 30 到 60 毫秒，CUDA 加速的 640x480 可以压到 5 毫秒以内。具体怎么选，就看你对点云质量的需求有多高。

7.2 地图数据结构的优化空间

全局点云用 PCL 的 pcl::PointCloud pcl::PointXYZ 存储简单直观，但在线增长型地图中会频繁触发内存分配、拷贝、遍历，性能瓶颈愈发明显。优化的方向有两个。

一是数据结构换用八叉树或哈希体素网格，这样点云的插入、近邻查询、体素降采样都能做到按需计算，而不是每次全图扫描。另一个方向是分级管理：把点云按下采样层级分成 Local Cloud（最近 N 帧附近）和 Global Cloud（历史区域），只在 Local Cloud 更新后做增量融合，Global Cloud 周期性触发一级降采样。这个小改动能让单帧处理时间从几十毫秒降回十几个毫秒区间，长时间运行也不会因为数据量增长而拉到帧率。

7.3 什么时候该上 GPU 或专用加速单元

如果你的目标环境是室内复杂场景，对点云质量和密度要求都比较高，同时又必须保证 30fps 实时显示，纯 CPU 方案的余量就不够了。这时可以考虑三种升级路径：一是把等距立体匹配的块匹配部分改为 CUDA 实现，这样可以保留下采样、滤波这些 CPU 逻辑，改动集中在计算热点上；二是引入 TensorRT 加速的深度学习立体匹配网络，比如基于 RAFT-Stereo 或 STTR 的模型，精度可以比传统方法上一个档次，代价是显存占用会明显上升；三是直接换 RGB-D 相机回避立体匹配，把深度图获取交给硬件，SLAM 侧只做深度图和位姿融合。

第三种的性价比往往比前两种更高。我认识不少做移动机器人的团队，一开始死磕单目立体匹配，最后全换成了小体积的 ToF 或结构光深度相机。原因很简单：硬件深度传感器的实时深度质量稳定，不受场景纹理干扰，虽然价格贵一点点，但省下来的算法调试成本远超传感器差价。不过，用 RGB-D 方案也会丢到“纯单目在线稠密”这个研究方向的延展性，所以怎么选，终究要看你的目标平台和产品定位。

8. 写在最后的一些体会与延展方向

实际做完这套在线稠密建图系统，我最大的感受是：ORB-SLAM2 这类稀疏 SLAM 系统的架构，比想象中更适合做功能扩展。它的 Tracking、LocalMapping、LoopClosing 原本专注定位，但关键帧、共视图、位姿图这些中间产物，简直是为稠密重建量身定做的。只要把 DenseMapping 模块的输入输出接口设计清楚，整个融合过程会很自然。

一个值得注意的体会是：很多人在读完论文和源码后，动手做 SLAM 二次开发时会陷入“什么都想加、最后什么都没调明白”的泥潭。我的经验是，任何扩展模块都要有一条“最简可用路径”——先把相机对准一面有纹理的墙，跑通从关键帧到点云的整条链路，看到屏幕上出现粗略的墙面点云，再逐步调参数、加后处理、处理边界情况。这条路径能让你区分“算法核心问题”和“工程实现问题”，不至于把大半时间耗在无关紧要的角落。

后面如果继续深入这个系列，我会考虑展开三个方向。第一个是稠密点云的纹理映射，把 RGB 颜色贴到重建表面上，让输出更接近真实场景；第二个是动态物体处理，当前方案把所有车辆、行人、动物都当成了静态环境的一部分，运动物体会在点云里拖出残影；第三个是稠密点云与语义分割的融合，点云有了语义标签，机器人才真正理解“地面在哪、墙壁在哪、障碍物是被允许的还是需要绕开的”。如果你正在用 ORB-SLAM2 做稠密建图，或者有自己的方案思路，欢迎带着具体问题来交流，把你们场景里遇到的细节拿出来聊聊，往往比单看技术方案有更多收获。