手推 Bundle Adjustment（1）--理论推导

Bundle Adjustment主要应用在从视觉重建中中恢复出最优的3D模型（路标）和相机参数（外参的位姿和内参矩阵）。属于从离散非线性非高斯系统中批量式最大后验估计最优状态问题。

投影模型

相机成像可以分为四个步骤：
!

(1)刚体变换只改变物体的空间位置(平移)和朝向(旋转)，而不改变其形状，可用两个变量来描述：旋转矩阵R和平移向量t
$R,t$称为相机的外参(Extrinsic)，相机外参决定了空间点从世界坐标系转换到相机坐标系的变换，也可以说外参描述了相机在世界坐标系中的位置和朝向。
$$P^{{}^{\prime }}=Rp+t=[X^{{}^{\prime }},Y^{{}^{\prime }},Z^{{}^{\prime }}{]}^T$$

$$\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ Z^{{}^{\prime }}\\ 1\end{array}\right]$$
(2)透视投影
我们可以将透镜的成像简单地抽象成下图所示：
!

设 $f=OB$表示相机的焦距， $n=OA$表示物距， $m=OC$表示像距,有
$$\frac{1}{f}=\frac{1}{m}+\frac{1}{n}$$
一般由于物距远大于焦距， $n>>f$，所以 $m\approx f$，可以用小孔模型代替透镜成像模型（后面假设所有的相机模型为小孔成像模型，实际上）：

!

