SLAM笔记（五）光束平差法(BundleAdjustment)

1.什么是光束平差法

前边的八点法&＃xff0c;五点法等可以求出闭式解的前提是已经知道确切的点对。但实际情况中往往存在大量的噪声&＃xff0c;点与点不是精确地对应甚至出现一些错误匹配。 
光束平差法由Bundle Adjustment翻译得来&＃xff0c;有两层意思&＃xff1a; 
对场景中任意三维点P&＃xff0c;由从每个视图所对应的的摄像机的光心发射出来并经过图像中P对应的像素后的光线&＃xff0c;都将交于P这一点&＃xff0c;对于所有三维点&＃xff0c;则形成相当多的光束&＃xff08;bundle&＃xff09;&＃xff1b;实际过程中由于噪声等存在&＃xff0c;每条光线几乎不可能汇聚与一点&＃xff0c;因此在求解过程中&＃xff0c;需要不断对待求信息进行调整&＃xff08;adjustment&＃xff09;&＃xff0c;来使得最终光线能交于点P。对m帧&＃xff0c;每帧含N个特征点的目标函数如下&＃xff1a; 
 
&＃xff08;1&＃xff09; 
其中&＃xff1a;表示受白噪声影响的估计二维点坐标&＃xff0c;为投影函数&＃xff0c;ruguo 如果点j出现在图i上&＃xff0c;则&＃xff0c;否则。 
这是一个非凸问题。 
式子&＃xff08;1&＃xff09;表示对所有点 
以上便是光束平差法目标函数的原理。由于场景中特征点往往较多&＃xff0c;该问题是一个巨大的高维非线性优化问题。接下来&＃xff0c;需要对上述式子进行求解&＃xff0c;这是光束平差法的核心内容。 
针对具体应用场景&＃xff0c;光束平差法有不同收敛方法。目前常用的方法有梯度下降法&＃xff0c;牛顿法&＃xff0c;高斯牛顿法&＃xff0c;Levenber-Marquardt等方法。

2.1 一阶方法——梯度下降法

所谓一阶方法&＃xff0c;即对问题的目标函数进行泰勒一阶展开后进行迭代求解的方法。梯度下降法是一阶方法之一。当梯度为负值时&＃xff0c;沿着梯度方向就是函数值f变小最快的方向。梯度下降法就是让函数沿着下降最快的方向去找函数值的最小值&＃xff0c;就像水流沿着斜率最大的方向流去。对于变量都为标量的函数&＃xff0c;形象的描述是始终用一条直线来拟合曲线。梯度下降法迭代式子如下&＃xff1a;

(2)

其中&＃xff0c;ϵ表示自己设置的迭代步长&＃xff0c;可用一维线性搜索动态确定。x表示自变量。

严格意义上&＃xff0c;梯度下降法并不决定函数f(x)下降方向&＃xff0c;因为它仅仅是一个余向量而非向量&＃xff0c;只能通过最终标量的正负而非实际的向量指引函数下降方向。梯度下降法的复杂度是Ο(n)&＃xff0c;其中n为待解决问题的大小&＃xff0c;比如矩阵E的行数。实际过程中&＃xff0c;常常使用一维线性搜索方法来寻找合适的步长。

2.2 二阶方法——牛顿法&＃xff08;Newton Method&＃xff09;

牛顿法是二阶优化方法&＃xff0c;即会将目标函数展开至泰勒二阶项然后进行优化求解。与梯度法相比&＃xff0c;它们利用到了目标函数的二阶导数。形象地讲&＃xff0c;如用牛顿法求解自变量为标量的函数时&＃xff0c;用二次曲线来拟合最优化点时的函数曲线。

对目标函数E&＃xff0c;其二阶泰特展开式为&＃xff1a;

其中g为E的雅克比矩阵&＃xff0c;H为E的海塞矩阵。

由于优化点的导数为0&＃xff0c;即&＃xff1a;

上式展开&＃xff0c;易知x的迭代式子为&＃xff1a;

由于牛顿法下降速度很快。实际中往往加上一个步长因子γϵ(0,1)&＃xff0c;来控制收敛的速度:

