vins-mono应用于单目加IMU的情况下。
首先是feature_tracker,对图像中的点进行追踪,这里使用的是光流法追踪内部的特征点,查看其中与上一帧匹配的特征点,然后记录跟踪到的次数,假如跟踪次数到达一定程度就会将该点固定,形成约束。清除特征点附近过密的特征点(根据特征点跟踪次序进行清除,从而优先保留次数较多的特征点),还要清除上一帧中在这帧中没有出现的特征点,然后提取新一帧中的一些新的特征点(新特征点的提取位置可以根据清除的旧特征点的位置进行动态的判断)。然后将特征点发布出去。
关于IMU预积分,这里进行预积分的原因是希望即使上一帧的参数被优化后出现了迭代的情况,不需要重新进行积分,此时可以将两边同时乘以,而偏置的影响可以通过泰勒展开的方式加以解决。接下来是对误差的分析,关于误差的求导以及计算,这方面有相应的公式以及推导。然后可以对相关误差进行矫正。
IMU数据以及特征点的图像数据显然是需要对齐的,也就是说需要将IMU消息与图像消息根据时间戳进行匹配,也就是一帧图像对应多个IMU,而IMU留下以用于积分。然后将需要重定位的帧进行设置保存。同时根据Process进程所留下的PVQ对其位置信息进行更新。还要判断图像是否为关键帧,假如是关键帧就需要marg滑窗最老帧,不是关键帧就marg滑窗上一帧。
接下来是初始化,作为单目相机,需要IMU来估算相关的深度尺度。而IMU的线速度以及角速度偏置也需要通过相机来消除。首先是对相机与IMU之间旋转外参的标定,我其实觉得这是固定的,但初始化算法里给出了相应的解决方法,那就是通过旋转一下,根据相机与IMU之间的差值计算相应的外参。接下来确定特征点与初始帧之间的位置关系,可以通过PNP以及三角化测量进行计算得到点的估计位置。然后通过相机旋转得到IMU的速度偏置,通过IMU平移优化初始帧的重力,速度并得到视觉尺度,然后将初始帧的重力与世界坐标系下的融合即可得到世界坐标系。现在详细记录一下其中的过程,首先对滑窗内所有加速度的标准差进行求解,从而得到其中的视差,确保有足以能够判断初始值的运动量,求出第一个与最新帧有足够大视差的帧,然后根据位姿的估计值计算出所有帧对该帧的旋转与平移。然后三角化该帧与最新帧的共视点,接着反过来根据pnp求滑窗内前后帧之间的相对运动,并且利用pnp求取相关点的坐标。同时去掉无3D坐标的点。假如符合要求的点小于15个就判定为优化失败,此时可以使用ceres进行优化。然后利用pnp方法求取所有帧的相对位姿,利用上文所述的方法计算重力,偏置以及其他变量。最后将重力转回z轴,得到世界坐标系与摄像坐标系之间的关系,然后将所有变量转换到世界坐标系下,至此初始化完成。
|