ORB-SLAM3：从特征提取到稠密点云建图

本文通过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特征提取器让机器“看见”世界的关键ORBOriented 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 vectorint 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.ptruchar(v)[u*3];p.g imRGB.ptruchar(v)[u*31];p.b imRGB.ptruchar(v)[u*32];这里利用了相机内参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_transformPCL库默认的坐标系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 官方论文arXivhttps://arxiv.org/abs/2007.11898ORB-SLAM3 官方 GitHub 仓库https://github.com/UZ-SLAMLab/ORB_SLAM3一文详解 ORB-SLAM3系统架构https://cloud.tencent.com.cn/developer/article/1748339高翔点云建图相关高翔稠密点云项目GitHubhttps://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map高翔稠密建图编译教程https://www.e-com-net.com/archives/30268ORB-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