牛顿法是二阶收敛的&＃xff0c;收敛速度很快。在实际应用中&＃xff0c;向量x往往非常大&＃xff08;每个视图中图像处理后特征点数量可能达到万个以上&＃xff09;&＃xff0c;海森矩阵H将非常大&＃xff0c;求海塞矩阵的逆的运算消耗将非常大&＃xff0c;对于牛顿法来说&＃xff0c;计算复杂度是O(n3)。此外&＃xff0c;由于海塞矩阵不一定可逆。其三&＃xff0c;对于大多数一阶优化方法&＃xff0c;可以采用诸如图形处理器&＃xff08;Graphics Processing Unit&＃xff09;并行的方式来加速&＃xff0c;但对于海塞矩阵求逆来说这显然无法实现。因此实际中往往出现一阶方法比二阶方法更快收敛。

2.3 拟牛顿法——高斯牛顿法&＃xff08;Gauss-Newton Method&＃xff09;

所谓拟牛顿法&＃xff0c;就是用其他式子来模拟替代海塞矩阵。假如牛顿法中的海塞矩阵不是正定&＃xff08;positive definitive&＃xff09;的&＃xff0c;无法求解&＃xff1b;此外&＃xff0c;海森矩阵H往往非常大&＃xff0c;求海塞矩阵的逆的运算消耗也很大&＃xff08;对于牛顿法来说&＃xff0c;计算复杂度是O(n3)&＃xff09;&＃xff0c;因此&＃xff0c;引入用拟牛顿法来用正定矩阵代替海塞矩阵和海塞矩阵的逆。常用的拟牛顿法有高斯牛顿法&＃xff08;Gauss-Newton Method&＃xff09;。

假设最小二乘问题目标函数如下&＃xff1a; 

其中ri(x)是对应观测值与预测值之间的残差。

仿照2.3可以得到牛顿法中的迭代式子&＃xff08;10&＃xff09;。不过其中梯度矩阵g:

海塞矩阵H:

如果对于一个近线性的优化问题&＃xff0c;则上式第二项更趋近于0&＃xff0c;因此舍弃第二项&＃xff0c;上式为&＃xff1a;

则&＃xff1a;

也即有&＃xff1a;

由于采用 &＃xff0c;计算量大大减少

应当特别指出&＃xff0c;上式成立的条件是。在结构与运动过程中&＃xff0c;由于一般认为到场景位置点的距离比较远的&＃xff0c;因此短暂的移动过程中&＃xff0c;可以认为从摄像机到场景位置点的距离是近似不变的。在距离不变&＃xff0c;也就是一个维度固定的前提下&＃xff0c;投影函数π是线性的。因此该近似符合应用场景&＃xff0c;是很好的近似。

2.4 Levenberg-Marquardt方法

另外一种思路是将牛顿法和梯度法融合在一起。数学上是阻尼最小二乘法的思路&＃xff0c;即近似只有在区间内才可靠。对于

此处μ是信赖区间半径&＃xff0c;D为对∆进行转换的矩阵&＃xff08;在Levenberg的方法中&＃xff0c;他将D设置为单位矩阵&＃xff09;。

即加上一个单位矩阵I的倍数和使之成为&＃xff1a;

这种方法时保证改进后的海塞矩阵可逆且正定。从效果上&＃xff0c;是用λ在牛顿法与梯度法之间做出权衡。当λ很小时&＃xff0c;上式几乎等同与牛顿法式子&＃xff0c;当λ很大时&＃xff0c;上式等同于梯度下降法的式子。

后来&＃xff0c;Levenberg&＃xff08;1944&＃xff09;对此方法进行了改进。他将H替换成高斯牛顿法中的拟合矩阵&＃xff1a;

其中&＃xff1a; &＃xff08;15&＃xff09;

但容易出现的问题是&＃xff0c;当 很小的时候&＃xff0c;λI可能很大。这样会极大地偏向梯度法&＃xff0c;降低收敛速度。因此为了提高收敛速度&＃xff0c;Marquardt 提出了一种新的自适应方法&＃xff1a;它的迭代式子中&＃xff1a;

因此当很小时&＃xff0c;该方法也不会特别偏向梯度法。

在L-M方法中&＃xff0c;采用了近似程度描述ρ

