接着上篇：从零手写VIO——（三）基于优化的 IMU 与视觉信息融合（上篇）   再次说明一下，本博客目前只是我在看深蓝时记录的，当然会有很多 PPT 上的内容，加上一点自己学的时候的理解，或者补全没有写全的推导，目的是能够留下一个电子形式的资料，直接目的是留给自己之后反复查看。每次视频我都是一点点啃下去的，属实对我这种本科生并不友好（本科大牛请绕路），所有内容都是一点点用 LaTeX 打上去的，不是截图搬运。也感谢贺博、高博及深蓝的精美课程！

VIO 残差函数的构建

带权重的残差计算
(18)
其中， [公式] 服从高斯分布，协方差为 [公式] 。
  1. 协方差 [公式] 用来 归一化，多传感器融合 [公式] 单位不统一，如：PnP的误差方程的单位是像素，即实际像素点和理论像素点之间的误差；IMU 的误差单位为米( [公式] )和 度( [公式] )。
  2. 权重。协方差越小代表数据越集中， [公式] 就越大，代表权重越高。如说残差方程相差大，协方差大，说明可能存在误匹配 - 坏值，那么 [公式] 较小权重低。
基于滑动窗口的 VIO Bundle Adjustment
(19)
高博的视觉SLAM十四讲里，对滑动窗口理论没有做过多的讲述，从上式(19) [公式] 是先验，是后入数据时扔掉的前数据转化成的part， [公式] 和 [公式] 是新窗口内数据的残差方程， [公式] 是上篇提到的鲁棒核函数。 定义优化的系统状态量：
(20)
  1. [公式] 是 [公式] 时刻 IMU 在惯性坐标系中的位置，姿态，速度，IMU 机体坐标系下加速度和角速度的偏置量估计。 [公式] 是 [公式] 时刻的观测点。
  2. [公式] 分别是滑动窗口里机体状态量和观测路标的起始时刻。N 代表状态量在窗口内的关键帧数，M 是窗口内所有关键帧观测到的路标点数。
视觉重投影误差
一个特征点在 归一化相机坐标系 下的估计值和观测值的差。
(21)
其中，待估计的状态量为特征点的三维空间坐标 [公式] ，观测值 [公式] 为特征在相机归一化平面的坐标。
逆深度参数化 Inverse Depth Parametrization
特征点在 归一化相机坐标系 与 相机坐标系 下的坐标关系为：
(22)
其中 [公式] 称为 逆深度[1]。逆深度更容易表示无穷远处的点，当 [公式] ，可以用来表示无穷远处；且 [公式] 更接近正态分布。在实际应用中，逆深度也具有更好的数值稳定性。     特征点逆深度在第 [公式] 帧中初始化得到，在第 [公式] 帧又被观测到，预测其在第 [公式] 中的坐标为：
(23)
这里的变换矩阵其实就是先从一帧的相机坐标系 [公式] 这帧的 IMU body 坐标系 [公式] World 坐标系 [公式] 另一帧的 body 坐标系 [公式] 另一帧的相机坐标系。第一帧的 u-v 是可以通过亚像素的方法配准的，故用 [公式] 这一个参数表示第一帧的观测。 视觉重投影误差为：
(24)
IMU 测量值积分
IMU 的真实值 [公式] ，测量值为 [公式] ，则有：
(25)
(26)
上标 [公式] 表示 gyroscope， [公式] 表示 Accelerometer， [公式] 表示世界坐标系 world， [公式] 表示imu 机体坐标系 body。 PVQ 对时间的导数可写成：
(27)
从第 [公式] 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ：
(28)
目标→解决：每次 [公式] 优化更新，都需要对之后时间线上的重新积分。
IMU 预积分[2]
积分模型到预积分模型：
(29)
那么，PVQ 积分公式中的积分项则变成相对于第 [公式] 时刻的姿态，而不是相对于世界坐标系的姿态：
(30)
预积分量
预积分量仅仅跟 IMU 测量值有关，它将一段时间内的 IMU 数据直接积分起来就得到了 预积分量：
(31)
PVQ 的积分公式：
(32)
认为短时间内的 bias 是不变的。 预积分误差：一段时间内 IMU 构建的预计分量做为测量值，对两时刻之间的状态量进行约束。
(33)
[公式] 表示只取四元数的虚部 [公式] 组成的三维向量， [公式] 是预积分估计值，四元数 [公式] 的结果是 [公式] 即
代表转角 [公式] 接近0，这里只取出 xyz 就是取出旋转角的残差乘以2得到 [公式] 。等号左边每项都是一个三维度的共十五维，除四元数外，其他项均为 j 处世界坐标系下的真值减去 预积分以外的估计值（通过 [公式] 从世界坐标系转换到 i 坐标系），再在 i 坐标系下与预积分值相减得到预积分误差。
预积分的离散形式
这里使用中值法，即两个相邻时刻 k 到 k+1 的位姿用两个时刻的测量值 [公式] 的平均值计算：
(34)
测量数据 [公式] 都减去了 bias 可以用过标定获得，bias 服从随机游走，故有了后两个式子。随后的目的是 如何求算离散形式的多个 IMU 数据积分形成的累计预积分量的协方差。
协方差的传递 Covariance Propagation
已知 [公式] ，则有 [公式] ，证明其实在之前提过：
假设已知相邻时刻误差的线性传递方程：
(35)
比如：状态量误差为 [公式] ，测量噪声为 [公式] 。 误差的传递由两部分组成：当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一刻。 协方差矩阵可以通过递推得到：
(36)
其中， [公式] 是测量噪声的协方差矩阵，方差从 i 时刻开始递推， [公式] ；Allan 方差计算出了测量噪声的方差量级。
状态误差线性递推公式的推导
  1. 基于一阶泰勒展开的误差递推方程。
  2. 基于误差随时间变化的递推方程。
