[发明专利]一种基于g2o与随机蕨类算法的稠密地图创建方法

权利要求书

1.一种基于g2o与随机蕨类算法的稠密地图创建方法，其特征在于，包括以下步骤：

(1)使用g2o构建模型对模型配准算法

具体包括以下4个步骤：

使用g2o构建模型对模型配准算法步骤一，使用g2o构造代价函数，使用李代数对相机位姿和路标点进行转换，观测误差e为：

e＝z-h(ξ,p) (1)

其中z为相机观测数据，ξ，p为李代数表示的相机参数与位姿，函数h为观测方程；把关键状态的观测量加入，得到代价函数为：

其中m，n为对应矩阵的行坐标与列坐标，i，j分别为由1到m和n的数值，e_ij，z_ij，p_ij为观测误差，相机观测数据与位姿矩阵中对应坐标的变量；

使用g2o构建模型对模型配准算法步骤二，使用g2o进行回环检测并优化时，对邻近的关键帧和历史数据集中的关键帧进行匹配优化校正全局地图；其中关键帧之间的匹配方式为ICP匹配，将图像上的点根据相机位姿投影在2D平面上的对应点，通过对应点的欧氏距离，法向量夹角计算和优化相机位姿矩阵；

使用g2o构建模型对模型配准算法步骤三，根据使用g2o构建模型对模型配准算法步骤一的代价函数和使用g2o构建模型对模型配准算法步骤二的位姿矩阵获取方法，使用g2o构造回环模型；选定4到7个帧为一个数据模型，建立模型与模型之间的约束关系，并在模型与模型匹配时加入随机的局部回环和大回环配准，保证全局SLAM的建立；

使用g2o构建模型对模型配准算法步骤四，由使用g2o构建模型对模型配准算法步骤三构造的模型，设计模型与模型之间的匹配迭代算法如下所示,f(x_k)为模型与模型间的匹配函数，J(x_k)为f(x_k)的雅可比矩阵，x_k为相机参数和相机坐标组成的向量，其中λ为拉格朗日乘子，“Δxk”表示相机参数和相机坐标组成的向量，在k时刻到k+1时刻中变化的向量；将算法转化为无约束优化问题，μ为Δx增量半径，D为非负数对角阵，变量ρ的由公式(4)得到，MaxTHρ为变量ρ近似可行的阈值，设定为0.9；

在迭代过程中，如果ρ3/4那么μ←2μ；如果ρ1/4那么μ←0.5μ；如果ρMaxTHρ那么x_k+1←x_k+Δx_k；不断迭代直到f(x_k)收敛时，x_k的最优值；

(2)使用随机蕨类进行识别曾到达区域

使用随机蕨类进行识别曾到达区域的步骤一，使用如公式(5)所示的方式，编码化关键帧；每幅图像Q的编码由a个block组成，每个block由b个Ferns组成，k，l分别为由1到a和b的数值；

使用随机蕨类进行识别曾到达区域的步骤二，每个Ferns的编码值，是由通道c上像素点u的灰度值θ来确定；如公式(6)所示，对关键帧进行编码化处理，其中对于一个新获取的关键帧Q，分别比较归一化的I(θ)与阈值τ来计算每一个block里的Ferns列表中的值；阈值τ设定为0.45；

使用随机蕨类进行识别曾到达区域的步骤三，当新获取一个关键帧Q时，对Q进行使用随机蕨类进行识别曾到达区域的使用随机蕨类进行识别曾到达区域的步骤二同样的编码化处理，并比对临近关键帧和历史关键帧；通过对每一个Ferns的比对，计算两帧之间的相似度；

使用随机蕨类进行识别曾到达区域的步骤四，对于图像上的像素点进行如公式(6)的判断，若相同则相似度加1，通过相似度判别，决定当前关键帧是否加入回环，或者存在回环，如果是则认为当前关键帧相似度大，通过和相似度大的图像配准，进行重定位。