即ρ&＃61;实际下降/近似下降。当ρ太大&＃xff0c;则减少近似范围&＃xff08;增大λ&＃xff09;&＃xff0c;当p太大&＃xff0c;则增加近似范围&＃xff08;减少λ&＃xff09;。

因此最常使用的光束平差法模型&＃xff0c; L-M算法计算步骤如下&＃xff1a;

a)给定初始值&＃xff1b; 
b)对第k次迭代&＃xff0c;求解 
c)计算ρ 
d)若ρ>0.75&＃xff08;经验值&＃xff09;&＃xff0c;则μ&＃61;2μ&＃xff08;经验值&＃xff0c;实际可视作变化迭代步长&＃xff09;; 
若ρ<0.25&＃xff08;经验值&＃xff09;&＃xff0c;则μ&＃61;0.5μ&＃xff08;经验值&＃xff09;; 
e)如果ρ大于某阈值&＃xff0c;则该次近似是可行的&＃xff0c;回到b&＃xff09;继续迭代&＃xff1b;否则算法已经收敛&＃xff0c;迭代结束。

2.5与高斯牛顿法相比LM算法的优缺点

高斯牛顿法的缺点是&＃xff1a;

• 可能是奇异的病态的&＃xff0c;无法保证求解的增量的稳定性&＃xff1b;
• 步长可能很大从而导致无法满足高斯牛顿的一阶能大致拟合的假设 
  优点&＃xff1a;
• 下降速度比LM快

LM是信赖区域优化法的代表&＃xff0c;而加上步长的高斯牛顿法是线搜索方法的代表。

3 关于光束平差法的其他问题

&＃xff08;1&＃xff09;初始值

光束平差法需要比较好的初始值才能比较快地收敛&＃xff0c;所以光束平差法一般作为重建流水线的最后一个步骤&＃xff0c;在此之前&＃xff0c;需要使用多视图几何中传统的八点法&＃xff0c;五点法等传统多视图几何算法先算出R,T等信息。

&＃xff08;2&＃xff09;步长控制

引入步长控制&＃xff0c;既可以是避免收敛时步长太大而在最优点附近震荡&＃xff0c;也可以加快收敛速度。加入迭代步长的原因&＃xff0c;是因为 牛顿法中 下降方向 可能和真实下降方向不一致* 。*比如可能会出现几个最优点相邻比较近的情况&＃xff0c;那么优化过程将在几个谷底之间跳来跳去迟迟不收敛。为了避免这种情况&＃xff0c;增加收敛速率&＃xff0c;加入一个迭代步长γ&＃xff0c;来使迭代朝着真实下降方向走。如果在鞍点&＃xff0c;在沿着海塞矩阵为负的方向迭代。实际应用中&＃xff0c;可以采用每一次迭代后&＃xff0c;再对γ进行一维搜索的方法来寻找合适步长。也可以采用L-M的方法&＃xff0c;通过改变信任区间的方式&＃xff0c;来进行步长控制。 
4.常用优化库&＃xff1a; 
常用BA库&＃xff1a; 
sba: A Generic Sparse Bundle Adjustment C/C&＃43;&＃43;

Apero/MicMac, a free open source photogrammetric software. Cecill-B licence.

Package Based on the Levenberg–Marquardt Algorithm (C, MATLAB). GPL.

cvsba: An OpenCV wrapper for sba library (C&＃43;&＃43;). GPL.

ssba: Simple Sparse Bundle Adjustment package based on the Levenberg–Marquardt Algorithm (C&＃43;&＃43;). LGPL.

OpenCV: Computer Vision library in the contrib module. BSD license.

mcba: Multi-Core Bundle Adjustment (CPU/GPU). GPL3.

libdogleg: General-purpose sparse non-linear least squares solver&＃xff0c;based on Powell’s dogleg method. LGPL. 
ceres-solver: A Nonlinear Least Squares Minimizer. BSD license. 
g2o: General Graph Optimization (C&＃43;&＃43;) - framework with solvers for sparse graph-based non-linear error functions. LGPL.

DGAP: The program DGAP implement the photogrammetric method of bundle adjustment invented by Helmut Schmid and Duane Brown. GPL. 
工程上常用的是g2o和ceres-solver。此上列表来源不可考&＃xff0c;如有侵权请联系我以删除。