ORB_SLAM2代码阅读(5)——Bundle Adjustment
ORB_SLAM2代码阅读（5）——Bundle Adjustment
- 1. 说明
- 2. Bundle Adjustment(BA)的物理意义
- 3. BA的数学表达
- 4. BA的求解方法
- 4.1 最速下降法
- 4.2牛顿法
- 4.3高斯牛顿法
- 4.4LM法
- 4.4 优化方法总结
1. 说明
前几篇博客介绍了一下ORB_SLAM2的Tracking、LocalMapping 和 LoopClosing三大线程。本篇博客介绍一下 Bundle Adjustment（简称BA）。本来介绍BA可以单独写一篇文章，不必放在ORB_SLAM2代码阅读系列，但是由于ORB_SLAM2中的用到了这种技术，所以我也就把这部分放到了ORB_SLAM2代码阅读系列中。本文将从BA的物理意义、数学表达、求解手段、求解工具和BA在ORB_SLAM2中的应用这几方面来进行介绍。
2. Bundle Adjustment(BA)的物理意义
Bundle Adjustment 中文译为光束法平差。当然，它也有其他的中文译法，比如束调整、捆集调整或者捆绑调整等等。其中bundle，来源于bundle of light，其本意就是指的光束，这些光束指的是三维空间中的点投影到像平面上的光束。而平差则是测量学中的概念，指的是：由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。为了提高成果的质量，处理好这些测量中存在的误差问题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。
说的简单一点： BA的本质是一个优化模型，其目的是减小重投影误差 。
我们可以用一个模型来表述BA的过程。
这个图形经常在各种介绍BA的文章中提到，我也在此使用一下这个图。
从图中我们可以看到，长方体的各个顶点分别投影到了 P 1 P_{1} P1 、 P 2 P_{2} P2 和 P 3 P_{3} P3代表的图像上。这就是一个普通的投影过程，并没有什么特殊的地方。但是在SLAM当中，前端的视觉里程计会计算出相邻两幅图像之间的变换关系和地图点的空间位置。放到上面这幅图中就是，我们可以计算出 P 1 P_{1} P1 、 P 2 P_{2} P2 和 P 3 P_{3} P3之间的变换关系和长方体各个定点的空间坐标，但是，不可否认的是我们的计算结果和真实值之间是存在差异的。由于计算误差的存在，长方体各个顶点的空间坐标与真实坐标并不会完全重合。既然我们计算出的顶点坐标与真实坐标存在差异，那么将我们计算出的顶点位置再次用投影模型投影到图像上（二次投影，重投影）得到的投影坐标与顶点真实位置的投影（第一次投影）坐标之间不可避免的存在差异，这个误差我们称之为 重投影误差 。
从上面的过程中我们可以知道，造成重投影误差的原因是我们计算的相机之间的变换关系和地图点的空间位置不准确造成。所以如果我们能够减小重投影误差，那么我们就能优化相机的位姿和地图点的位置。这正是SLAM后端优化部分要做的事情。
那么，如何能够减小重投影误差就是BA要做的事情了。
3. BA的数学表达
投影过程可以分为以下几个步骤：
- 首先，把世界坐标转换到相机坐标，这里将用到相机外参数 ( R ; t ) (R; t) (R;t)：
P ′ = R p + t = [ X ′ , Y ′ , Z ′ ] T P'=Rp+t =[X',Y',Z']^T P′=Rp+t=[X′,Y′,Z′]T - 然后，将 P ′ P' P′投至归一化平面，得到归一化坐标：
P c = [ u c ; v c ; 1 ] T = [ X ′ / Z ′ ; Y ′ / Z ′ ; 1 ] T P_{c} = [u_{c}; v_{c}; 1]^T = [X'/Z'; Y'/Z'; 1]^T Pc=[uc;vc;1]T=[X′/Z′;Y′/Z′;1]T - 对归一化坐标去畸变，得到去畸变后的坐标。这里暂时只考虑径向畸变：
- 最后，根据内参模型，计算像素坐标：
以上四步可以用一个方程表示，即
z = h ( x , y ) z = h(x,y) z=h(x,y)
其中 ， z z z 表示像素坐标 ( u s , v s ) (u_s,v_s) (us,vs)， x x x 表示外参 ( R , t ) (R,t) (R,t)， y y y 表示空间点 p p p。
那么重投影误差我们就可以表示为：
e = z ^ − h ( x , y ) e = \hat{z}-h(x,y) e=z^−h(x,y)
其中 z ^ \hat{z} z^表示重投影坐标
然后，把其他时刻的观测量也考虑进来，我们可以给误差添加一个下标。那么整体的误差函数为：
1 2 ∑ x = 1 m ∑ y = 1 n ∣ ∣ e i j ∣ ∣ 2 = 1 2 ∑ x = 1 m ∑ y = 1 n ∣ ∣ z i j ^ − h ( x i , y j ) ∣ ∣ 2 \frac{1}{2} \sum^{m}_{x = 1}\sum^{n}_{y=1}{||e_{ij}||^{2}} = \frac{1}{2} \sum^{m}_{x = 1}\sum^{n}_{y=1}{||\hat{z_{ij}}-h(x_i,y_j)||^{2}} 21x=1∑my=1∑n∣∣eij∣∣2=21x=1∑my=1∑n∣∣zij^−h(xi,yj)∣∣2
这是一个最小二乘问题，对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA。
4. BA的求解方法
我们在前面说过，BA是一个优化模型。既然是优化模型，就应该用用各种优化算法来进行计算。我们先来看看各种常用的优化算法。
4.1 最速下降法
如果对梯度比较熟悉的话，那应该知道梯度方向是函数上升最快的方向，而此时我们需要解决的问题是让函数最小化。你应该想到了，那就顺着梯度的负方向去迭代寻找使函数最小的变量值。梯度下降法就是用的这种思想，用数学表达：
x k = x k − 1 − λ Δ f ( x k − 1 ) x_k = x_{k-1} - \lambda\Delta f(x_{k-1}) xk=xk−1−λΔf(xk−1)
其中 λ \lambda λ为步长。最速下降法保证了每次迭代函数都是下降的，在初始点离最优点很远的时候刚开始下降的速度非常快，但是最速下降法的迭代方向是折线形的导致了收敛非常非常的慢。
4.2牛顿法
现在先回顾一下中学数学，给定一个开口向上的一元二次函数，如何知道该函数何处最小？这个应该很容易就可以答上来了，对该函数求导，导数为0处就是函数最小处。
Newton型方法也就是这种思想，首先将函数利用泰勒展开到二次项：
f ( x + δ x ) = ψ ( δ x ) = f ( x ) + J ( x ) δ x + 1 2 δ x T H ( x ) δ x f(x+\delta x) =\psi (\delta x)= f(x)+J(x)\delta x + \frac{1}{2}\delta x^TH(x)\delta x f(x+δx)=ψ(δx)=f(x)+J(x)δx+21δxTH(x)δx
其中 J J J为Jacobi矩阵，对矩阵函数求一次偏导而来，梯度也是对向量函数求一次偏导而来。将标量考虑为1x1的矩阵，将向量考虑nx1的矩阵，其实这些求导都是求Jacobi矩阵。 H H H为Hessian矩阵，也就是二次偏导矩阵。
也就是说牛顿方法将函数局部近似成一个二次函数进行迭代，然后令 x x x在方向上迭代直至收敛，接下来自然就对这个函数求导了：
ψ ′ ( δ x ) = J ( x ) + H ( x ) δ x = 0 \psi '(\delta x) = J(x) + H(x)\delta x = 0 ψ′(δx)=J(x)+H(x)δx=0
可得
δ x = − H − 1 J \delta x = -H^{-1}J δx=−H−1J
牛顿型方法收敛的时候特别快，尤其是对于二次函数而言一步就可以得到结果。但是该方法有个最大的缺点就是Hessian矩阵计算实在是太复杂了，并且牛顿型方法的迭代并不像最速下降法一样保证每次迭代都是下降的。
4.3高斯牛顿法
既然牛顿法计算Hessian矩阵太困难了，那有没有什么方法可以不计算Hessian矩阵呢？将泰勒展开式的二次项也去掉好像就可以避免求Hessian矩阵了吧，就像这样：
f ( x + δ x ) = f ( x ) + J ( x ) δ x f(x+\delta x) = f(x)+J(x)\delta x f(x+δx)=f(x)+J(x)δx
这好像变成了一个线性函数了啊，线性函数如果要最小化的话好像是需要增加其他的约束条件的啊。那这里有没有其他的约束条件呢？仔细思考一下，我们需要最小化的是重投影误差，它的最小值是什么呢？理想状态下当然是等于0了。所以这个时候就不应该求导了，而是直接令函数为0。此时，令 f ( x ) = ϵ f(x) =\epsilon f(x)=ϵ 有
ϵ + J ( x ) δ x = 0 \epsilon+J(x)\delta x = 0 ϵ+J(x)δx=0
即
J T J δ x = − J T ϵ J^TJ \delta x = -J^T \epsilon JTJδx=−JTϵ
x = x + δ x x =x+ \delta x x=x+δx
由此x在 δ x \delta x δx方向上迭代直至 ∣ ∣ ϵ ∣ ∣ ||\epsilon|| ∣∣ϵ∣∣最小。
高斯牛顿法就避免了求Hessian矩阵，并且在收敛的时候依旧很快。但是依旧无法保证每次迭代的时候函数都是下降的。
4.4LM法
LM方法就是在以上方法基础上的改进，通过参数的调整使得优化能在最速下降法和高斯牛顿法之间自由的切换，在保证下降的同时也能保证快速收敛。高斯牛顿法最后需要求解的方程为
J T J δ x = − J T ϵ J^TJ \delta x = -J^T \epsilon JTJδx=−JTϵ
LM算法在此基础上做了更改，变成了
( J T J + λ I ) δ x = − J T ϵ (J^TJ+\lambda I) \delta x = -J^T \epsilon (JTJ+λI)δx=−JTϵ
通过参数 λ \lambda λ 的调节在最速下降法和高斯牛顿法之间切换。做个不很数学的直观分析吧，当 λ \lambda λ很小时，显然和高斯牛顿法是一样的；当 λ \lambda λ很大时，就变成了这样：
λ I δ x = − J T ϵ → δ x = − λ − 1 J T ϵ \lambda I \delta x = -J^T \epsilon \rightarrow \delta x = -\lambda ^{-1}J^T \epsilon λIδx=−JTϵ→δx=−λ−1JTϵ
这和最速下降法的形式一致。
这里还存在一个问题，当 λ \lambda λ 取某个值的时候可能会导致 J + λ I J+\lambda I J+λI 不可逆，所以这里变成了
( J T J + λ d i a g ( J T J ) ) δ x = − J T ϵ (J^TJ+\lambda diag(J^TJ)) \delta x = -J^T \epsilon (JTJ+λdiag(JTJ))δx=−JTϵ
其实LM算法的具体形式就笔者看到的就有很多种，但是本质都是通过参数 λ \lambda λ在最速下降法和高斯牛顿法之间切换。
LM算法就由此保证了每次迭代都是下降的，并且可以快速收敛。
4.4 优化方法总结
| 算法名 | 特点 |
|---|---|
| 最速下降法 | 每次迭代函数都是下降的，在初始点离最优点很远时下降速度非常快，但是由于迭代方向是折线形的导致收敛非常慢 |
| 牛顿法 | 收敛速度快，但是Hessian矩阵计算复杂，并不是每次迭代都是下降的 |
| 高斯牛顿法 | 高斯牛顿法就避免了求Hessian矩阵，并且在收敛的时候依旧很快。但是依旧无法保证每次迭代的时候函数都是下降的 |
| LM法 | 保证了每次迭代都是下降的，并且可以快速收敛 |
未完待续。。。。。