[发明专利]一种融合点线特征的视觉惯性里程计方法

权利要求书

1.一种融合点线特征的视觉惯性里程计方法，其特征在于，包括以下步骤：

步骤1：初始化阶段

根据原始的2D图像信息和IMU测量值，利用一个运动恢复结构例程使位姿和地图坐标信息充分收敛；随后利用视觉系统的精度来补偿惯导的精度，估计出惯导的偏置和重力向量，对惯导参数进行修正；最后得到一个小规模的3D地图；

步骤2：光流跟踪阶段

使用KLT光流跟踪像素中的特征点，并将匹配的点特征传输给后端；在光流跟踪中，根据两帧之间的IMU短时积分的结果预测上一帧对应的特征点在当前帧的投影点，以此为初始值参与光流计算；

步骤3：线特征跟踪阶段

利用线特征跟踪算法对线特征进行跟踪，包括基于前后帧运动信息的线段预测和基于预测结果的线段检测阶段，并将匹配的线特征传输给后端；考虑到跟踪算法本身引入的累计误差，在线特征跟踪发生退化或者跟踪一定时间以后，将重新提取和匹配线特征，利用新的线特征继续进行跟踪；

步骤4：后端估计阶段

将完成两帧之间的IMU短时积分提供给前端的光流跟踪阶段和线特征跟踪阶段；在接收到来自前端跟踪的匹配点特征对和匹配线特征对后，将视觉信息和IMU信息融合，求解系统运动状态的五元向量组和视觉观测的三维坐标，对初始化的3D地图进行更新；

在加入新的关键帧时，估计器将实施集束优化，降低系统的累积误差；在滑动窗口中的关键帧达到指定数目后，系统将实施边缘化，控制维护的最大状态数量，从而保持计算实时性。

2.根据权利要求1所述的融合点线特征的视觉惯性里程计方法，其特征在于，步骤1中在运动恢复结构例程中加入线特征信息，在所述重力向量的估计过程使用线特征来加快修正过程。

3.根据权利要求1所述的融合点线特征的视觉惯性里程计方法，其特征在于，步骤3中采用基于LSD算法的实时线特征跟踪算法，首先基于前后帧之间的运动信息对当前帧的线特征坐标进行预测，再利用预测坐标附近的图像像素信息对预测结果进行更新，生成最终的线特征提取和匹配结果。

4.根据权利要求1所述的融合点线特征的视觉惯性里程计方法，其特征在于，所述的步骤3具体为：

3.1：在跟踪开始前，首先对场景中的线特征进行三角化；在跟踪开始后，通过两帧之间的IMU短时积分获得当前帧的位姿信息；

线段投影预测：根据预测得到的位姿结果，将已三角化的三维线特征变换到当前帧获得相应坐标，计算三维线性特征在当前帧的二维线特征投影，根据三维线特征长度与二维线特征长度的比值，对二维线特征进行筛选，获得预测线段；

采样点光流跟踪：在前一帧的线段和当前帧投影预测的线段上进行均匀采样，获得若干采样点，将采样点投影到当前帧，并通过光流跟踪对两组采样点进行跟踪，修正预测线段上的采样点；

3.2：以预测后的采样点为中心，拓展线段支持域并生成直线段，对于所有采样点对应的多条线段，检测其与预测线段之间的方向一致性；

3.3：对于与预测线段方向一致性较好的线段，融合其方向及长度即为最终获得的线段检测结果。

5.根据权利要求4所述的融合点线特征的视觉惯性里程计方法，其特征在于，拓展线段支持域对应的二维线段，需要检测其与预测线段之间的夹角，舍弃夹角大于阈值的二维线段。

6.根据权利要求1所述的融合点线特征的视觉惯性里程计方法，其特征在于，根据点特征、线特征的重投影误差与IMU预积分误差项，构造联合损失函数，通过滑动窗口实现点特征、线特征和惯导信息的紧耦合融合。

7.根据权利要求1所述的融合点线特征的视觉惯性里程计方法，其特征在于，对于边缘化策略，当某一帧到达时，滑动窗口中的最新帧与该帧存在关联，则将根据滑动窗口中次新帧是否为关键帧决定边缘化的对象；当滑动窗口中的次新帧为关键帧时，将边缘化滑动窗口中最早的帧；当滑动窗口中的次新帧并非关键帧时，将舍弃次新帧，但是保留该帧的IMU预积分项。