本发明属于slam(simultaneouslocalizationandmapping，同步定位与建图)领域，具体涉及一种基于关键平面的视觉惯性里程计方法。
背景技术：
传统的基于网格提取平面的视觉惯性里程计(visual-inertialodometry，vio)方法对每一帧图像使用2d德劳内三角化形成2d网格，当特征点被三角化后生成3d网格，随后建立直方图，超过阈值的平面被筛选出当做面特征。将特征点和线分配到检测到的每个平面，并且在后端优化面特征和点线特征。
这种方法虽然考虑到了特征点线和检测到平面之间的限制，但是没有考虑到平面和平面之间的限制，例如平行和垂直。
技术实现要素：
针对现有vio技术中的不足与难题，本发明旨在提供一种基于关键平面的视觉惯性里程计方法，在后端中增加平面和平面的限制，提高定位精度。
本发明通过以下技术方案予以实现：
基于关键平面的视觉惯性里程计方法，该方法包括以下步骤：
步骤1、单目相机以一定频率采集图像；imu以一定频率采集惯性数据；
步骤2、对采集到的每帧图像，提取点线特征并且进行跟踪；
步骤3、imu预积分：对连续两帧图像之间的imu数据进行预积分；
步骤4、2d德劳内三角化；生成3d网格；检测平面；对每个检测到平面分配特征；判断是否是关键平面。
步骤5、紧耦合视觉惯性定位：根据残差解决最小二乘问题并得出最有估计，实现对目标载体的定位。
进一步地，步骤2具体如下：
步骤2.1、对图像提取特征点，并在后一帧中建立与这些特征的关系，形成跟踪；
步骤2.2、对图像提取特征线，并在后一帧中建立与这些特征的关系，形成跟踪。
进一步地，步骤3具体为：对任意两帧图像之间的imu数据，进行预积分，得到三个方向上的积分量；若imu的零偏改变，则可以直接用现有的预积分来更新。
进一步地，步骤4具体如下：
步骤4.1、2d德劳内三角化：对每一帧图像，根据提取到的点特征和线特征，进行2d德劳内三角化，形成的网格属于2d网格；
步骤4.2、生成3d的网格：对所有2d网格，如果网格中的三角形的三个顶点都被三角化，则计算三个顶点的3d坐标，并且将这个2d三角形变为3d三角面片，许多个三角面片组成3d网格；
步骤4.3、检测平面：对每个3d三角面片，收集他们顶点的三维坐标，随后建立直方图提取水平平面和垂直平面；
步骤4.4、对每个检测到平面分配特征，对每一个已经三角化的特征进行遍历，若它与任何一个平面距离较近，则把该特征分配给平面，将特征-平面残差添加到优化目标函数；
步骤4.5、判断平面是否为关键平面：每次优化后会再进行一次特征分配给平面的过程。若分配给某个平面的特征数量小于一定数值，那么它就被剔除；剔除的时候会进行判断，如果该平面连续一定数量的帧没有被剔除，并且剔除时的帧和被检测时候的帧的视差超过一定阈值，那么将其视为关键平面，将平面-关键平面残差添加到优化目标函数。
进一步地，步骤5中的紧耦合视觉惯性定位具体为：将边缘化先验信息，imu预积分残差，点和线重投影误差，特征-平面残差，平面-关键平面残差加入目标优化函数进行优化。
与现有技术相比，本发明有益效果包括：
(1)本发明所提出的方法用于视觉惯性定位，所述方法同步视觉与imu信息，提出了筛选关键平面的策略，通过引入关键平面，增加探测到平面之间的规律性，提高了定位精度。
(2)使用本发明提出的方法，相较于传统的仅仅基于特征点的视觉惯性定位系统，定位精度可提高约16％。
附图说明
图1是本发明基于关键平面视觉惯性里程计流程图；
图2是本发明3d网格建图示意图；
图3是本发明采取关键平面策略建立的光滑平面；
图4是没采取关键平面策略建立的粗糙平面；
图5是本发明2d德劳内三角化示意图。
具体实施方式
下面结合附图，对本发明作进一步地说明。
本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
如图1，一种基于关键平面的视觉惯性里程计方法，具体包括下述步骤：
步骤s1、通过双目相机采集图像，其分辨率为752*480，频率为20hz，实验中只采用左目图像；通过imu测量角速度和加速度，频率为100hz。
步骤s2、对每帧图像提取特征并且追踪这些特征；
s2a，对每帧图像提取角点，并且通过lucas光流法在下一帧图像确定角点的位置进行跟踪，筛选角点的时候用基于基础矩阵的ransac(randomsamplingconsistency，随机采样一致性)来剔除离群值；
s2b，对每帧图像提取线特征并追踪，使用lsd(linesegmentdetector，线段检测器)提取线特征，并在每个线特征提取lbd(linebinarydescriptor，线二进制描述子)描述子来确定在下一帧中的位置。
步骤s3、imu预积分：根据两帧图像之间的所有惯性数据做预积分，这里的预积分给估计器的imu残差使用：
imu测量模型如下：
式(1)中，at和ωt分别为t时刻imu坐标系下加速度真实值和角速度真实值，am和ωm分别为t时刻加速度与角速度测量值，
根据imu测量模型，计算预积分测量值：
其中：
式(2)、(3)中，
步骤s4、2d德劳内三角化；生成3d网格；检测平面；对每个检测到平面分配特征；判断是否是关键平面：
s4a，2d德劳内三角化：对每一帧图像，提取点特征和线特征后，采取2d限制德劳内三角化；图像中不仅含有点特征的信息还有线特征的信息，因此在构建2d网格的时候还有考虑图像中的线特征。
s4b，生成3d网格：首先将2d德劳内限制三角化后得到的网格的每个三角形剖分下来，对于每个三角形，如果求出三个顶点的位置，那么三角形的3d坐标也能够得到，经过对特征的三角化后，点线特征的3d位置有机会得到；如果某个三角形的三个顶点的三维坐标都已知，我们将该三角形的三维坐标求出，很多个三角面片组成一个3d网格；同时并不是每个3d网格都能被使用，本方法设置了两个条件，两个条件都满足时该三角面片才能被使用，条件(1)为三角面片中最小的角度不得超过十五度；条件(2)为三角面片至少有两个相邻的三角面片。
s4c，检测平面：这里只检测水平和垂直平面；对于水平平面，根据imu提供的重力加速度方向，找出法向量近似与重力加速度的三角面片，随后将这些三角面片的顶点高度提取出来，建立一维直方图，坐标为顶点高度，超过阈值的量被选为水平平面；对于垂直平面，根据imu提供的重力加速度方向，找出法向量近似与重力加速度垂直的三角面片，随后将这些三角面片的投影到xy平面，提取出轴角和离原点的距离，建立二维直方图，其中一维坐标为轴角，另一维是离远点的距离，超过阈值的被选为垂直平面。
s4d，对每个检测到的平面分配特征：对滑动窗口的所有的点特征和线特征遍历，若该特征到检测平面的距离低于阈值，则将该特征认定为该平面所属，同时将特征-平面残差添加进入估计器。
s4e，判断是否是关键平面：对每个面特征，我们实行剔除准则：若该平面拥有的点线特征数量低于阈值，则将该面特征删除；删除之后，实行关键帧筛选准侧，(1)该平面连续一定数量的帧没有被剔除；(2)剔除时的帧和被检测时候的帧的视差超过一定阈值，如果上述两个条件都满足，那么将其视为关键平面，将平面-关键平面残差添加到优化目标函数。
步骤s5、紧耦合视觉惯性定位：根据残差解决最小二乘问题并得出最有估计，实现对目标载体的定位；
对滑动窗口所有待估计的状态变量变量如下：
式(4)中，xk为imu坐标系k时刻的状态，包括该时刻imu坐标系到世界坐标系的平移、旋转、在世界坐标系下的速度以及零偏；λi表示第i个点特征在第一次被观察到时相机坐标系的逆深度；oi为第i个线特征在世界坐标系下的正交坐标，k表示从0到n的正整数；πi＝[ni，di]表示第i个平面的坐标，其中ni为该平面的法向量，di为该平面到原点的距离；
类似的，πki＝[nki，dki]表示第i个平面的坐标，其中nki为该平面的法向量，dki为该平面到原点的距离。
最小化以下函数求解状态量：
式(5)中，
对应的残差表示如下：
式(6)中，
式(7)中，
式(8)中，d(s，l)为点s到线l的距离，
式(9)中，λk为该特征点在第一帧观测zj中的逆深度，nj和dj分别为平面的法向量和离原点的距离。
关于平面-关键平面残差，可以分为两种情况：
(1)关键平面为水平平面
式(10)中，nj和nki表示平面与关键平面的法向量，dj和dki表示平面与关键平面到原点的距离。
(2)关键平面为垂直平面
式(11)中，nj和nki表示平面与关键平面的法向量，dj和dki表示平面与关键平面到原点的距离，g表示重力加速度。
本发明的软件具有四个线程：点特征提取与跟踪、线特征提取与跟踪，网格生成与平面提取，估计器优化；四个线程并行运行，实现实时地对相机和imu数据处理。
使用开源数据集euroc进行算法评估，该数据集在一个工厂和室内环境采集惯性和视觉数据，工厂和室内环境有丰富的面结构，以使发明的算法提高性能。其图像采集频率为20hz，imu采样频率为200hz，数据集提供真实轨迹。本实例使用的硬件处理为8g的intelcorei5-8300笔记本，软件平台为ros。本方法(表格中记为proposed)实验结果与基于特征点的视觉惯性里程计算法结果作比较。
图2为发明算法建立的网格地图，绿色的表示水平平面，如地板；红色代表垂直平面，如墙壁，地板和墙壁在空间中的分布符合实际规律互相垂直，这得益于本发明的关键平面算法。
图3为基于本发明关键平面算法得到的墙壁网格地图，对比图四表面更加光滑。
图4为基于仅点特征的视觉惯性里程计算法得到的墙壁网格地图，对比图四表面更加粗糙。
图5展示了2d德劳内三角化，红点代表点特征，蓝线段代表线特征，绿色网格代表经过限制德劳内三角化得到的2d网格。
最后给出了euroc序列在本方法和基于点特征两种方法的均方根定位误差(单位为厘米)定位结果，由表1可知本发明定位精度由于现有算法，定位平均精度提高了约16％。
表1
以上所述仅表达了本发明的优选实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形、改进及替代，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。
1.基于关键平面的视觉惯性里程计方法，其特征在于，所述方法包括以下步骤：
步骤1、单目相机以一定频率采集图像；imu以一定频率采集惯性数据；
步骤2、对采集到的每帧图像，提取点线特征并且进行跟踪；
步骤3、imu预积分：对连续两帧图像之间的imu数据进行预积分；
步骤4、2d德劳内三角化；生成3d网格；检测平面；对每个检测到平面分配特征；判断是否是关键平面；
步骤5、紧耦合视觉惯性定位：根据残差解决最小二乘问题并得出最有估计，实现对目标载体的定位。
2.根据权利要求1所述的基于关键平面的视觉惯性里程计方法，其特征在于，所述步骤2具体如下：
步骤2.1、对图像提取特征点，并在后一帧中建立与这些特征的关系，形成跟踪；
步骤2.2、对图像提取特征线，并在后一帧中建立与这些特征的关系，形成跟踪。
3.根据权利要求1所述的基于关键平面的视觉惯性里程计方法，其特征在于，所述步骤3具体为：对任意两帧图像之间的imu数据，进行预积分，得到三个方向上的积分量；若imu的零偏改变，则可以直接用现有的预积分来更新。
4.根据权利要求1所述的基于关键平面的视觉惯性里程计方法，其特征在于，所述步骤4具体如下：
步骤4.1、2d德劳内三角化：对每一帧图像，根据提取到的点特征和线特征，进行2d德劳内三角化，形成的网格属于2d网格；
步骤4.2、生成3d的网格：对所有2d网格，如果网格中的三角形的三个顶点都被三角化，则计算三个顶点的3d坐标，并且将这个2d三角形变为3d三角面片，许多个三角面片组成3d网格；
步骤4.3、检测平面：对每个3d三角面片，收集他们顶点的三维坐标，随后建立直方图提取水平平面和垂直平面；
步骤4.4、对每个检测到平面分配特征，对每一个已经三角化的特征进行遍历，若它与任何一个平面距离较近，则把该特征分配给平面，将特征-平面残差添加到优化目标函数；
步骤4.5、判断平面是否为关键平面：每次优化后会再进行一次特征分配给平面的过程；若分配给某个平面的特征数量小于一定数值，那么它就被剔除；剔除的时候会进行判断，如果该平面连续一定数量的帧没有被剔除，并且剔除时的帧和被检测时候的帧的视差超过一定阈值，那么将其视为关键平面，将平面-关键平面残差添加到优化目标函数。
5.根据权利要求1所述的基于关键平面的视觉惯性里程计方法，其特征在于，所述步骤5中的紧耦合视觉惯性定位具体为：将边缘化先验信息、imu预积分残差、点和线重投影误差、特征-平面残差、平面-关键平面残差加入目标优化函数进行优化。
技术总结