1.一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于,具体包括如下步骤:
步骤1、分别从图像传感器和imu中采集图像和imu数据;
步骤2、图像和imu数据的鲁棒预处理;
步骤3、系统鲁棒初始化;
步骤4、启动位姿估计模块,持续不断地对系统位姿进行估计并向外输出系统最新的位姿;
步骤5、启动回环检测和位姿图优化模块,基于点特征和线特征描述子构建图像数据库,将到来的最新关键帧图像和数据库中的图像进行对比,如果相似度大于某个阈值,则认为出现了回环,利用检测到的回环信息消除最新关键帧的累积误差。
2.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤1中,所述图像传感器和imu集成在一起构成视觉惯性模组,并开放相关的api,通过调用api即可采集得到图像和imu数据。
3.根据权利要求2所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:在视觉惯性模组中集成一个晶振,图像和imu数据的时间戳均由同一个晶振产生,以此降低图像和imu数据之间的时延,保证图像和imu数据的时间戳在硬件上的同步。
4.根据权利要求3所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤2包括图像的鲁棒预处理和imu数据的鲁棒预处理,
其中,图像的鲁棒预处理包括如下步骤:
步骤2.1、检测imu数据中的加速度值,如果加速度值超过了一定的阈值,则认为发生了敏捷运动,在进一步处理之前需要使用维纳滤波对图像进行去模糊处理;如果加速值没有超过阈值,则无需进行去模糊的处理;
步骤2.2、对图像调用lsd线段提取算法,提取图像中所包含的线段;提取完线段之后,再在图中非线段处提取特征点;
imu数据的鲁棒预处理包括如下步骤:
步骤2.3、确定imu数据为载体加速度值和角速度值,根据图像和imu数据所带的时间戳对图像和imu数据进行时间戳对齐;
步骤2.4、imu数据预积分,确定tk时刻和tk+1时刻内,位移、速度和方向的增量,分别为
其中:
对步骤2.4当中的连续型积分公式,采用中值积分进行离散化处理,对于第i+1个imu数据ai+1,ωi+1,中值积分的公式如下:
其中
将[tktk+1]时刻内的所有imu数据都按上述公式进行处理后,可以得到时刻tk和tk+1之间的位移、速度和方向的增量
5.根据权利要求4所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:为了后续的imu残差项的构建,还需要对imu预积分量
6.根据权利要求5所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:利用公式
pk+1=fpkft+vqvt(6)
进行预积分量协方差的递推,其中pk是tk时刻的协方差矩阵,f为误差项对状态增量的一阶导,v为误差项对噪声项的一阶导而q为噪声项的协方差矩阵,这样,最新时刻的状态增量的协方差矩阵pk+1得到更新和维护。
7.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤3包括求解滑窗内所有帧的位姿、所有3d点的位置、所有线段的相关参数、重力方向、尺度因子、陀螺仪的bias以及每一图像帧所对应的速度。
8.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤4包括如下步骤:在位姿估计模块中,系统维护的核心数据结构是大小为10的滑动窗口;对于滑动窗口中包含的每一个关键帧都含有一个总的带估计变量x,变量x包含了所有需要估计的具体变量,具体地有
根据滑窗中所维护的数据结构,构建imu残差项、特征点残差项、线特征残差项以及滑动窗口滑动时边缘化所产生的先验项;在构建完所有的残差项之后,将这些残差项一起构成联合优化函数,通过最小化联合优化函数得到状态的最优估计。
9.根据权利要求8所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:具体地步骤如下:
构建imu残差项:
根据滑动窗口中两个图像帧之间的预积分量,构建imu残差函数如下:
特征点残差项:假设第l个特征点首次在第i个图像帧中被观测到,转换到当前的第j个图像帧所对应的相机坐标系下,则定义视觉重投影误差为:
其中,
线特征残差项与特征点构成重投影残差类似,每一条空间直线可以构成如下的重投影误差函数:
其中,elk,j=d(zk,j,khcwlw,j)表示空间直线的重投影误差,lw,j为空间直线的普吕克坐标,ρ为鲁棒性核函数;
当滑动窗口开始滑动时,对滑窗中的状态进行边缘化,由边缘化可以产生先验残差;
通过联合前面几个残差函数构成联合优化项,对于该联合优化项进行优化,随后,将系统最新的位姿向外输出;系统都会往外输出一个最新位姿,如此不断往复,持续对系统位姿进行估计并输出。
10.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤5包括如下步骤:
步骤5.1、使用大量的特征描述子进行训练,通常采用离线的方式构建视觉词典;
步骤5.2、利用kmeans++算法进行聚类,在进行训练之前指定词典数的深度l和每层的节点数k;
步骤5.3、在建立好视觉词典后,对输入图像和历史图像之间的相似性进行对比。