论文：
《ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM》
ICE-BA：增量、一致且高效的视觉惯性SLAM的Bundle Adjustment

主要思想

1. 算法框架Local & globalBA：
   该算法框架包含局部优化（Local BA）和全局优化（Global BA）两个部分。局部优化在一个临时滑动窗口内进行，以减少累积误差并尽快扩展地图；全局优化则以较低的频率运行，优化那些已从局部滑动窗口移除但在全局地图中选择为关键帧的帧。
2. 高效的增量Bundle Adjustment（ICE-BA）：
   提出一种增量求解方法，使用Schur补+PCG加速求解。在每次迭代时，不对整个信息矩阵重新计算，仅对变化量较大的参数进行重新线性化。PCG用于求解位姿，而点的坐标使用回代计算得到。
3. 全局一致性问题的解决：
   在很多先进的SLAM系统中，全局一致性问题尚未得到解决。本文的方法保证在闭环过程中最小化重投影函数和惯性约束函数。

约束建立IMU&Local & globalBA

命名解释：
位姿$C_t=(T_t,M_t)=((R_T,p_t),(v_t,b_t))$
点：3维：$X_j$，二维图像坐标：$x_{i}j$表示点$X_j$被第i帧相机观察到的图像坐标。

IMU预积分

IMU预积分与VINS类似，采用QVPB的变量模式，即姿态、速度、位置、偏差(加速度、陀螺仪偏差)。
$$\begin{array}{rl}& f_{ij}^{\mathrm{imu}}({\mathbf{C}}_i,{\mathbf{C}}_j)=({\mathbf{e}}_r^T,{\mathbf{e}}_v^T,{\mathbf{e}}_p^T,{\mathbf{e}}_b^T{)}^T\sim \mathcal{N}(0,{\mathbf{\Sigma }}_{ij}^{\mathrm{imu}})\\ & {\mathbf{e}}_r=\mathrm{Log}((\mathrm{Exp}(\Delta {\mathbf{J}}_{ij}^r({\mathbf{b}}_i-{\hat{\mathbf{b}}}_i))\Delta {\mathbf{R}}_{ij}{)}^T{\mathbf{R}}_j{\mathbf{R}}_i^T)\\ & {\text{e}}_v={\mathbf{R}}_i({\mathbf{v}}_j-{\mathbf{v}}_i-\mathbf{g}\Delta t_{ij})-(\Delta {\mathbf{v}}_{ij}+\Delta {\mathbf{J}}_{ij}^v({\mathbf{b}}_i-{\mathbf{v}}_{ij}))\\ & {\mathbf{e}}_p={\mathbf{R}}_i({\mathbf{p}}_j-{\mathbf{p}}_i-{\mathbf{v}}_i\Delta t_{ij}-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2)\\ & -\left(\Delta {\mathbf{p}}_{ij}+\Delta {\mathbf{J}}_{ij}^p({\mathbf{b}}_i-{\hat{\mathbf{b}}}_i)\right)\end{array}$$

此外，由于初始帧(锚点)相机位姿及yaw不可观，在VIO SLAM系统中，且IMU不包含磁力计。因为首帧相机位姿需要固定为先验。

视觉误差也同样与VINS类似，与VINS一致，包含3个方程：

• 先验，marg后的数据
• 视觉重投影，点的参数只有深度逆。
• imu预积分数据

LocalBA

包含一定窗口大小的误差方程，窗口中包含普通帧与关键帧。约束方程如下：

