本篇为Eigen的实战环节: eigen求解BA问题。

一、什么是BA

BA全称Bundle Adjustment,就是最小化重投影误差,本质就是一个特定的优化问题. 其特点在于待优化的Hessian矩阵具有特殊结构,很大却很稀疏,能够使用各种技巧进行加速,最常用的技巧就是shur complement.
首先BA问题的传统解法,可以拆分成4步,具体来说:
1. 列正规方程 $$H \Delta x=-Jf(x) \tag{1}$$ 不同优化方法,正规方程形式不同; GN方法用$J^TJ$作为Hessian的近似, LM方法则又结合了梯度下降法,左边变为$(H +\lambda I) \Delta x$。

2. 求schur complement
对于1中的Hessian矩阵,其结构如下: $$ H=\left[\begin{array}{cc} B & E\\ E^{T} & C \end{array}\right] \tag{2} $$
其中,B代表cam与cam之间的联系,E代表cam与point之间的联系,C代表点与点之间的联系;
在SLAM中,实际结构大概是这样的:

BA_hessian

BA_Hessian (图片来自wikipedia)

对于如此稀疏的矩阵,我们可以先计算右上角的E进行高斯消元,原式:

$$ \left[\begin{array}{cc} B & E\\ E^{T} & C \end{array}\right]\left[\begin{array}{c} \Delta x_{c}\\ \Delta x_{p} \end{array}\right]=[\begin{array}{c} v\\ w \end{array}] \tag{3} $$

高斯消元: $$ \left[\begin{array}{cc} B-EC^{-1}E^{T} & 0\\ E^{T} & C \end{array}\right]\left[\begin{array}{c} \Delta x_{c}\\ \Delta x_{p} \end{array}\right]=[\begin{array}{c} v-EC^{-1}w\\ w \end{array}] \tag{4} $$

通过此式,即可先求解出$\Delta x_c$。
这一步所谓计算 Schur complement 即是计算子块$ B-EC^{-1}E^T $ 的过程。

3. 计算$\Delta x_c$
求解下式: $$ \left[B-EC^{-1}E^{T}\right]\Delta x_{c}=v-EC^{-1}w \tag{5}$$
此式可用多种方法求解,另外,由于其本身仍具备稀疏性,并且对称正定,通常会选用 Sparse Cholesky 进行求解。
4. 计算$\Delta x_p$ 求解出$\Delta x_c$之后就可以代入求解$\Delta x_p$ $$ \Delta x_{p}=C^{-1}(w-E^{T}\Delta x_{c}) \tag{6}$$

二、Eigen实现

对于这样的稀疏矩阵,务必需要使用Sparse Matrix。
这里的问题测试,采用LM方法,使用Schur Complement Trick.

三、进阶

实际上,上述的Schur Complement方法,是Preconditioner的一种特例。 使用通用的SSOR方法可以达到同样的效果,能够更加通用的实现BA.