[发明专利]一种基于改进RGB-D SLAM的Octomap优化方法

权利要求书

1.一种基于改进RGB-D SLAM的Octomap优化方法，其特征在于，包括如下步骤：

1)构造最小误差函数优化RGB-D SLAM室内定位系统相机位姿，获得点云图：在RGB-DSLAM系统获取点云过程中，对相机位姿估计进行优化，依据图优化理论，通过3D-3D模型，在特征点匹配时建立最小误差模型，过程如下：相邻关键帧F_K和F_k+1，提取它们的特征点并匹配它们的描述符，得到两个特征点集S_k＝{s₁^k,s₂^k,...,s_n^k}和S_k+1＝{s₁^k+1,s₂^k+1,...,s_n^k+1}，结合给定图像深度的深度信息，得到3D特征点集X_k＝{x₁^k,x₂^k,...,x_n^k}和X_k+1＝{x₁^k+1,x₂^k+1,...,x_n^k+1}，相机内部参数为C，利用下述公式(1)可求得旋转矩阵R和平移向量t为：

其中d₁^k,d₁^k+1分别表示关键帧F_K和F_k+1对应特征点的深度；

构造每个点的最小误差函数为公式(2)：

整合得到全局优化函数公式(3):

其中n表示特征点索引，T表示矩阵转置，最后使用g2o框架对点云图进行优化，得到全局点云图；

2)使用K维树进行点云组织：使用K维树对点云进行组织，K维树使用垂直于每个坐标轴的平面来逐步穿过区域，每个点对应于二叉树的节点；

3)为点云设置K值：依据需要为点云设置K值；

4)计算平均K-最近领距离：K近邻的平均距离用公式(4)计算：

其中n为点索引，得出每个点的平均K最近邻距离d_n，再依据每个点的平均距离计算出全局平均距离μ和标准差σ，计算依据为公式(5)：

其中m为点索引；

5)定义异常值并从点云图中删除：假设平均距离μ和标准差σ服从高斯分布，平均距离在特定范围之外的点可以被定义为异常值并从点云图中删除，因此设置一个置信区间[c₁,c₂]为公式(6)：

其中α是比例因子，用于调整范围的大小，α的值取决于近邻分析的大小，在置信区间之外的点将被定义为奇异值并从点云图中删除；

6)建立Octomap地图：结合步骤4)和步骤5)调用Octomap库把点云地图转换为Octomap地图。