紧耦合单目VIO:

初始化之后,执行一个基于滑窗的紧耦合单目VIO,得到精度更高鲁棒性更好的状态估计值.

滑窗中的状态向量定义为:

xk是某一时刻捕获第k帧图像时IMU的状态.共包括滑动窗口内的n+1个所有相机的状态,相机和IMU的外参,m+1个3d点的逆深度.

采用BA优化,它的目标函数是–建立视觉惯性的BA优化模型:

其中三个残差项分别是边缘化的先验信息,IMU测量残差,视觉的重投影残差.

a.IMU测量残差

优化变量为:

b.视觉测量残差

一般的针孔相机模型将重投影误差定义在图像平面上,而论文中介绍了将相机测量残差定义在单位球面上,使得它能适用于更多的相机类型,如广角相机,鱼眼相机.

b1.针孔相机

残差定义为一个特征点在归一化相机坐标系下的估计值与观测值的差:
                                                                        

其中某个路标点在第i帧中第一次观测到,其转换到第j帧的坐标为(估计值):

因为要优化的状态量是IMU坐标系的位姿,因此需要加上从相机到IMU的外参.

将方程拆成三维坐标的形式:

要优化的变量为:
                                                

因此Jacobian为:

b2.一般相机模型
                                                
                                        

测量值和估计值都处理为单位向量,所以视觉残差的*度为2,再将视觉残差投影到正切平面上.b1和b2是两个正切平面的正交基向量.(其实这里不太明白:为什么单位化要用到相机内参,这个b1和b2的取值算法1不太明白)

c.marginalization

功能: 边缘化策略是边缘化掉滑动窗口中的帧,不再计算这一帧的位姿和路标点,但是保留该帧对窗口内其他帧的约束.

目的: 是为了限制优化的复杂度,同时不丢失信息保证求解精度.滑窗限制了优化pose的个数,但是当滑窗移动有新的帧进入,丢弃旧的帧时,如果直接丢弃会导致有些约束被丢掉,降低求解的精度,所以在移除位姿或特征时,将与被边缘化掉的帧相关联的约束转化为先验信息.(①控制滑窗大小,从而控制运算量②让滑窗中的帧有足够的视差量③尽可能使得矩阵稀疏,减小运算量)

实现:

①如果第二latest frame是一个关键帧,则将oldest frame边缘化,移出滑动窗口;

②如果第二latest frame不是一个关键帧,则丢弃该帧的视觉测量边,但是保留IMU预积分值,不进行边缘化.因为认为次新帧和最新帧很相似,直接丢弃视觉信息不会造成整个约束关系丢失过多,但是保留IMU预积分的信息,保证预积分的连贯性.