基于一阶泰勒展开的误差递推方程
令状态量为 [公式] ，其中，真值为 [公式] ，误差为 [公式] 。另外，输入量 [公式] 的噪声为 [公式] 。 非线性系统 [公式] 的状态误差的线性递推关系如下：
(37)
其中， [公式] 是状态量 [公式] 对状态量 [公式] 的雅可比矩阵， [公式] 是状态量 [公式] 对输入量 [公式] 的雅可比矩阵。 对非线性状态方程进行一阶泰勒展开有：
(38)
基于误差随时间变化的递推方程
如果能够推导状态误差随时间变化的导数关系，如：
(39)
则误差状态的传递方程为：
(40)
对比两个方法有：
(41)
为什么会有误差随时间变化这种方法?
因为 VIO 系统中已知状态的导数和状态之间的转移矩阵。比如说速度的导数与状态量之间的关系：
(42)
那么就可以推导速度的误差和状态误差之间的关系，再每一项上都加上各自的误差就有：
(43)
预积分的误差递推公式推导
白噪声的值不能像 bias 一样估计，故白噪声不能像 bias 一样减去。 下面误差的 Jacobian 是，在递推公式（ [公式] 用了中值法）等式左右分别对偏导分子分母加上一个误差 [公式] 后求出该 Jacobian。对于 [公式] 是用 [公式] 。
这个递推公式，可以理解为 k 时刻的该项误差，是由 k-1 时刻的误差及高斯白噪声（不像 bias 可以估计出来减去）递推得出的，但是为了简化，使用了对各项的一阶泰勒展开近似，下面↓来求这里的 F 和 G。
用前面一阶泰勒展开的推导方式，我们希望推导出误差的递推公式：
(44)
[公式] 为两个时刻间的协方差传递矩阵， [公式] 表示各时刻的误差。 [公式]。
(45)
(46)
其中系数为：
(47)
对部分项做详细推导↓对于导数中与变量无关的项省去：
[公式] 对各状态量的雅可比推导， [公式] 第三行
速度预计分量 [公式] 的递推计算形式：
(48)
 
[公式] ：对上一时刻速度预积分量的 Jacobian
(49)
[公式] ：对角度预积分量的 Jacobian
(50)
(51)
Part 1 对应的雅可比为：
(52)
Part 2 对应的雅可比为：
(53)
合起来就是(47)中的 [公式] 。
[公式] ：速度预积分量对 k 时刻角速度 bias 的 Jacobian
就有了这样的雅可比推导过程：
(54)
旋转预计分量的 Jacobian， [公式] 第二行
旋转预积分递推公式：
[公式] ：前一时刻的旋转误差 [公式] 如何影响当前旋转误差 [公式]
假设两个时刻的真值是 [公式] ，不考虑受到角速度 bias 的影响，两时刻的增量
三元组四元数相乘有如下性质（其实就是伴随性质，在李群李代数证明过）：
(56)
其中 [公式] 是和 [公式] 对应的旋转矩阵， [公式] 为 [公式] 的实部， [公式] 为 [公式] 的虚部。
(57)
(58)
故第 k+1 时刻的旋转预积分的误差相对于第 k 时刻的 Jacobian 为：
(59)

残差 Jacobian 的推导

归一化平面视觉残差为：
对于第 i 帧中的特征点，它投影到第 j 帧相机坐标系下的值为：
写成三维坐标形式为：
(60)
这里的 [公式] 是因为是从 [公式] 的变化，是相反的。此处把旋转和平移拆解开，平移部分为， [公式] 的平移部分 [公式] 会经过三次旋转加平移的变换直到 j 的 camera 坐标系下。 变量定义：
(61)
Jacobian 对状态量，外参和逆深度求导：
(62)
step 1：先求error对 [公式] 的导数，与 《视觉SLAM十四讲》不同之处是这里在归一化平面。
(63)
step 2： [公式] 对 i 时刻的状态量求导。 对位移求导↓
(64)
对角度求导↓
(65)
只保留与 [公式] 有关的项：
(66)
这里的 [公式] 即 i 坐标系下的坐标转换到 world 坐标系下。 Jacobian 为：
(67)
step 3： [公式] 对 j 时刻的状态量求导。 对位移求导↓
(68)
负号的原因可以看式(65)， [公式] 在式中与 [公式] 相乘，求导固然结果如此。 对角度求导↓
(69)
Jacobian 为：
(70)
step 4：对 imu 与相机之间的外参求导。 就是对两传感器之间的旋转和平移求导。 对位移求导↓
(71)
对角度求导↓Jacobian 为：
(72)
step 5：视觉误差对特征逆深度的求导
(73)
IMU 误差相对于优化变量的 Jacobian
预积分误差如下图↓ 这里的误差和上部分提到的误差是两码事，我是这么理解的，上部分提到的误差是通过递推 k 到 k+1 时刻间的误差传递；这里的误差是预积分误差，是求出的两时刻的位置、速度与真实值之间的误差。
上式中， [公式] 是估计值。 由于 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的，很复杂。故预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开近似。
(74)
对 [公式] 进行推导：
  • 对 i 时刻位移：由于与位移无关，故 Jacobian 为 0；
  • 对 i 时刻旋转：
(75)
  • 对 i 时刻速度： [公式] ；
  • 对 i 时刻的加速度 bias 的 Jacobian，由于加速度只影响预积分，故 [公式] 。
对 [公式] 进行推导：
  • 对 i 时刻姿态求导：
(76)
这里的 [公式] 就是提取 xyz 虚部，乘以系数 2，就是微小转动的 [公式] ，前面有解释。然而(76)式中对分子几个四元数 [公式] ，取共轭后的 [公式] 等于在整个式子前加负号。
  • 对 i 时刻陀螺仪偏置 [公式] ：
(77)
式中对角速度 bias 求 Jacobian，用一阶泰勒展开的形式，最后矩阵内 1 对 [公式] 求导为 0。    

参考

  1. ^Civera J , Davison A J , J. M Martínez Montiel. Inverse depth parametrization for monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5):932-945. http://web.mit.edu/2.166/www/handouts/davison_tro2008.pdf
  2. ^Forster C , Carlone L , Dellaert F , et al. On-Manifold Preintegration for Real-Time Visual--Inertial Odometry[J]. ieee transactions on robotics, 2017, 33(1):1-21. https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7557075