arg ⁡ min ⁡ { C i , ρ j ∣ i = t 0 ⋯ t , j ∈ V i } ∑ i = t 0 t ∑ j ∈ V i ∣ ∣ f i j v i s ( T i , T s j , ρ j ) ∣ ∣ Σ i j v i s + ∣ ∣ f t 0 p r i o r ( C t 0 ) ∣ ∣ Σ t 0 p r i o r + ∑ i = t 0 t − 1 ∣ ∣ f i , i + 1 i m u ( C i , C i + 1 ) ∣ ∣ Σ i , i + 1 i m u \arg\operatorname*{min}_{\{\mathbf{C}_{i},\rho_{j}|i=t_{0}\cdots t,j\in\mathcal{V}_{i}\}}\sum_{i=t_{0}}^{t}\sum_{j\in\mathcal{V}_{i}}||f_{ij}^{\mathrm{vis}}(\mathbf{T}_{i},\mathbf{T}_{s_{j}},\rho_{j})||_{\boldsymbol{\Sigma}_{ij}^{\mathrm{vis}}}\\+||f_{t_{0}}^{\mathrm{prior}}(\mathbf{C}_{t_{0}})||_{\boldsymbol{\Sigma}_{t_{0}}^{\mathrm{prior}}}+\sum_{i=t_{0}}^{t-1}||f_{i,i+1}^{\mathrm{imu}}(\mathbf{C}_{i},\mathbf{C}_{i+1})||_{\boldsymbol{\Sigma}_{i,i+1}^{\mathrm{imu}}} arg{Ci​,ρj​∣i=t0​⋯t,j∈Vi​}min​i=t0​∑t​j∈Vi​∑​∣∣fijvis​(Ti​,Tsj​​,ρj​)∣∣Σijvis​​+∣∣ft0​prior​(Ct0​​)∣∣Σt0​prior​​+i=t0​∑t−1​∣∣fi,i+1imu​(Ci​,Ci+1​)∣∣Σi,i+1imu​​

其中${\mathcal{V}}_i$表示第 i帧相机观测到的左右点的集合。

GlobalBA

全局BA工作在低频的状态下，只处理关键帧。
关键帧的选定标准：当某帧包含了一定数个其他帧没有的特征点，则该帧被认定为关键帧。
其约束如下：
arg ⁡ min ⁡ { C i , ρ j ∣ i ∈ { k 1 ⋯ k m } , j ∈ V i } ∑ i = k 1 k m ∑ j ∈ V i ∣ ∣ f i j v i s ( T i , T s j , ρ j ) ∣ ∣ Σ i j v i s + ∣ ∣ f 0 p r i o r ( C k 1 ) ∣ ∣ Σ 0 p r i o r + ∑ i = 1 m − 1 ∣ ∣ f k i , k i + 1 i m u ( C k i , C k i + 1 ) ∣ ∣ Σ k i , k i + 1 i m u + ∑ ∣ ∣ f i r e l ( { T k ∈ L i } ) ∣ ∣ Σ i r e l \begin{aligned} &\arg\min_{\{\mathbf{C}_{i},\rho_{j}|i\in\{k_{1}\cdots k_{m}\},j\in\mathcal{V}_{i}\}}\sum_{i=k_{1}}^{k_{m}}\sum_{j\in\mathcal{V}_{i}}||f_{ij}^{\mathrm{vis}}(\mathbf{T}_{i},\mathbf{T}_{s_{j}},\rho_{j})||_{\boldsymbol{\Sigma}_{ij}^{\mathrm{vis}}}+ \\ &||f_{0}^{\mathrm{prior}}(\mathbf{C}_{k_{1}})||_{\boldsymbol{\Sigma}_{0}^{\mathrm{prior}}}+\sum_{i=1}^{m-1}||f_{k_{i},k_{i+1}}^{\mathrm{imu}}(\mathbf{C}_{k_{i}},\mathbf{C}_{k_{i+1}})||_{\boldsymbol{\Sigma}_{k_{i},k_{i+1}}^{\mathrm{imu}}}+ \\ &\sum||f_{i}^{\mathrm{rel}}(\{\mathbf{T}_{k\in\mathcal{L}_{i}}\})||_{\mathbf{\Sigma}_{i}^{\mathrm{rel}}} \end{aligned} ​arg{Ci​,ρj​∣i∈{k1​⋯km​},j∈Vi​}min​i=k1​∑km​​j∈Vi​∑​∣∣fijvis​(Ti​,Tsj​​,ρj​)∣∣Σijvis​​+∣∣f0prior​(Ck1​​)∣∣Σ0prior​​+i=1∑m−1​∣∣fki​,ki+1​imu​(Cki​​,Cki+1​​)∣∣Σki​,ki+1​imu​​+∑∣∣firel​({Tk∈Li​​})∣∣Σirel​​​
其中：${\mathcal{L}}_i$表示与第i帧想关联的关键帧的集合。
相比localBA，globalBA多了一项Relative Marginalization相关的约束，用于约束最新被localBA marg的关键帧与窗口内最新的帧之间的关系。从而避免localBA的漂移。

