SLAM--BA优化

一、BA优化

BA（Bundle Adjustment）优化，是指从视觉重建中提炼出最优的 $3D$ 模型和相机参数(内参数和外参数)。从每一个特征点反射出来的几束光线(bundles of light rays),在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后,最后收束到相机光心的这个过程。和重投影不同的是，BA优化是对多段相机的位姿和位姿下的路标点的空间坐标进行优化。

我们的误差可以写成：
$$e(\xi ,p)=z-\frac{1}{s}KTP$$
即：
$$e(\xi ,p)=z-\frac{1}{s}K\cdot \exp ({\xi }^{\wedge })\cdot P$$
根据我之前写的文章：

SLAM–最小化重投影误差

我们可以得出误差$e$分别对位姿和路标坐标的偏导$J_{\delta \xi }$和$J_p$：
$$J_{\delta \xi }=\frac{\partial e\text{e}e}{\partial \delta \xi }=-\left[\begin{array}{ccccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}& \\ & & & & & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{Y}^{\prime }{X}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}& \end{array}\right]$$

$$J_p=\frac{\partial e\text{e}e}{\partial P\text{P}P}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ & & \\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]\cdot R\text{R}R$$

我们令位姿变量为$\xi$，特征点空间位置为$p$，对于m个位姿和n个特征点：
$$x\text{x}x=[{\xi }_1{\xi }_2...{\xi }_m|p_1{p}_2...p_n{]}^T$$

优化的目标函数为：
$$\frac{1}{2}{\Vert f(x+\Delta x)\Vert }^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{\Vert e_{ij}+F_{ij}\Delta {\xi }_i+E_{ij}\Delta p_j\Vert }^2$$
其中，$F_{ij}$是相对姿态的偏导数，$E_{ij}$是相对与路标点的偏导；

为了简明公式，我们令：
$$\begin{gathered} x_c=[{\xi }_1{\xi }_2...{\xi }_m] \\ {x}_p=[p_1{p}_2...p_n] \end{gathered}$$
我们的目标函数可以写成:
$$\frac{1}{2}{\Vert f(x+\Delta x)\Vert }^2=\frac{1}{2}{\Vert e+F\Delta x_c+E\Delta x_p\Vert }^2$$
对于线性增量方程：$H\Delta x=g$，这里$H=J^{T}J$；

我们可以把雅克比矩阵分块：
$$J=[FE],J^T=\left[\begin{array}{c}{F}^T\\ \\ E^T\end{array}\right]$$
则：
$$H=J^{T}J=\left[\begin{array}{ccc}{F}^{T}F& & F^{T}E\\ & & \\ E^{T}F& & E^{T}E\end{array}\right]$$
 

 

二、利用稀疏性求解

$J_{ij}$表示在位姿${\xi }_i$只观测到$p_j$时的雅克比矩阵，所以其他部分的导数为零。
$$J_{ij}(x)=\left[\begin{array}{cc}0,& 0,...,(\frac{\partial {e\text{e}e}_{ij}}{\partial \delta {\xi }_i}{)}_{2\times 6},...,0_{2\times 6}，|0_{2\times 3}，...,(\frac{\partial {e\text{e}e}_{ij}}{\partial p_i}{)}_{2\times 3}\end{array}\right]$$
所以对于整体：
$$H=\sum _{ij}{J}_{ij}^T{J}_{ij}$$
由此可以把所有的$J_{ij}$放到一个$J$表达式里：
$$J=\left[\begin{array}{c}{J}_{11}\\ J_{12}\\ ⋮\\ J_{i1}\\ J_{i2}\\ ⋮\\ J_{ij}\end{array}\right]$$
又有稀疏性和对称性，我们把$H$分块化：
$$H=\left[\begin{array}{cc}B& E\\ E^T& C\end{array}\right]$$
由$H\Delta x=g$可变为：
$$\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]=\left[\begin{array}{c}v\\ w\end{array}\right]$$
利用稀疏性进行高斯消元：
$$\begin{gathered} \left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\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]=\left[\begin{array}{cc}I& -E{C}^{-1}\\ 0& I\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right] \\ ⟹\left[\begin{array}{cc}B-E{C}^{-1}{E}^T& 0\\ E^T& C\end{array}\right]\left[\begin{array}{c}\Delta x_c\\ \Delta x_p\end{array}\right]=\left[\begin{array}{c}v-E{C}^{-1}w\\ w\end{array}\right] \end{gathered}$$
整理得：
$$(B-E{C}^{-1}{E}^T)\Delta x_c=v-E{C}^{-1}w$$
继而可以求出：
$$∆x_p=C^{-1}(w-E^T∆x_c)$$