1.一种视觉惯性SLAM方法,其特征在于,包括如下步骤:步骤1:提取并跟踪图像上的特征点;
步骤2:对所述特征点的集合进行德劳内三角剖分,将其转换到世界坐标系,得到三角网格;
步骤3:提取有界平面并进行数据关联,包括:步骤31:重建单帧平面,基于所述三角网格的聚类结果得到所述有界平面的参数;
步骤32:利用所述有界平面的参数关联所述单帧平面;
步骤33:统计处于一个滑动窗口中的所有有界平面,并对这些有界平面进行面与面的数据关联;
步骤4:对滑动窗口内的平面进行特征点检查与合并;
步骤5:构建因子图模型,并在所述因子图模型中引入平面的几何约束,优化全局位姿;其中,利用惯性测量残差模型、视觉残差模型、先验信息残差模型以及点面融合残差模型构建所述因子图模型,利用整体优化方程求出其最小二乘解。
2.如权利要求1所述的视觉惯性SLAM方法,其特征在于,步骤1中,采用KLT光流算法进行特征点跟踪。
3.如权利要求1所述的视觉惯性SLAM方法,其特征在于,步骤2还包括:结合惯性测量单元获取初始值。
4.如权利要求3所述的视觉惯性SLAM方法,其特征在于,所述获取初始值的步骤包括:步骤21:通过检查最新一帧和所有先前帧之间的特征对应关系,找出能够实现稳定跟踪和足够视差的对应帧;
步骤22:恢复所述对应帧之间的相对旋转和非真实尺度平移;
步骤23:对所述对应帧中观察到的所有特征进行三角化;
步骤24:基于三角化的结果,估计滑动窗口中其他帧的姿态;
步骤25:最小化所有特征观测的总重投影误差。
5.如权利要求1所述的视觉惯性SLAM方法,其特征在于,步骤31中,对所述三角网格进行聚类得到所述聚类结果,聚类方法包括:检查畸形的三角网格,畸形的三角网格的边与角的关系满足如下条件中的至少一个:含有两个角度小于第一阈值的锐角;最长边与最短边的比值大于第二阈值;以及,至少有一条边的长度大于第三阈值;
水平面聚类;以及
垂直面聚类。
6.如权利要求1所述的视觉惯性SLAM方法,其特征在于,步骤5中,采用回环检测获得回环的候选帧,基于所述候选帧优化全局位姿。
7.如权利要求6所述的视觉惯性SLAM方法,其特征在于,采用回环检测获得候选帧的方法包括:从后端优化完成的关键帧中额外检测若干个角点;
将所述关键帧添加到关键帧数据库中,计算当前帧与词袋的相似度分数,并与关键帧数据库中所有帧进行对比;
进行回环一致性检测,获得回环的候选帧。
8.如权利要求7所述的视觉惯性SLAM方法,其特征在于,基于所述候选帧优化全局位姿的方法包括:当检测到闭环后,进行基于描述子的邻域匹配;
当匹配点数大于阈值后,对匹配点进行异常值剔除,并求解相对位姿;
当剔除后的匹配点仍超过阈值时,将该帧纳入优化。