求解方法

求解localBA以及globalBA构成的最小二乘问题，典型的方法是使用高斯牛顿法，逐步迭代得到全局最小解。
每次迭代时，需要通过计算最小二乘问题$\arg {\min }_{\varphi }{\sum }_k||{\tilde{f}}_k(\varphi )|{|}^2$的信息矩阵A及雅可比矩阵b来得到参数的更新量：
$\delta \varphi =A^{-1}b=(J^{T}F{)}^{-1}{J}^{T}r$

注意最小二乘问题的雅可比$b$，这里是有高斯牛顿法近似得到的，不同于$f_k$的雅可比。
$$f_k({\varphi }^{-}\oplus \delta \varphi )\approx {\mathbf{J}}_k\delta \varphi +{\mathbf{e}}_k$$$${\mathbf{J}}_k=\frac{\partial f_k({\varphi }^{-}\oplus \delta \varphi )}{\partial \delta \varphi }{|}_{\delta \varphi =0}$$$${\mathbf{e}}_k=f_k({\varphi }^{-})$$

求解：
$$\begin{array}{rl}\mathbf{A}\delta \varphi & =\mathbf{b}\\ [\mathbf{A}|\mathbf{b}]& =\sum _k[{\mathbf{A}}_k|{\mathbf{b}}_k]\\ [{\mathbf{A}}_k|{\mathbf{b}}_k]& =[{\mathbf{J}}_k^T{\mathbf{J}}_k|-{\mathbf{J}}_k{\mathbf{e}}_k]\end{array}$$

常用的求解思路

技术手段：增量更新信息矩阵；Schur分块+PCG求解
由于SLAM的信息矩阵的特殊性：

其点云的为对角块矩阵，比位姿的维度大很多，利用Schur补，将point先marg出去，而后求解位姿，得到位姿后，通过回代再得到点的坐标解。
首先将信息矩阵的增广矩阵分块
$$[\mathbf{A}|\mathbf{b}]=\left[\begin{array}{ccc}\mathbf{U}& \mathbf{W}& \mathbf{u}\\ {\mathbf{W}}^T& \mathbf{V}& \mathbf{v}\end{array}\right]$$
$$\varphi =({\varphi }_c^T,{\varphi }_p^T{)}^T$$

采用增量式计算信息矩阵及雅可比的方式：
$$[\mathbf{A}|\mathbf{b}{]}^{+}=[\mathbf{A}|\mathbf{b}{]}^{-}+[\begin{array}{cc}{\sum }_{k\in \mathcal{L}}\delta {\mathbf{A}}_k& {\sum }_{k\in \mathcal{L}}\delta {\mathbf{b}}_k\end{array}]$$

使用$S$表示marg点后得到的关于位姿的信息矩阵，问题变为：
$$\begin{array}{rl}& \mathbf{S}\delta {\varphi }_c=\mathbf{s}\\ & [\mathbf{S}|\mathbf{s}]=\left[\begin{array}{cc}\mathbf{U}-\mathbf{W}{\mathbf{V}}^{-1}{\mathbf{W}}^T& \mathbf{u}-\mathbf{W}{\mathbf{V}}^{-1}\mathbf{v}\end{array}\right]\end{array}$$
具体计算：
$$\left[{\mathbf{S}}_{i_1{i}_2}|{\mathbf{s}}_i\right]=[\begin{array}{cc}{\mathbf{U}}_{i_1{i}_2}-{\sum }_{j\in {\mathcal{V}}_{i_1}\cup {\mathcal{V}}_{i_2}}{\mathbf{S}}_{i_1{i}_2}^j& {\mathbf{u}}_i-{\sum }_{j\in {\mathcal{V}}_i}{\mathbf{s}}_i^j\end{array}]$$
$$[{\mathbf{S}}_{i_1{i}_2}^j|{\mathbf{s}}_i^j]=\left[\begin{array}{c}{\mathbf{W}}_{i_{1}j}{\mathbf{V}}_{jj}^{-1}{\mathbf{W}}_{i_{2}j}^T\left|{\mathbf{W}}_{ij}{\mathbf{V}}_{jj}^{-1}{\mathbf{v}}_j\right]\end{array}\right]$$

作者提出了增量式的S的更新方式：
$$\begin{array}{rl}[{\mathbf{S}}_{i_1{i}_2}|{\mathbf{s}}_i{]}^{+}& ={\left[{\mathbf{S}}_{i_1{i}_2}|{\mathbf{s}}_i\right]}^{-}+[\begin{array}{c}{\sum }_{j\in {\mathcal{P}}_{i_1{i}_2}}\delta {\mathbf{S}}_{i_1{i}_2}^j\left|{\sum }_{j\in {\mathcal{P}}_{ii}}\delta {\mathbf{s}}_i^j\right]\end{array}]\\ {\mathcal{P}}_{i_1{i}_2}& =\mathcal{P}\cup {\mathcal{V}}_{i_1}\cup {\mathcal{V}}_{i_2}\end{array}$$

在得到$[S|s]$后，由于S的稀疏性($s_{i_1,i_2}$非0的条件是，$i_1$, $i_2$相机有共同观测点)，使用PCG求解${\varphi }_c$，而后使用回代法求得${\varphi }_p$
$$\delta {\varphi }_{p_j}={\mathbf{V}}_{jj}^{-1}\left({\mathbf{v}}_j-\sum _{i\in {\mathcal{X}}_j}{\mathbf{W}}_{ij}^T\delta {\varphi }_{c_i}\right)$$
${\mathcal{X}}_j$表示能够观测点点j的相机集合。

这里引入文中原话：
“due to the nature of SLAM problem, new states and measurements always arrive incrementally. As a result, only a small portion of variables change at each iteration”。这句话的因果。迭代iteration是指下降法中的迭代，每次迭代都需要对变量进行更新，从而得到新的线性化点。SLAM是时间顺序的，每帧会增加一些观测。这里混淆了每帧的到来，和每次的迭代这两个含义。从而使得增量法的出发点有了一定的问题。

关于iSAM的增量法可行原因是，iSAM是基于好的线性化点假设，可以理解为通过一次迭代便能够接近最小解。而使用iSAM的方法也可以计算一次迭代，而iSAM的增量计算不能工作在每次的迭代中。

此外，引入作者另一篇论文《Robust Keyframe-based Dense SLAM with an RGB-D Camera》关于这部分求解的流程图：
普通解法：

增量解法流程：

从这篇RGBD论文中，作者将变量的更新值大于一定阈值的变量才会进行重新线性化，即更新雅可比。从而解释了上面关于SLAM每次迭代只需要更新很少雅可比的原因。但于此同时，同步引入误差到解中，一种精度换效率的方法。

改进增量BA方法

1、 增量ImprovementforLocalBA
IBA(Schur补，PCG增量求解) 能够显著加快global BA的求解速度。因为globalBA中存放的都是关键帧，帧间共同观测的点比较少，从而会保证S的稀疏性，从而保证PCG的有效性。
IBA(Schur补，PCG增量求解) 能够显著加快global BA的求解速度。因为globalBA中存放的都是关键帧，帧间共同观测的点比较少，从而会保证S的稀疏性，从而保证PCG的有效性。
作者提出将$(x{)}_j$集合分为多个小的集合，如下所示：

点的深度也对应上图，分为对应个。从而使得S矩阵从稠密矩阵变为对角带状矩阵。从而增加稀疏性。
2、 IncrementalPCGforIBA
当$\delta {\varphi }_c$比较小时，对应的雅可比不更新。
3、 RelativeMarginalization
为了减少localBA由于边缘化的先验等引入的漂移误差。作者提出通过相对边缘化，保持先于边缘化和global BA 之间的一致性。
将当前位姿$C_i$，以及观测点的第一帧$C_{s_j}$表示到参考帧下。
即都乘了$T_{k_0}^{-1}$

参考帧选的是最近的关键帧，用相对参考帧的描述(位姿和点在参考系下的表示)。而不是绝对的位姿和点的世界坐标系的表示。

总结：
这篇论文提出了几个关于增量迭代求解的技术思路，精度换效率。多是在技术角度上提出，而非算法理论创新。