Imu和视觉的融合部分,imu因为数据频率比视觉高很多,所以在融合的过程像是一把带有刻度的尺子(多个imu数据)去测量两个点(关键帧i、j)之间的距离。这需要imu数据的时间要能够包住视觉的时间。
如图所示,两个黄色的正元表示关键帧的数据,下面带有刻度的尺子表示imu的数据。由上图可以看出关键帧i和j在imu数据it和it+10内。所以想要计算ij之间的P,V,Q需要计算出it到it+10内对应的数据即可。因为i帧和j帧没有完全对应尺子上的整数刻度,也就表示视觉和imu的时间戳没有对应,所以在求积分的时候,首尾要做一次处理再去积分。
上图表示,关键帧i对应的时间戳在it和it+1之间,距离it的时间为a,距离it+1的时间为b。关键帧j对应的时间戳在it+9和it+10之间,距离it+9的时间为c,距离it+10的时间为d。
首帧处理:
假设关键帧正对的imu数据的时间戳为(it+x),现在需要计算it+x时刻imu的数据(a,w)。本次积分过程的时间为b,
尾帧处理:
这里基本和首帧处理相同,假设关键帧正对的imu数据的时间戳为(it+y)。
接下来是正常的预积分过程,以[it+x,it+1]为例。
在[it+x,it+y]之间,计算不同imu时刻的w和a时,减的ba,bg都是相同的。
计算出w之后转换成四元数形式: