VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导)

1. 前言

之前看过崔华坤的《VINS论文推导及代码解析》还有深蓝学院的VIO课程，对VINS的后端非线性优化有了较为清晰的认识，但是一直没有时间整理写成笔记，最近看到Manni的博客VINS-Mono理论学习——后端非线性优化 概括得很不错，针对这三份资料还有自己的一些理解重新整理下，感谢优秀的大佬们提供的参考资料。

2. 非线性最小二乘

尽管非线性最小二乘是很常见的问题，可参考《SLAM14讲》第六章节，《多视图几何》附录6，乔治梅森大学Timothy Sauer的《数值分析》(忘记第几章了，书也没带回家，哭.jpg)，在我之前的博客非线性最小二乘也对Guass-Newton和LM进行了介绍，这里简单回顾一下，由于VINS-Mono中在视觉重投影误差添加了鲁棒核函数，因此也整理下对 残差(也可以说是误差，一个意思，以下内容不分残差和误差) 添加鲁棒核函数对需要优化的状态量增量方程的影响。

2.1 Guass-Newton 和 Levenberg-Marquardt

对于最常见的最小二乘问题有：$$\underset{x}{\min }F(x)=\underset{x}{\min }\frac{1}{2}{\Vert f(x)\Vert }_2^2$$ 将残差写成组合向量的形式：$f(x)=\left[\begin{array}{c}{f}_1(x)\\ \cdots \\ f_m(x)\end{array}\right]$, 则$$F(x)=\frac{1}{2}{\Vert f(x)\Vert }_2^2=\frac{1}{2}{f}^T(x)f(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$ 同理，如果记$J_i(x)=\frac{\partial f_i(x)}{\partial x}$, 则有：$$\frac{\partial f(x)}{\partial x}=J^{m\times n}=\left[\begin{array}{c}{J}_1(x)\\ \cdots \\ J_m(x)\end{array}\right],J_i(x{)}^{1\times n}=\left[\begin{array}{c}\frac{\partial f_i(x)}{\partial x_1}\frac{\partial f_i(x)}{\partial x_2}\cdots \frac{\partial f_i(x)}{\partial x_n}\end{array}\right]$$

对残差函数$f(x)$进行一阶泰勒展开：$f(x+\Delta x)=f(x)+J\Delta x$, 其中$J$是残差函数$f$的雅可比矩阵，代入损失函数(以下推导用$f$代替$f(x)$)：$$\begin{gathered} F(x+\Delta x)\approx L(\Delta x)=\frac{1}{2}(f+J\Delta x{)}^T(f+J\Delta x)=\frac{1}{2}{f}^{T}f+\Delta x^T{J}^{T}f+\frac{1}{2}\Delta x^T{J}^{T}J\Delta x \\ =F(x)+\Delta x^T{J}^{T}f+\frac{1}{2}\Delta x^T{J}^{T}J\Delta x\text{\hspace{1em}(1)} \end{gathered}$$ 这样损失函数就近似成了一个二次函数，如果雅可比矩阵是满秩的(并不一定满秩)，则$J^{T}J$正定，损失函数有最小值，且易得 $F^{\prime }(x)=(J^{T}f{)}^T,F^{\prime \prime }(x)=J^{T}J$

Gauss-Newton Method：
令公式(1)的一阶导数等于0，得到$$(J^{T}J)\Delta x_{gn}=-J^{T}f$$ 由于$H=J^{T}J$可能出现H矩阵奇异或者病态，此时高斯牛顿法增量的稳定性较差，导致算法不收敛。同时对于步长问题，若求出来的$\Delta x_{gn}$太长，则可能出现局部近似不够准确，无法保证迭代收敛。

Levenberg-Marquardt Method (LM):
在高斯牛顿法的基础上引入阻尼因子：$$(J^{T}J+\mu I)\Delta x_{lm}=-J^{T}f,s.t.\mu \ge 0$$ 其中$\mu$称为阻尼因子。

阻尼因子的作用： $\mu >0$保证$(J^{T}J+\mu I)$正定，迭代朝着下降方向进行。
阻尼因子的初始化： ${\mu }_0=\tau max\left\{\begin{array}{c}(J^{T}J{)}_{ij}\end{array}\right\}$, 按需设定 $\tau \in [10^{-8},1]$。
阻尼因子$\mu$的更新策略：

• 如果$\Delta x\to F(x)\uparrow$，则 $\mu \uparrow \to \Delta x\downarrow$，增大阻尼减小步长，拒绝本次迭代。
• 如果$\Delta x\to F(x)\downarrow$，则 $\mu \downarrow \to \Delta x\uparrow$，减小阻尼增加步长，减少迭代次数。

$\Delta x$和阻尼因子$\mu$的关系：
阻尼因子的更新由比例因子来确定：
$$\rho =\frac{F(x)-F(x+\Delta x_{lm})}{L(0)-L(\Delta x_{lm})}=\frac{实际下降}{近似下降}$$ 其中 $$\begin{gathered} L(0)-L(\Delta x_{lm})=-\Delta x_{lm}^T{J}^{T}f-\frac{1}{2}\Delta x_{lm}^T{J}^{T}J\Delta x \\ =-\frac{1}{2}\Delta x_{lm}^T(2J^{T}f+(J^{T}J+\mu I-\mu I)\Delta x_{lm}) \\ =-\frac{1}{2}\Delta x_{lm}^T(J^{T}f-\mu I\Delta x_{lm}) \\ =\frac{1}{2}\Delta x_{lm}^T(\mu \Delta x_{lm}-J^{T}f) \end{gathered}$$ 首先比例因子分母始终大于$0$，因为是沿负梯度方向进行的$\Delta x_{lm}$的调整，故$L(0)>L(\Delta x_{lm})$，如果：

• $\rho <0$，则$F(x)\uparrow$，应该$\mu \uparrow \to \Delta x\downarrow$，增大阻尼减小步长。
• 如果$\rho >0$且比较大，减小$\mu$，让LM接近Gauss-Newton使得系统更快收敛。
• 反之，如果是比较小的正数，则增大阻尼$\mu$，缩小迭代步长。

2.2 鲁棒核函数下状态量增量方程的构建

由上得 $$F(x)=\frac{1}{2}{\Vert f(x)\Vert }_2^2=\frac{1}{2}{f}^T(x)f(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$ 鲁棒核函数直接作用于残差$f_i(x)$上，则有
$$\underset{x}{\min }\frac{1}{2}\sum _i\rho ({\Vert f_i(x)\Vert }_2^2)=\underset{x}{\min }\frac{1}{2}\sum _i\rho (f_i(x{)}^2)$$ 将误差的平方项记作$s_i={\Vert f_i(x)\Vert }_2^2$，对鲁棒核误差函数进行二阶泰勒展开有：
$$\rho (s+\Delta s)\approx \rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s\text{\hspace{1em}(2)}$$

---

对于$\Delta s$的计算，我们有：
$$\begin{gathered} \Delta s={\Vert f_i(x+\Delta x)\Vert }_2^2-{\Vert f_i(x)\Vert }_2^2\approx {\Vert f_i(x)+J_i\Delta x\Vert }_2^2-{\Vert f_i(x)\Vert }_2^2 \\ =(f_i(x)+J_i\Delta x{)}^2-f_i^2(x)=2f_i^T(x)J_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x\text{\hspace{1em}(3.1)} \end{gathered}$$
若误差项为$\Sigma$范数(表征为误差项的协方差矩阵，协方差为对称矩阵，故协方差的逆(称为信息矩阵)亦为对称矩阵)而非二范数，我们有：
$$\begin{gathered} \Delta s={\Vert f_i(x+\Delta x)\Vert }_{{\Sigma }_i}^2-{\Vert f_i(x)\Vert }_{{\Sigma }_i}^2\approx {\Vert f_i(x)+J_i\Delta x\Vert }_{{\Sigma }_i}^2-{\Vert f_i(x)\Vert }_{{\Sigma }_i}^2 \\ =(f_i(x)+J_i\Delta x{)}^T{\Sigma }_i^{-1}(f_i(x)+J_i\Delta x)-f_i^T(x){\Sigma }_i^{-1}{f}_i(x) \\ =({\Sigma }_i^{-T}{f}_i(x){)}^T{J}_i\Delta x+(J_i\Delta x{)}^T{\Sigma }_i^{-1}{f}_i(x)+\Delta x^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x \\ =2f_i^T(x){\Sigma }_i^{-1}{J}_i\Delta x+\Delta x^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x\text{\hspace{1em}(3.2)} \end{gathered}$$

---

将公式$(3)$代入公式$(2)$，令$f_i(x)=f_i$ ，$\rho =\rho (s)$，可得
$$\begin{gathered} \frac{1}{2}\rho (s+\Delta s)\approx \frac{1}{2}(\rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s) \\ =\frac{1}{2}(\rho +{\rho }^{\prime }[2f_i^T{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x]+\frac{1}{2}{\rho }^{\prime \prime }[2f_i^T{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{J}_i\Delta x{]}^2) \\ \approx \rho +{\rho }^{\prime }{f}_i^T{J}_i\Delta x+\frac{1}{2}{\rho }^{\prime }(\Delta x{)}^T{J}_i^T{J}_i^T\Delta x+{\rho }^{\prime \prime }(\Delta x{)}^T{J}_i^T{f}_i{f}_i^T{J}_i\Delta x \\ =\rho +{\rho }^{\prime }{f}_i^T{J}_i\Delta x+\frac{1}{2}(\Delta x{)}^T{J}_i^T({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T)J_i\Delta x\text{\hspace{1em}(4.1)} \end{gathered}$$
若误差项为$\Sigma$范数：
$$\begin{gathered} \frac{1}{2}\rho (s+\Delta s)\approx \frac{1}{2}(\rho (s)+{\rho }^{\prime }(s)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(s){\Delta }^{2}s) \\ =\frac{1}{2}(\rho +{\rho }^{\prime }[2f_i^T{\Sigma }_i^{-1}{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x]+\frac{1}{2}{\rho }^{\prime \prime }[2f_i^T{\Sigma }_i^{-1}{J}_i\Delta x+(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x{]}^2) \\ \approx \rho +{\rho }^{\prime }{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+\frac{1}{2}{\rho }^{\prime }(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+{\rho }^{\prime \prime }(\Delta x{)}^T{J}_i^T{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x \\ =\rho +{\rho }^{\prime }{f}_i^T{\Sigma }_i^{-1}{J}_i\Delta x+\frac{1}{2}(\Delta x{)}^T{J}_i^T({\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1})J_i\Delta x\text{\hspace{1em}(4.2)} \end{gathered}$$

---

由于$\Delta x$是一个极小量，故其三阶及以上的项数值很小，近似忽略$\Delta x$三阶及其以上的项。
对公式$(4)$中的$\Delta x$进行求导后，令其等于0，得到： $$\sum _i{J}_i^T({\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T)J_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{f}_i$$ 化简得：$$\sum _i{J}_i^{T}W{J}_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{f}_i$$ 其中 $W={\rho }^{\prime }I+2{\rho }^{\prime \prime }{f}_i{f}_i^T$

若误差项为$\Sigma$范数：
$$\sum _i{J}_i^T({\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1})J_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{\Sigma }_i^{-1}{f}_i$$ 化简得：$$\sum _i{J}_i^{T}W{J}_i\Delta x=-\sum _i{\rho }^{\prime }{J}_i^T{\Sigma }_i^{-1}{f}_i$$ 其中 $W={\rho }^{\prime }{\Sigma }_i^{-1}+2{\rho }^{\prime \prime }{\Sigma }_i^{-1}{f}_i{f}_i^T{\Sigma }_i^{-1}$

---

3. 局部Bundle Adjustment代价函数的构建

对于需要优化的状态向量，包括滑动窗口内的所有IMU状态${\mathbf{x}}_k$(位姿$P$、速度$V$、旋转$Q$、加速度偏置$b_a$，陀螺仪偏置$b_w$)、相机到IMU的外参 ${\mathbf{x}}_c^b$、所有3D点的逆深度 ${\lambda }_m$：【论文公式(21)】$$\mathcal{X}=[{\mathbf{x}}_0,{\mathbf{x}}_1,\cdots ,{\mathbf{x}}_n,{\mathbf{x}}_c^b,{\lambda }_0,{\lambda }_1,\cdots ,{\lambda }_m]$$ $${\mathbf{x}}_k=[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_g],k\in [0,n]$$ $${\mathbf{x}}_c^b=[{\mathbf{p}}_c^b,{\mathbf{q}}_c^b]$$ 其中${\mathbf{x}}_k$代表相机获取图片时对应时刻$k$的IMU状态，$n$为滑动窗口内的关键帧的数量，$m$为滑动窗口内3D视觉特征(空间点)的数量，${\lambda }_m$为第$m$个3D视觉特征的逆深度(第一次被观察时计算得到的值)。
目标函数为 【论文公式(22)(23)】：$$\underset{\mathcal{X}}{\min }\left\{\begin{array}{c}{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathcal{X}\Vert }^2+\sum _{k\in \mathcal{B}}{\Vert {\mathbf{r}}_{\mathcal{B}}({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathcal{X})\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in \mathcal{C}}\rho ({\Vert {\mathbf{r}}_{\mathcal{C}}({\hat{\mathbf{z}}}_l^{c_j},\mathcal{X})\Vert }_{{\mathbf{P}}_l^{c_j}}^2)\end{array}\right\}\text{\hspace{1em}(5)}$$ $$\rho (s)=\{\begin{array}{c}1,s\ge 1\\ 2\sqrt{s}-1,s<1\end{array}$$ 其中这三项分别为边缘化的先验信息 (由滑动窗口产生的关键帧边缘化)、IMU的测量误差、视觉的重投影误差，三种残差都是用马氏距离表示。${\mathbf{P}}_{b_{k+1}}^{b_k}$代表IMU残差的协方差，${\mathbf{P}}_l^{c_j}$代表视觉重投影误差的协方差，通过协方差的逆 (即信息矩阵) 对各个变量计算残差时进行加权。$\rho (s)$为Huber核函数。

4. IMU测量残差 (增量误差) 及其对状态量雅克比矩阵、协方差递推方程的推导

4.1 IMU测量残差 (增量误差)

在IMU预积分中我们已经得到了：
$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2)+{\alpha }_{b_{k+1}}^{b_k}$$ $$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}$$ $$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$$
因此俩帧之间的PVQ和bias的变化量(增量)误差为：【论文公式[24]】
r B 15 × 1 ( z ^ b k + 1 b k , X ) = [ δ α b k + 1 b k δ β b k + 1 b k δ θ b k + 1 b k δ b a δ b w ] = [ R w b k ( p b k + 1 w − p b k w − v k w Δ t k + 1 2 g w Δ t k 2 ) − α b k + 1 b k R w b k