本文通过ORB-SLAM3中的两个核心模块------ORB特征提取器和稠密点云建图，来深入探索视觉SLAM背后的技术原理。本文将以源代码解析的方式，带你走进SLAM的奇妙世界。
一、ORB-SLAM3：从稀疏到稠密的演进
ORB-SLAM3是一个支持视觉、视觉加惯导、混合地图的SLAM系统，可以在单目、双目和RGB-D相机上利用针孔或者鱼眼模型运行。它是第一个基于特征的紧耦合VIO系统，仅依赖于最大后验估计（包括IMU在初始化时）。
然而，ORB-SLAM3生成的是稀疏点云地图，只能定位，无法直接用于导航任务。稀疏地图虽然能够满足定位需求，但在需要与真实环境进行物理交互的场景中（如机器人导航、避障、抓取等），我们需要更密集的环境表示------这就是稠密点云建图的意义所在。
为了实现稠密建图，基于ORB-SLAM3的架构，额外添加了一个稠密建图线程。这个线程在Tracking过程中，将关键帧的RGB图像和深度图像加入到稠密点云线程中，利用RGB-D图像和相机姿态数据，实现了3D场景的密集点云重建。其实高翔的开源项目（ORBSLAM2_with_pointcloud_map）已经有针对ORBSLAM2的稠密建图功能。现在从高博的源码略加修改，实现OBRslam3的稠密建图功能。
二、ORB特征提取器：让机器"看见"世界的关键
ORB（Oriented FAST and Rotated BRIEF）特征是一种兼具速度和旋转不变性的局部特征。ORB-SLAM3中的ORBextractor.cc文件实现了这一核心模块。
2.1 构建图像金字塔 ------ 解决尺度不变性
为了让机器在不同距离下都能认出同一个物体，系统会构建图像金字塔：将原图像逐层缩小，形成一系列不同尺度的图像。这样，远处的小物体在金字塔底层也能变成"大物体"被检测到。
// 逐层计算缩放系数
mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
代码中，每一层的缩放因子累乘得到，使得算法具有了尺度不变性。
2.2 FAST角点检测 ------ 快速找到"兴趣点"
FAST角点检测算法通过比较像素与其周围圆周上像素的灰度差异，快速筛选出潜在的角点。ORBextractor中采用了自适应阈值：先用较严格的阈值iniThFAST检测，如果角点数量不足，则改用更宽松的minThFAST。
2.3 四叉树均匀化 ------ 让特征点分布更均匀
直接提取的FAST角点往往会扎堆在纹理丰富的区域，导致地图构建不均衡。ORBextractor使用四叉树算法，将图像不断分割成小方块，并保证每个子区域内只保留响应值最大的一个特征点。
2.4 计算特征点方向 ------ 旋转不变性
ORB特征的一大创新是加入了灰度质心法计算方向。简单来说，把特征点周围的图像块看作一个"刚体"，计算其灰度重心，从几何中心指向重心的方向就是该特征点的主方向。
static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
{
// 计算图像矩 m10, m01
// 使用 fastAtan2 得到角度
return fastAtan2((float)m_01, (float)m_10);
}
有了这个方向，后续的BRIEF描述子就可以通过旋转采样点来获得旋转不变性。
2.5 生成BRIEF描述子 ------ 给特征点"身份证"
BRIEF描述子通过比较特征点周围随机点对的灰度值，生成一个二进制串（256位）。ORB在BRIEF的基础上，利用前面计算的方向对随机点进行旋转，从而得到Steered BRIEF，使其具备旋转不变性。
最终每个特征点都有一个256位的独特"指纹"，可用于快速匹配。
三、稠密点云建图：让世界从"点"变成"面"
ORB-SLAM3原始版本只输出稀疏的特征点地图。而许多应用（如机器人导航、三维重建）需要更密集的环境表示。pointcloudmapping.cc文件实现了一个增强模块，利用RGB-D相机（或双目）构建稠密点云地图。
3.1 从关键帧生成点云
每当系统插入一个新的关键帧，insertKeyFrame函数会接收对应的彩色图和深度图。在单独的线程中，generatePointCloud函数将每个像素反投影到三维空间：
p.z = d;
p.x = ( u - mCx) * p.z / mFx; p.y = ( v - mCy) * p.z / mFy;
p.r = imRGB.ptr<uchar>(v)[u*3];
p.g = imRGB.ptr<uchar>(v)[u*3+1];
p.b = imRGB.ptr<uchar>(v)[u*3+2];
这里利用了相机内参（mFx, mFy, mCx, mCy）和深度值，计算出每个像素在世界坐标系下的坐标及颜色。
3.2 点云滤波与压缩
原始的稠密点云数据量巨大，且包含噪声。模块中使用了两种滤波器：统计滤波（剔除离群点，如深度传感器噪声）和体素滤波（将空间划分为小立方体，每个体素内只保留一个代表点，大幅压缩数据量）。
voxel.setLeafSize( resolution, resolution, resolution); statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
3.3 回环检测时的点云更新
回环检测是SLAM中至关重要的一环。当系统检测到回环时，关键帧的位姿会被全局优化调整。相应地，已经生成的点云也需要"搬家"到正确的位置。
updatecloud函数会遍历所有受回环影响的关键帧，重新变换它们对应的局部点云，并合并到全局地图中。代码甚至用"五角星"图案在控制台打印出回环处理的醒目提示，非常有趣：
void PointCloudMapping::Five_pointed_star() { // 打印一个漂亮的大五角星 }
3.4 坐标系转换与发布
为了便于在ROS中可视化，模块将点云从SLAM坐标系转换到机器人常用的坐标系（如/pointCloud），并通过ROS话题发布，方便使用Rviz等工具实时查看建图效果。
Eigen::Matrix4f m;
m<< 0,0,1,0, -1,0,0,0, 0,-1,0,0;
pcl::transformPointCloud (source, out, transform);
四、从ORB-SLAM2到ORB-SLAM3：点云建图模块的移植
pointcloudmapping.cc这个文件，正是高翔博士在其开源项目ORBSLAM2_with_pointcloud_map中的核心工作。高翔博士修改增加了点云模块，在程序中添加了pointcloudmapping的相关程序，使ORB-SLAM在运行数据集时能直接显示点云图。其基本思想是：为每个关键帧构造相应的点云，然后依据从ORB-SLAM2中获取的关键帧位置信息将所有的点云拼接起来，形成一个全局点云地图。下面是一些需要改动的内容介绍。
4.1 核心逻辑：基本一致
-
点云生成（generatePointCloud）
：功能逻辑完全一致，都是从深度图反投影生成三维点云，并利用体素滤波和统计滤波进行优化。
-
回环更新（updatecloud）
：高翔原版代码并未实现回环检测时的点云更新功能。因此，这部分逻辑是你提供的代码独有的增强功能。
-
坐标系转换（Cloud_transform）
：PCL库默认的坐标系（x前，y左，z上）与SLAM系统的相机坐标系（z前，x右，y下）存在差异，因此需要一个变换矩阵来转换坐标轴方向。这个核心的变换逻辑在两个版本中是完全一致的。
4.2 代码适配微调
由于两个版本构建在完全不同的SLAM系统上，为适配新的系统架构，进行以下修改是必然的：
-
命名空间调整
：为了适配ORB-SLAM3，代码所在命名空间从ORB_SLAM2变为了ORB_SLAM3。
-
API接口适配
：为了与ORB-SLAM3新的KeyFrame类和Converter工具类对接，相关的函数调用进行了更新。
4.3 回环更新（updatecloud）
高翔原版的 ORBSLAM2_with_pointcloud_map 虽然能够生成稠密点云，但并没有处理回环检测后地图不一致的问题。也就是说，当 SLAM 系统发现之前走过的路径形成闭环，并对所有关键帧位姿进行全局优化调整后，原有点云依然停留在旧位姿下，导致地图出现"重影"或错位。您这份代码实现了一个完整的回环点云修正流程，下面我们逐段剖析其实现细节。
ORB-SLAM 的后端会不断优化相机轨迹和地图点位置。当检测到回环时，系统会执行全局光束法平差（Global BA），调整所有关键帧的位姿。此时，之前根据旧位姿拼接起来的点云就与真实世界不再对齐。如果不更新点云，最终得到的地图会在回环闭合处出现明显的断裂或双重结构。因此，updatecloud 的核心任务就是：利用回环优化后的新位姿，重新生成受影响的局部点云，并替换掉全局地图中的错误部分。
代码逐段解析
4.3.1 触发条件：lastKeyframeSize == LoopKfId
在 NormalshowPointCloud 主循环中，有一个判断：
if(lastKeyframeSize == LoopKfId) updatecloud();
这里的 LoopKfId 应该是在外部（如 LoopClosing 线程）检测到回环后设置的一个关键帧 ID 阈值，lastKeyframeSize 是当前已处理的关键帧计数。当两者相等时，意味着回环发生且相关关键帧已到位，于是调用 updatecloud() 进行点云修正。
4.3.2 获取受回环影响的关键帧列表
currentvpKFs 是在 insertKeyFrame 中传入的，它保存了经过回环优化后的关键帧集合（通常是回环两侧的所有关键帧）。内层循环遍历已存储的所有点云块（pointcloud 向量），找到 ID 匹配的关键帧对应的原始点云（pcE，即 generatePointCloud 返回的局部点云）。注意：这里保存的是生成时相对于相机坐标系的原始点云，而不是已经变换到世界坐标系的点云。这为后续用新位姿重新变换提供了基础。
4.3.3 用优化后的位姿重新变换点云
currentvpKFs[i]->GetPoseMat() 获取的是回环优化后的相机位姿（世界 → 相机）。由于 pcE 原本是相机坐标系下的点云（由深度图反投影得到，原点在相机光心），要将其变换到世界坐标系，需要乘以 T 的逆（即相机→世界的变换矩阵）。所有重新变换后的点云累加到临时点云 tmp1 中。
4.3.4 体素滤波降采样
重新生成的点云可能非常密集，直接用 tmp1 替换全局地图会导致内存爆炸。因此先用体素滤波器（voxel）进行下采样，生成 tmp2。
4.3.5 安全更新与备份
这里通过判断当前全局点云点数是否足够来确认回环有效性（点数太少说明还未构建足够地图，可能误触发）。确认后，清空旧全局点云，用 swap 高效替换为修正后的点云。
4.4 移植步骤参考
如果你也想在自己的环境中完成类似的移植，可以参考以下步骤：
-
依赖库安装
：安装PCL点云库、Eigen、OpenCV等必要依赖。
-
源代码修改
：将现在的pointcloudmapping.h和pointcloudmapping.cc添加到ORB-SLAM3的src/目录下。
-
CMakeLists配置
：在CMakeLists.txt中添加PCL库的链接，并设置相关路径。
-
编译与测试
：重新编译项目，使用RGB-D数据集（如TUM数据集）进行测试，验证稠密点云建图效果。
-
输出结果
：程序运行结束后，会生成.pcd格式的点云文件，可用PCL自带的pcl_viewer或CloudCompare等工具查看。
五、参考文献与引用链接
以下链接供你进一步了解和复核：
官方与权威资源
-
ORB-SLAM3 官方论文（arXiv）：https://arxiv.org/abs/2007.11898
-
ORB-SLAM3 官方 GitHub 仓库：https://github.com/UZ-SLAMLab/ORB_SLAM3
-
一文详解 ORB-SLAM3（系统架构）：https://cloud.tencent.com.cn/developer/article/1748339
高翔点云建图相关
-
高翔稠密点云项目（GitHub）：https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map
-
高翔稠密建图编译教程：https://www.e-com-net.com/archives/30268
-
ORB-SLAM2 构建点云详解：https://blog.csdn.net/qq_41839222/article/details/128388410
稠密建图改进相关
-
基于 ORB-SLAM3 的改进型特征匹配与稠密建图算法（知网）：https://wap.cnki.net/touch/web/Dissertation/Article/126810316104.html
-
基于增强型 ORB-SLAM3 算法的黄瓜植株稠密建图：https://www.j-csam.org/article/doi/10.6041/j.issn.1000-1298.2024.12.018