vins-mono(1)数据预处理

一.视觉追踪

       每当采集到一帧图像,都会进行KLT稀疏光流追踪,同时保证每帧图像提取的Harris角点数目在100-300个,将跟踪到的特征点push到图像队列中.

主要实现在readImage函数中:

1.对原始图像进行预处理,如果图像过亮或太暗,则使用自适应直方图均衡化的方法增强图像对比度,img为当前要处理的图像

2.prev_img为上一帧图像,cur_img为光流追踪的前一帧,forw_img为光流追踪的下一帧,这三个量不断更新.

采用KLT稀疏光流追踪cur_img的特征点,得到特征点在forw_img中出现的位置,如果追踪到的点不在边界内,将它剔除

(光流是指图像表现运动的速度,它的三个假设条件为它的工作原理:①灰度不变假设②某一个窗口的运动有相同的运动③时间连续)

3.对追踪到的点计算F矩阵,采用RANSAC算法剔除外点

4.将特征点按照追踪到的次数进行排序,如果mask为255则保留(??)设置该特征点的圆域,在此范围内不再提取特征点,避免特征点分布不均匀—这里不是很理解

5.再次提取一定数量的特征点

6.添加新的特征点,更新prev_img,cur_img等量

(这里跟踪的特征点给后端优化用)

二.IMU预积分

公式推导
IMU预积分的目的:计算出IMU数据的观测值(即IMU预积分值)+残差的协方差矩阵+残差的雅克比矩阵

Background:

1.将IMU数据进行积分,得到当前时刻的位置,速度和旋转(PVQ),同时计算在后端优化中将用到的预积分增量,预积分误差的Jacobian矩阵和协方差矩阵.

2.相机的采样频率明显低于IMU的采样频率,相机一般在几十HZ,而IMU可以达到几百HZ,而在slam中我们通常是求解每个图像时刻的pose,因此对两个图像之间的IMU观测值进行积分,可以得到i帧和j帧之间的位姿和速度关系.

3.当前时刻的PVQ积分出每帧的位姿,为视觉提供初始值

                    

Tip:

a.观测值

        IMU中的原始数据包含角速度和加速度两部分,它们都是基于IMU坐标系的.在测量值中,角速度会受到bias和噪声的影响,加速度除了受到bias和噪声的影响外,还会受到重力的影响.

                                
                                   
                                  ;
                                     

通过原始测量值一段时间的积分就可以得到当前时刻的PVQ:
                

可以看到状态估计是想估计任意时刻在全局坐标系下的位置和姿态,PVQ,即当前时刻的状态可以通过上一时刻状态来迭代求得,但是这里有个qwt是t时刻在world系下的旋转,是一个要估计的量,在后面的估计过程中,这个量会被不断更新改变,如果直接积分,则每变化一次就需要重新积分,为了减少计算量采用预积分的方法.
                

预积分的思想就是:把参考坐标系从世界坐标系转换到局部坐标系bk下,也就是积分区间的第一帧.可以通过左乘旋转矩阵qbkw实现,此时的积分结果可以理解为bk+1对bk之间的相对运动量,bk的状态改变并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递.预积分量只和IMU测量值有关,它将一段时间内的IMU数据直接积分起来就得到了预积分量.

得到的预积分量就是IMU数据的观测值.实际中采样是离散的形式,所以我们将它进行欧拉离散化:由于高斯白噪声未知,将它设置为0,ai=aj,wi=wj:
                

完成了IMU预积分测量值的推导,要将IMU预积分运用到非线性优化中,需要求解协方差矩阵和误差对状态量的雅克比矩阵.

b.协方差矩阵:需要建立线性高斯误差状态递推方程,由线性高斯系统的协方差推导方程的协方差矩阵.

描述观测值的不确定性.

采用基于误差随时间变化的递推方程:

如果能推导出状态误差随时间变化的导数关系如下:

                                                

则误差状态的传递方程为:

                              

而VIO系统中已经知道了状态的导数与状态之间的转移矩阵,如:
                                                                 

那么就可以推导速度的误差与状态误差之间的关系.

得到了非线性系统的线性化递推方程,可以根据当前时刻的值预测下一时刻的协方差:

                                                   

                                                   

<根据连续形式的增量递推方程,我们可以推导出离散形式的递推方程>

同时可以得到一阶Jacobian矩阵的传递方程:

                                      

这个J的意义是:我们在计算预积分量时是假设偏置ba,bw已经确定的情况下,实际上偏置也是需要优化的变量,那么在每次迭代时,bias发生变化就需要根据公式重新求所有帧之间的积分,当偏置变化很小时,可以用预积分量在i时刻的bias附近用一阶泰勒展开来调整,否则进行重新传递.一阶近似更新如下:

                                      

这个雅克比是用来计算预积分值的.

c.在后端优化中,还需要用到误差对状态量的雅克比矩阵:

PVQ积分公式

一段时间内IMU构建的预积分量作为测量值,对两时刻之间的状态量进行约束

此时的优化变量是:k时刻的PVQ和k+1时刻的PVQ,以及两个时刻的bias