ORB-SLAM2从理论到代码实现（九）：Bundle Ajustment(上)重投影、非线性优化

本人邮箱[email protected],欢迎交流！

Bundle Adjustment可以翻译为光束法平差、束调整、捆集调整或者捆绑调整等等。但是我觉得还是英文来得更直观，下文统一简写为BA。它的本质是一个优化模型，其目的是最小化重投影误差。

1.重投影

相机在拍照时会把空间点投影到图像上，这是第一次投影。我们利用这些图像进行特征匹配，然后对匹配的特征点进行三角化，得到三维空间点位置（如上图中的）。最后我们利用计算得到的三维点的坐标（估计值，具有误差）和我们计算得到的相机矩阵（具有误差）进行第二次投影，也就是重投影。

这里我们把重投影误差表示为：

 

BA的目的就是优化的值以及变换矩阵T，使上式最小。

2. 非线性优化

2.1 非线性最小二乘

假设为非线性函数，最小二乘问题表示为,如果，可以解出该方程，则解可能为极大值、极小值或鞍点处的值。只要比较函数值大小就可得出所求x。

如果方程不容易求解，我们只能用迭代的方法求解。下图为迭代过程。

步骤如下：

1. 给定某个初始值 。 

2. 对于第 k 次迭代，寻找一个增量 ，使得  达到极小值。 

3. 若 足够小，则停止。 

4. 否则，令  ，返回 第2步。

这让求解导函数为零的问题，变成了一个不断寻找梯度并下降的过程。 

怎么确定？看以下三种方法。

2.2 一阶和二阶梯度法

为目标函数，将其在x附近泰勒展开

这里 J 是 关于 x 的导数（雅可比矩阵），而 H 则是二阶导数（海塞（Hessian） 矩阵） 

如果保留二阶梯度信息，那么增量方程为：

对上式右侧求关于 的导数，令其为0，得

 由于H矩阵不好计算，所以该方法并不实用，常用得是下面两种方法。

2.3 高斯牛顿法Gaussian Newton

将 f(x) 进行一阶的泰勒展开（这里目标函数不再是f（x）的平方）：

J（x）是f（x）关于x的导数，我们的目的是找到下降矢量让达到最小。

可得：

展开目标函数，可得 

 

这里 关于求导得

 记为

步骤：

2.4 列文伯格-马夸尔特法 Levenberg-Marquardt

上面都提到足够小，但是多少算足够小没有说明，因此我们给其加一个信赖区域

ρ 的分子是实际函数下降的值，分母是近似模型下降的值。如 果 ρ 接近于 1，则近似是好的。如果 ρ 太小，说明实际减小的值远少于近似减小的值，则 认为近似比较差，需要缩小近似范围。反之，如果 ρ 比较大，则说明实际下降的比预计的 更大，我们可以放大近似范围。 （高翔书中原话，我觉得挺好理解的）

 

 

 

发现两篇博文写的特别好：

https://blog.csdn.net/jinshengtao/article/details/53310804

https://blog.csdn.net/zhubaohua_bupt/article/details/74973347