2.ORB-SLAM2改进版本--稠密建图版本解析

最新推荐文章于 2024-05-09 22:45:16 发布

APS2023

最新推荐文章于 2024-05-09 22:45:16 发布

阅读量1.8k

点赞数 1

分类专栏： ORB-SLAM2算法进阶篇文章标签：图像处理计算机视觉

本文链接：https://blog.csdn.net/qq_41694024/article/details/128958959

版权

本文介绍了ORB-SLAM2的稠密建图版本改进，包括增加点云相关文件、统计滤波与体素滤波在外点去除的应用、初始化点云线程配置、跟踪线程及闭环线程的修改。通过统计滤波去除离群点，体素滤波降采样以提高效率，实现更精确的全局点云地图。

摘要由CSDN通过智能技术生成

1.修改思路

1.在ORB_SLAM2上增加了几个文件: PointCloude.h，PointCloude.cpp，pointcloudmapping.h，pointcloudmapping.cc。里面定义了PointCloudMapping类。

初始化的时候进行参数配置
2.tracking线程中生成关键帧的时候生成点云图

3.地图中新增全局点云地图

4.闭环线程：全局BA后更新全局点云图

5.显示线程：显示更新的点云图

2.基本概念

2.1 统计滤波

        PCL中类StatisticalOutlierRemoval，统计分析进行滤除外点。
什么是外点、离群点？

        离群点对应的英文是outliers，也叫外点，就是明显偏离"群众"的点，比如我们用激光扫描一面平坦的墙壁，正常情况下得到的应该是差不多也位于同一个平面的点云，但是由于设备测量误差等原因，会产生少量脱离群众的空间点，离本来的墙壁过远，我们就叫这部分点为离群点。

为什么要移除离群点？

        因为离群点会使局部点云特征（如表面法线或曲率变化）的估计复杂化，导致错误的值
从而可能导致点云配准失败。而且这些离群点还会随着积累进行传导，造成重影等问题，必须去除。

统计滤波原理？

        对每个点的邻域进行统计分析，剔除不符合一定标准的邻域点。具体来说，对于每个点，我们计算它到所有相邻点的平均距离。假设得到的分布是高斯分布，我们可以计算出一个均值 $\mu$ 和一个标准差 $\sigma$ ，那么这个邻域点集中所有点与其邻域距离大于 $\mu +th * \sigma$ 区间之外的点都可以被视为离群点，并可从点云数据中去除。th 是标准差倍数的一个阈值，可以自己指定。
代码使用示例：
pcl::StatisticalOutlierRemoval<pc1::PointXYZ> sor; //创建滤波器对象
sor.setInputcloud(cloud);    //设置待滤波的点云
sor.setMeanK (50);            //设置在进行统计时考虑的临近点个致
sor.setstddevMu1Thresh (1.0);//没置判断是否为离群点的阀值，用来倍乘标准差，也就是上面th
sor.filter (*cloud_filtered);  //滤波结果存储到cloud_fi1tered
设置对每个点分析的临近点的个数设置为50，并将标准差的倍数设置为 th=1，这意味着如果一个点的距离超出了平均距离加上一个标准差以上，则该点被标记为离群点。并将它移除。

2.2 体素滤波

PCL的VoxelGrid类通过输入的点云数据创建一个三维体素栅格（可把体素栅格想象为微小的空间三维立方体的集合)，然后在每个体素（瞰三维立方体）内，用体素中所有点的重心来近似显示体素中其他点，这样该体素就内所有点就用一个重心点最终表示，相当于降采样。对于所有体素处理后得到过滤后的点云。
pc1::Pointcloud<pc1:: PointXYZ>::Ptr cloud_filter(new pc1::Pointcloud<pc1::PointxYz>);
pc1::voxelGrid<pc1::pointXYZ> sor;
sor.setInputcloud(cloud);
sor.setLeafsize(0.3f，0.3f，0.3f);//体素大小设置为30*30*30cm
sor.fi1ter(*cloud_filter) ;
代码中的滤波方法：统计滤波的输出作为体素滤波器的输入，通过滤波的点输出放入全局地图中。

3.修改代码：初始化点云线程配置

设置滤波参数：在yaml文件里

//体素滤波尺寸
PointcloudMapping.Resolution: 0.01
//kmeans的值
meank : 50
thresh: 2.0

system.cc中设置点云滤波的参数

//滤波参数
float resolution = fssettings["PointcloudMapping.Resolution"];
float meank = fssettings["meank"];
float thresh = fssettings["thresh"];

//Create Drawers. These are used by the Viewer
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

*****dense modify start *******
// 初始化PointcloudMapping实例,并用mppointcloudMapping指向它
mpPointCloudMapping = make_shared<PointCloudMapping>( resolution,meank,thresh );

//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this
constructor)
//将mppointcloudMapping指针类放入跟踪线程
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpPointCloudMapping, mpKeyFrameDatabase, strSettingsFile, mSensor