根据相似三角形（现在你需要把上面的图想象成一个立体的形状）可以得到：
$$\{\begin{array}{l}\frac{X}{X^{{}^{\prime }}}=\frac{Z}{f}\\ \frac{Y}{Y^{{}^{\prime }}}=\frac{Z}{f}\end{array}$$
进一步简化便于写成矩阵形式（大家都爱写矩阵形式）：
$$\{\begin{array}{l}{X}^{{}^{\prime }}=f\frac{X}{Z}\\ Y^{{}^{\prime }}=f\frac{Y}{Z}\end{array}$$
写为矩阵形式为：
$$Z\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$$

至此，用$[u_c,v_c]$代替$[X^{{}^{\prime }}，Y^{{}^{\prime }}]$表示在相机归一化平面的坐标。(c表示camera)
(3)在相机坐标系进行畸变校正
通过切向畸变参数$p_1,p_2$和径向畸变参数$k_1,k_2$进行去畸变。
$$\{\begin{array}{l}{u^{{}^{\prime }}}_c=u_c(1+k_1{r_c}^2+k_2{r_c}^4)+2p_1{u}_c{v}_c+p_2(r^2+2{u_c}^2)\\ {v^{{}^{\prime }}}_c=v_c(1+k_1{r_c}^2+k_2{r_c}^4)+2p_1(r^2+2{v_c}^2)+p_2{u}_c{v}_c\end{array}$$
其中：$r=\sqrt{{u_c}^2+{v_c}^2}$
(4)矫正后的点可以通过内参矩阵投影到像素平面上

数字化图像：
$[u_i,v_i]$表示图像上的一个点的像素坐标（i表示image）
PS:这里的$f_x,f_y,c_x,c_y,k_1,K_2,k_3,p_1,p_2$表示相机内部参数。是我们需要标定的。一旦相机结构固定，包括镜头结构固定，对焦距离固定，我们就可以用这9个的参数去近似这个相机。这里说的「镜头结构固定」，按我个人的理解，除了焦距固定之外，也应当包含光圈固定，因为改变光圈的大小，除了景深之外，是有可能改变针孔相机模型中的光心位置，但是影响并不是很大。这意味着标定好的相机如果改变光圈大小，会使得标定误差变大但应该不会大到难以接受的地步。
$$\{\begin{array}{l}{u}_i=f_x{u^{{}^{\prime }}}_c+c_x\\ v_i=f_y{v^{{}^{\prime }}}_c+c_y\end{array}$$

BA求解

相机模型和路标的优化表示

现在我们把：
外参$R,t$用李代数$\xi$表示，
像素坐标$(u_i,v_i)$用$z$表示，
三维世界的路标点$[X,Y,Z{]}^T$用$p$表示，
观测误差可以表示为：
$$e=z-h(\xi ,p)$$
其中$h()$为上面介绍的相机观测模型函数。

代价函数表示为：
$$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n{e}_{ij}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n[z_{ij}-h({\xi }_i,p_j){]}^2$$
上面式子的物理意义是在${\xi }_i$处观察路标$p_j$得到的数据为$z_{ij}$
我们的目标是优化$[\xi ,p]$，使得满足如下关系：
$$[\hat{\xi },\hat{p}{]}^T=arg\underset{\xi ,p}{\min }\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n[z_{ij}-h({\xi }_i,p_j){]}^2$$
在实际的slam过程中，我们需要优化局部的轨迹里面的多个位姿和局部地图点，所以这里进一步将$[\xi ,p]$表示为如下：
$$x=[{\xi }_1,...{\xi }_m,，p_i,...p_n{]}^T$$
相应的增量方程表示为
$$\frac{1}{2}{\parallel f(x+\Delta x)\parallel }^2\approx \parallel e_{ij}+F_{ij}\Delta {\xi }_i+E_{ij}\Delta p_i{\parallel }^2$$
令$x_c=[{\xi }_1,{\xi }_2,...,{\xi }_m{]}^T,x_p=[p_1,p_2,...,p_n{]}^T$,
$$\frac{1}{2}\parallel f(x+\Delta x){\parallel }^2=\frac{1}{2}\parallel e+F\Delta x_c+E\Delta x_p{\parallel }^2$$
令雅克比矩阵$J=\left[\begin{array}{cc}F& E\end{array}\right]$
以高斯牛顿法为例子，H矩阵为
$$H=J^{T}J=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ E^{T}F& E^{T}E\end{array}\right]$$
当需要优化很多位姿和地图点时，我们需要计算每一个雅克比子矩阵，即$J_{ij}$，这里我们讨论一下每一个雅克比的子矩阵的数学推算。
$$J_{ij}(x)=(0_{2\times 6},...,0_{2\times 6},\frac{\partial e_{ij}}{\partial {\xi }_i},0_{2\times 6},...,0_{2\times 3},\frac{\partial e_{ij}}{\partial p_j},...,0_{2\times 3},...)$$
上式的物理含义为第i个相机位姿观测到第j个路标点。其余部分的导数都为0。
接下来我们具体介绍雅克比矩阵的推导。

雅克比矩阵推导方法

从前面的相机模型我们可以已知世界坐标系下的坐标P，相机坐标系下的坐标$P^{{}^{\prime }}$，对应图像像素坐标系的点$(u,v)$，优化相机位姿${\xi }_i$和世界坐标系下的路标$P_j$。
这里已有的公式有
$$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ Z^{{}^{\prime }}\end{array}\right]$$
简化得到：
$$u=f_x\frac{X^{{}^{\prime }}}{Z^{{}^{\prime }}}+c_x{v}_{=}{f}_y\frac{Y^{{}^{\prime }}}{Z^{{}^{\prime }}}+c_y$$
首先我们求$\frac{\partial e_{ij}}{\partial {\xi }_i}$,当我们求误差时，可以把$u,v$与实际的测量值相互比较，求差，其中误差$e$是二维向量，定义了中间变量后，对${\xi }^{\wedge }$求左乘扰动$\delta \xi$,然后考虑误差的变化对扰动量的导数。
$$\frac{\partial e}{\partial \delta \xi }=\frac{\partial e}{\partial P^{{}^{\prime }}}\frac{\partial P^{{}^{\prime }}}{\partial \delta \xi }$$
根据：
$$su=K{P}^{{}^{\prime }}$$得
$$\frac{\partial e}{\partial P^{{}^{\prime }}}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{{}^{\prime }}}& \frac{\partial u}{\partial Y^{{}^{\prime }}}& \frac{\partial u}{\partial Z^{{}^{\prime }}}\\ \frac{\partial v}{\partial X^{{}^{\prime }}}& \frac{\partial vv}{\partial Y^{{}^{\prime }}}& \frac{\partial v}{\partial Z^{{}^{\prime }}}\end{array}\right]=-\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]$$
$$\frac{\partial TP}{\partial \delta \xi }=\left[\begin{array}{cc}I& -P^{{}^{\prime }\wedge }\\ 0^T& 0^T\end{array}\right]$$

