在IMU在slam系统中的应用(三)中我们推导了IMU的雅可比矩阵迭代公式和协方差矩阵的传递公式,那么,这两个迭代公式在耦合IMU的slam中的用处是什么呢?这里我们来推导一下他们的作用。
(一)雅可比矩阵迭代公式
之前已经推到过,雅可比矩阵迭代公式为:
因此,在预积分中,可以有另一种求法,预积分的公式可以写成(符号稍微与前文有点变化,实际含义不变):
对上式进行一阶泰勒展开(噪声是保持不变的)近似,雅可比矩阵可以根据上一个时刻递推而得:
(二)协方差矩阵的传递公式
协方差矩阵传递的作用是用于后端优化的IMU信息矩阵的迭代以及边缘化先验信息的误差函数。
(1)补充知识:H矩阵的稀疏结构以及边缘化操作
在非线性优化的高斯牛顿方法中,我们在迭代求解增量,是根据高斯牛顿方程来求解:
高斯牛顿方法中用作为牛顿法中的二阶
矩阵的近似求解增量方程。
考虑一个简单的观测模型:误差项描述相机以
姿态对路标点
进行观测的误差,那么整体的代价函数可以表示为:
其中,为相机观测的像素点,
表示路标
在位姿
时的投影模型。
考虑只涉及到了第
个相机位姿和第
个路标点,对其余部分的导数为
,把位姿变量放在误差项的前面,路标变量放在误差项的后头,所以,可以得到该误差项对应的雅可比矩阵:
和
表示
矩阵,对相机姿态的偏导维度为
,对路标点的偏导维度为
,该误差项对应的雅可比矩阵对
矩阵的贡献为
,因此
只在
,
,
,
存在非零块。
对于整体的矩阵,令
在所有的相机位姿中取值,
在所有的路标点中取值,那么,对矩阵按照位姿和路标分块,
可以表示为:
很容易可以看出和
对角阵,
和
稀疏还是稠密取决于具体的观测数据。这显示了
矩阵的稀疏结构,对线性方程的加速求解也利用了矩阵
的稀疏性。
现在将模型变复杂一点,考虑一个场景实例,假设一个场景内有两个相机位姿(、
)和六个路标点(
、
、
、
、
、
),对应的变量为
和
,因此很容易写出该场景下的误差函数为:
现在把所有变量按照位姿在前,路标点在后的顺序排列可得变量为:
因此可以得到对应的雅可比矩阵
为:
用带颜色方块将该矩阵可视化为下图(较宽的黑块表示较大的维度):
进一步可视化表示整体目标函数对应的雅可比矩阵:
所以相应的矩阵的系数情况也可以可视化出来,
矩阵中的非对角部分的非零矩阵块表示变量之间存在约束:
现在继续考虑更一般的情况,在实际的slam系统中,假设存在个相机位姿和
个路标点,相机位姿的数量远远小于路标点的数量,即
,所以对应的
矩阵左上角显得特别小,可视化以后如下图:
对于具有这种稀疏结构的,线性方程
的求解通常采用边缘化(
)的操作,也被称为
消元。
首先把阵分成四个区域,如下图,记为
,
,
,
。
所以对应的线性方程组可以变为如下形式:
由于和
是对角矩阵,对对角矩阵求逆是十分方便的,考虑到这个特性,对方程组高斯消元,目标是消去右上的非对角矩阵
,得到:
即处理后得到:
所以可以得到方程组:
该过程即消元,或者为slam中的边缘化,加速了线性方程中
的求解。
(2)VINS中的目标函数
VINS是视觉和IMU紧耦合的slam系统,使用滑动窗口的方法进行滤波和优化。在后端非线性优化中,优化的是包括滑动窗口在内的个相机的状态(包括位置,朝向,速度,加速度计的零偏和陀螺仪的零偏),相机到IMU的外参,以及
个
点的逆深度:
所以给出滑动窗口优化的目标函数(见VINS论文):
这个目标函数中的三个残差项即误差项分别为边缘化的先验信息,IMU测量残差和视觉的重投影残差,三种残差都是使用马氏距离表示,这里只讨论与IMU信息矩阵(协方差的逆)有关的前两个残差,第三个残差会另外写文章讨论。
由高斯牛顿法我们知道,以IMU测量残差为例子,当优化变量有一个微小增量时,对其进行一阶泰勒展开可以得到:
同视觉14讲中,展开目标函数的平方项,并对求导,令其为零,可以得到增量
:
同理,整体目标函数的增量方程可以写成:
这里为IMU预积分噪声项的协方差误差,
为观测的协方差误差,二者描述的是观测的可靠性,所以其绝对值没有意义,考虑的是相对性,进一步将上式简化:
(3)IMU测量残差
之前已经推导过IMU残差矩阵,这里直接相邻两帧的PVQ和bias变化量的差写出:
从全局目标函数优化变量可以看出,这里优化变量的是相邻两帧的IMU姿态:
接下来给出残差矩阵对于这些优化变量的雅可比矩阵(采用扰动的方式求解,推导过程后续补充):
另外,协方差由预积分中的协方差迭代公式得到,为:
到这里IMU残差部分的推导已经结束,之后根据高斯牛顿方法求解优化变量即可。
(4)边缘化的先验信息
重点:VINS中的边缘化和利用矩阵的稀疏结构边缘化虽然处理方法相同,但是意义不同。
利用矩阵的稀疏结构边缘化主要是在全局
中先消去路标点变量,先求解相机位姿,然后利用相机位姿来求解路标点变量,目的是为了加速求解速度。
而VINS中的边缘化操作的目的把即将剔出滑动窗口的帧中的路标点点和位姿给边缘化,这样使得求解过程中只出现滑动窗口里的帧的优化变量,但是又保留了该剔出帧对滑动窗口内其他帧的约束关系。(按我的理解是在求解过程中引入了历史信息,即引入了先验)。
边缘化的操作上文已经详细推导过,这里不在赘述,仅仅详细说一下为什么边缘化可以引入先验。考虑这么一个观测模型,有四个相机姿态和六个路标点
,相机和路标点的连线表示相机对路标点的观测,相机之间的连线表示IMU约束,用以下示意图表示:
我们可以画出关于上述观测模型的矩阵,即初始矩阵:
下面我们先把位姿给marg,marg的操作就是把跟
有关的部分移到左上角,见下图:
然后边缘化,边缘化的操作即把
高斯消元,这里
是在上方,与上文中描述的边缘化消元略有区别,上文中描述的消元是消去下方的变量,但是方法一样,可以自行推导公式,利用边缘化公式为
,得到:
上图中我们发现矩阵变稠密了,有三部分被fill-in,这时候图关系产生了新的约束,变为
同样的道理,marg掉之后的
为:
对应的图关系为:
这里marg掉并没有使得矩阵更稠密,原因是
并未与其他的姿态产生观测关系,即并没有被观测到,所以如果marg未被其他的相机姿态观测到的路标点,是不会使得
矩阵变得稠密。
待更 //补充一下VINS的边缘化策略,FEJ以及新测量信息和旧测量信息构建信息矩阵所存在的问题