VINS后端非线性优化目标函数
1. 状态变量
vins在后端优化中,使用了滑动窗口,其状态向量包含窗口内的n+1个相机的状态(位置,旋转,速度,加速度计bias及陀螺仪bias)、相机到imu的外参、m+1个路标点的逆深度:
2.代价函数
我们建立后端需要优化的代价函数:
代价函数中的3个残差项分别对应边缘化先验信息,IMU残差,视觉重投影残差,需要注意的是,三种残差都是使用马氏距离进行表示的(相比欧式距离,多了协方差矩阵)。
我们将IMU残差使用最小二乘法进行求解,当优化变量产生增量后,则有以下公式:
其中
为相应Jacobian表达式,我们使用马氏距离公式对上式进行展开,并且求导,令导数为0,则可以得到增量的表达式为:
由此递推,我们可以得到代价函数的展开式:
其中
为IMU预积分的协方差矩阵,
为视觉观测的协方差矩阵。当IMU的协方差矩阵越大时,其逆越小,说明此时IMU的数据越来越不可靠,我们应该相信视觉的数据。
我们将上市简化,可以得到后端优化的增量方程:
其中,左侧全部为Hessian矩阵。
2.1 IMU残差
使用估计值减去测量值,我们可以得到IMU的残差表达式:
上述各增量对bias的Jacobian可从IMU预积分的大Jacobian公式相应位置获取。
我们对下面的优化变量开始求导:
Jacobian具体推导过程比较简单,全部都是导数的基本性质,只有在对旋转求导时较为麻烦,需要通过导数的定义,四元数的定义,左右乘的性质进行推导:
2.2 视觉残差
视觉主要是利用重投影,建立残差,我们直接给出视觉残差公式,推导公式比较简单,利用前后两帧的坐标系关系即可建立等式,不再赘述:
我们对以下变量进行求导:
Jacobian具体推导过程不再给出,主要利用了四元数的右乘扰动性质,并且忽略高阶导数。具体表达式如下:
视觉协方差公式:
2.3 边缘化和舒尔补公式
VINS中使用的边缘化为传统的边缘化策略,当有新的帧进来的时候,我们希望删除最老的帧或者次新帧,不希望对这一帧的位姿及路标点再次进行计算,减少计算量,但我们不能直接删除,否则会破坏约束关系,导致求解崩溃,因此我们通过舒尔补公式,保留需要marg那一帧的约束关系。我们通过公式进行说明,将非线性优化公式
改写为:
其中,
与
分别为我们需要merg掉的变量与需要保留的变量,使用舒尔补进行消元:
其中,
就是
在
中的舒尔补项,我们将上式展开得:
至此,后端非线性优化的代价函数就全部介绍完成,相应的求导,即Jacobian矩阵也全部求解完成,剩下的就需要合理的非线性优化算法根据求得的Jacobian对代价函数进行求解了。
3.边缘化例子
这里使用一个实例对边缘化进行说明,并且从图表中可以清楚的看到矩阵的变化情况,稠密或稀疏。我从网上找了一张示意图,读者请务必理解下图的变化过程。
假设有4个相机位姿,其看到了6个路标点,即特征点,相机与路标点之间表示观测,相机与相机之间表示IMU约束:
下图为上图的详细表示,每一个方块都代表对应的舒尔补项:
然后我们现在marg掉
,则上图变化如下:
我们可以看到,与
相关的都从矩阵中移除了,到了左上方,留下的矩阵即
,我们计算
(黄色),然后得到
的示意图:
我们将上图转成关系图后,如下,可以发现,约束关系变了。
在此基础上,我们merg掉
后的矩阵如下:
对应的关系为:
从这里我们可以看出,如果不想让矩阵变得稠密,只需要merg掉那些不被观测的路标点即可。