第一项是一个2x3的矩阵，第二项是一个3*6的矩阵，最后乘起来是一个2x6矩阵。
$$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\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(1+\frac{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{X}^{{}^{\prime }}{Y}^{{}^{\prime }}}{Z^{{}^{\prime }2}}& \frac{f_y{X}^{{}^{\prime }}}{Z^{{}^{\prime }}}\end{array}\right]$$
除了位姿优化外，还需要得到特征点的空间位置的优化，

$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{{}^{\prime }}}\frac{\partial P^{{}^{\prime }}}{\partial P}$$
第一项已经推导出来了，第二项为：
$$P^{{}^{\prime }}=exp({\xi }^{\wedge })P=RP+t$$所以：

$$\frac{\partial P^{{}^{\prime }}}{\partial P}=R$$
$$\frac{\partial e}{\partial 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]R$$
如果使用g2o的话上面已经全部写好了。区别就是g2o
对应的C++代码如下：

PS:上面是高博的视觉十四讲的内容（书上第169页）
_jacobianOplusXi是误差到空间点的导数，
_jacobianOplusXj是误差到相机位姿的导数。
和前面公式的区别就是这里面采用$f$来统一描述$f_x,f_y$,并且李代数的定义顺序也不同（g2o是旋转在前，平移在后）
我把每行对应的元素都写在了后面。

至此，把雅克比矩阵中每个子矩阵的表示方法就讲完了。

稀疏性和边缘化实现

$$H=J^{T}J=\left[\begin{array}{cc}{F}^{T}F& F^{T}E\\ E^{T}F& E^{T}E\end{array}\right]=\sum _{i,j}{J}_{ij}^T{J}_{ij}$$
把H矩阵进行分块，H矩阵还可以表示为：
$$H=\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]$$
高博十四讲的P251页的图很好的表示了这个矩阵对应的相机位姿和路标点之间的关系。
我的总结是:

1. $H_{11}$的对角线的每个矩阵块（注意不是元素）是误差关于相机位姿的雅克比矩阵的平方，为6x6的矩阵块，矩阵块的个数为相机位姿的数量。对应图中的相机顶点。是一个对角矩阵。
2. $H_{21}$和$H_{12}$每个子矩阵对应的是误差关于路标$P$的雅克比矩阵（2x3大小）的转置矩阵（3x2大小）与误差关于相机位姿的雅克比矩阵（2x3大小）的乘积得到的矩阵（3x6大小）。并且他们之间互为转置矩阵。对应到图中的话是位姿顶点到路标顶点的边。
3. $H_{22}$对角线每个子矩阵对应的是误差关于路标的雅克比矩阵的平方（3x2与2x3矩阵的乘积为3x3的矩阵）。对应到图中的话是路标的顶点。是一个对角矩阵。
4. 通过上面的每个子矩阵块以及图中顶点与边的表示，可以得到$H$矩阵。

高博的十四讲P252页 把这四个矩阵用$B,E,E^T,C$来分别表示$H$的每个大块。（这里面很多的矩阵块很容易搞晕~）
可以表示为如下：
$$\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]$$
接下来的指导思想是：
矩阵求逆的计算量比较大，所以尽量避免矩阵求逆，而且对角块矩阵求逆相对容易一些。所以可以采用高斯消元的思想去把$H_{12}$或$H_{21}$某一个消去，然后回来求解$\Delta x_c$，$\Delta x_p$。这个过程就叫做Marginalizetion或者Schur消元。下面是边缘化的数学推理过程：
$$\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]$$

$$\{\begin{array}{l}\left[\begin{array}{c}B-E{C}^{-1}{E}^T\end{array}\right]\Delta x_c=v-E{C}^{-1}w\\ \Delta x_p=c^{-1}(w-E^T\Delta x_c)\end{array}$$
高博的十四讲在P254和P255页还讨论了$\Delta x_c$的系数$S$矩阵的稀疏性在不同slam实践当中的区别以及S矩阵中共视的物理意义，总体来说第十讲讲的非常具体。