1. 前言

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

2. 非线性最小二乘

尽管非线性最小二乘是很常见的问题，可参考《SLAM14讲》第六章节，《多视图几何》附录6，乔治梅森大学Timothy Sauer的《数值分析》都有提及，在我之前的博客非线性最小二乘也进行了总结，这里简单回顾一下，由于VINS-Mono中在视觉重投影误差添加了鲁棒核函数，因此也整理下对残差添加鲁棒核函数对需要优化的状态量增量方程的影响。
对于最常见的最小二乘问题有： $\underset{x}{min}F(x) = \underset{x}{min}\frac{1}{2} \left \| f(x) \right \|_{2}^{2}$ 将残差写成组合向量的形式： $f(x) = \begin{bmatrix} f_{1}(x) \\ \cdots \\ f_{m}(x)\end{bmatrix}$, 则 $F(x)=\left \| f(x) \right \|_{2}^{2}=f^{T}(x)f(x)=\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} = \begin{bmatrix} J_{1}(x) \\ \cdots \\ J_{m}(x)\end{bmatrix}\ , \ J_{i}(x) ^{1 \times n} = \begin{bmatrix} \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{bmatrix}$

对残差函数 $f(x)$进行一阶泰勒展开： $f(x+\Delta x ) = f(x) + J \Delta x$, 其中 $J$是残差函数 $f$的雅可比矩阵，代入损失函数(以下推导用 $f$代替 $f(x)$)： $F(x+\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 \tag{1}$ 这样损失函数就近似成了一个二次函数，如果雅可比矩阵是满秩的(并不一定满秩)，则 $J^{T}J$正定，损失函数有最小值，且易得 $F'(x) = (J^{T}f)^{T}, \ F''(x) = J^{T}J$

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

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

阻尼因子的作用： $\mu > 0$保证 $(J^{T}J+\mu I)$正定，迭代朝着下降方向进行。

阻尼因子 $\mu$的更新策略：

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

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}, \mathbf{\lambda}_{0}, \mathbf{\lambda}_{1}, \cdots, \mathbf{\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{\mathbf{\mathcal{X}}}{min}\begin{Bmatrix}\left \| \mathbf{r}_{p}-\mathbf{H}_{p} \mathbf{\mathcal{X}} \right \|^{2} + \underset{k \in \mathcal{B}}{\sum} \left \| \mathbf{r}_{\mathcal{B}}(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathbf{\mathcal{X}} )\right \| _{\mathbf{P}_{b_{k+1}}^{b_{k}}} ^{2} + \underset{(l,j)\in \mathcal{C}}{\sum} \rho(\left \| \mathbf{r}_{\mathcal{C}}(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathbf{\mathcal{X}})\right \|_{\mathbf{P}_{l}^{c_{j}}}^{2}) \end{Bmatrix}$ $\rho(s)=\left\{\begin{matrix} 1, s\geq 1\\ 2\sqrt{s}-1, s<1 \end{matrix}\right.$ 其中这三项分别为边缘化的先验信息 (由滑动窗口产生的关键帧边缘化)、IMU的测量误差、视觉的重投影误差，三种残差都是用马氏距离表示。 $\mathbf{P}_{b_{k+1}}^{b_{k}}$代表IMU残差的协方差， $\mathbf{P}_{l}^{c_{j}}$代表视觉重投影误差的协方差，通过协方差的逆 (即信息矩阵) 对各个变量计算残差时进行加权。 $\rho(s)$为Huber核函数。

由于在IMU测量误差和视觉的重投影误差通过协方差的逆进行了加权，因此实际优化的是 $min(d) = min(r_{k}^{T}P^{-1}R_{k})$, 这里 $r_{k}$代表 $k$时刻时候的残差， $P$代表协方差。而Ceres只接受诸如 $min(r_{k}^{T}r_{k})$的优化，因此在代码里，需要将信息矩阵 $P^{-1}$做 $LLT$分解，即 $LL^{T} = P^{-1}$ ,代码中对应为sqrt_info,这样就可以将优化进行转换： $min(d) = min(r_{k}^{T}P^{-1}R_{k}) = min ((L^{T}r_{k})^{T}(L^{T}r_{k})) = min(r_{k}'^{T} r_{k})$