Bundle Adjustment(后面简称BA),直译就是捆绑调整,也有翻译成光束法平差(这个翻译貌似是摄影测量学里的)的,总感觉这两个翻译怪怪的,不过从字面上看,就是捆绑到一起,然后调整。那么问题就来了,要把什么捆绑到一起,怎么调整,要调整到什么状态。

捆绑些啥

先来回顾下基于图像的三维重建的流程:

  1. 拍摄图像
  2. 提取特征
  3. 特征匹配
  4. 姿态估计
  5. 三角化计算三维点

理想情况下,也就是相机没有畸变,特征匹配完全准确、姿态估计完全准确的情况下,三维坐标应该也是完全准确的。但是现实总是跟理想有差距,几乎所有相机都存在畸变,相机标定也只是估计出了相机的内参,而不是计算出了完全准确的值,因此畸变校时就存在误差;特征匹配也只是找到了最相似的两个点,并不能完全保证这两个点就是完全匹配的,这也是误差来源之一;由于有了之前的误差,从相机出发经过图像上的对应点的两条本应相交的射线也不一定相交,不相交就没法计算三维坐标,就只能估计一个三维坐标,这同样存在误差;

这样一看,几乎每一步都存在误差,可是我们总是希望能够得到准确的三维模型,很明显需要进行优化,单独每一步来看貌似没有太大的优化空间,那就全弄到一起优化就完了嘛。也就这个捆绑就捆绑了:相机姿态、三维点坐标、 相机内参

貌似优于相机标定和三维重建时分开的,所以比较少见到相机内参参与优化的。。。我也就省略它,只说包含相机姿态和三维点的情况。

调整的目标

我们把相机姿态和三维点捆绑到一起了,就该进行调整了,调整的目标很明显就是更精确的相机姿态,更精确的三维点坐标。恩,这是废话,关键是怎么用数学的语言来描述它。再来想想理想情况下(这真是个好东西),也就是三维点和相机姿态都是完全精确的情况下,三维点通过这个相机姿态投影到图像上,应该和它在图像上的对应点是重合的,,,,很显然这种情况在现实中发生的概率约等于中彩票。。。这也就是说实际中,投影点和它对应点是不重合的,他们之间有差异、差异、差异,有了差异就有了误差,有了误差就可以建立误差函数,然后就可以优化函数,,,,问题又来了,怎么度量这个差异,理论上应该重合就是说投影点和对应点之间的距离应该为0,那就用两点之间的距离来度量他们之间的差异嘛,这个就是我们的重投影误差\[ \sum_i^n\sum_j^mv_{i,j}d(Q(a_j,b_i),x_{i,j})^2 \] 上面这个公式来自于维基百科,他的各项符号含义分别是:

  • \(n,m\): 分别代表三维点的个数和图片的数量;

  • \(v_{i,j}\):第 \(i\) 个三维点在第 \(j\) 幅图像上是否有投影点,也就是相机在拍摄第 \(j\) 幅图像时能否“看到”三维点 \(i\) ;

  • \(d(x,y)\):图像上两个点 \(x,y\) 之间的欧氏距离;

  • \(Q(a_j,b_i)\):投影方程,利用相机姿态(\(R_j,T_j\))和相机内参,把三维点 \(i\) 投影到图像 \(j\) 上,\(x = K[R_jX_i + T_j]\)

  • \(x_{i,j}\):三维点 \(i\) ,在图像 \(j\) 上的对应点坐标。

我们的目标就变成了通过调整相机姿态和三维点坐标来让重投影误差最小,这也其实就变成了一个非线性最小二乘的问题。

怎么调整

下面涉及到的数学推导并不是很严谨,只是用于说明计算的方法。

有了目标就已经成功了一半了,接下来就只需要不断调整相机姿态和三维点的坐标来优化重投影误差,可是怎么调整又成了问题,总不能每次瞎猜一个值然后带进去算吧,,,,一般都需要用迭代的方法来求解,例如高斯牛顿、LM算法。

为了描述简便,用函数 \(f(x)\) 来表示我们的目标函数,其中 \(x\) 是包含了我们所有参数的向量,我们的目标是求解 \(f(x)\) 在何处取得极小值。这个函数的参数这么多,很明显不能直接一步到位,但是从某个值出发(对于三维重建的问题,这个出发点已经有了,就是前面计算出来的相机姿态和三维点坐标)慢慢向正确的值靠近,一次靠近一丢丢(\(\delta x\)),很多次后就可以很接近或者达到正确的值了。于是嘛,变成了我们一次靠近多少的问题了。

梯度下降(最速下降)法

从名字就可以看出来,这个方法是用了梯度信息。梯度的方向是函数上升最快的方向,我们要求的是最小值,那就沿着梯度方向的反方向进行靠近就可以了嘛。于是梯度下降法就是每次沿着梯度反方向以一个给定的步长进行逼近,这样就可以一步一步的靠近最小值。

这个方法虽然可以保证每一次都是在下降,但是下降的速度却并不像它的名字那样很快,收敛的速度较慢。

牛顿法

这篇文章 很好的解释了牛顿法的原理,但是貌似只能求零点啊,这个求极值有啥关系,函数导数为0的地方不就是极值么。。假设在第\(k\) 次迭代的时候,我们的参数向量为\(x_k\) ,本次迭代的增量为 \(\delta x_k\) (也就是本次迭代要求解的东西),那么函数就变成了\(f(x_k+\delta x_k)\) ,首先将这个函数进行泰勒展开到二次项: \[ f(x_k+\delta x_k) \approx f(x_k) + \delta x_k f'(x_k) +\frac{1}{2} (\delta x_k)^2 f''(x_k) = \phi(\delta x_k) \] 然后对\(\phi(\delta x_k)\) 求导: \[ \phi '(\delta x_k) = f'(x_k) + f''(x_k)\delta x_k \] 令上面的导数为0就可以求得\(\delta x_k\) ,但是需要注意的是,\(x_k、\delta x_k\) 都是向量,所以\(f'(x_k)、f''(x_k)\) 都是矩阵,它们分别叫做雅可比(Jacobi)矩阵(记为\(J\))和海森(Hessian)矩阵(记为\(H\)),雅克比矩阵是函数对所有参数求一阶偏导得到,海森矩阵是函数对所有参数求二阶偏导得到。于是: \[ J+H\delta x_k = 0 \\ \delta x_k = -H^{-1}J \] 这种方法下降速度比梯度下降要快很多(至于为啥快,我就不知道了,,),但是他不能保证每次迭代都是在下降的(但是总体是在下降的)。这样貌似问题解决了,但是,回头去看看我们的函数,要对这个函数求二阶偏导,简直太复杂了,大家选择放弃,,,,

高斯牛顿法

前面说海森矩阵的求解太麻烦,但是牛顿迭代有收敛很快,于是大家决定偷个懒,不求它。我们在泰勒展开的时候就只展开到一次项: \[ f(x_k+\delta x_k) \approx f(x_k)+ J\delta x_k \] 这样貌似等式右边就变成了类型\(ax+b\) 这种线性的形式了啊,这懒偷得有点多了,必须找其他的约束补回来,,,比如我们的优化的是重投影误差,我们最希望它为零啊,,,于是嘛就直接令上式为零就得到了: \[ f(x_k) + J\delta x_k = 0 \] 貌似\(\delta x_k\) 就可以求出来了:\(\delta x_k = -J^{-1}f(x_k)\) ,但是总感觉有啥不对,,,恩,\(J\) 的大小是\(m\times n\) 的,它可能不是一个方阵!!!所以它是不能直接求逆的,所以需要按照这样算: \[ \begin{split} &f(x_k) + J\delta x_k = 0 \\ &J^Tf(x_k) + J^TJ\delta x_k = 0 \\ &\delta x_k = -(J^TJ)^{-1}J^Tf(x_k) \end{split} \] 这里可以看出来牛顿法和高斯牛顿法计算 \(\delta x_k\) 的公式长得貌似挺像的,其实高斯牛顿迭代的思想是利用雅克比来近似海森矩阵:\(H\approx J^TJ\) 。这里只是解决了不计算海森矩阵,但是它还是存在不能保证每次迭代都是在下降的问题。

Levenberg-Marquard算法

梯度下降可以保证每次迭代都是下降的,但是收敛慢,高斯牛顿收敛快但是不能保证每次迭代都是下降的。于是LM算法就出现啦,它把梯度下降和高斯牛顿法结合了起来,使得每次迭代都是下降的,而且收敛速度还比较快。至于怎么结合的,简直不能更粗暴,用一个参数\(\lambda\) 来控制增量计算方式是靠近高斯牛顿迭代的方法还是靠近梯度下降的方法,它把上面计算增量的方程改成了下面的形式: \[ \delta x_k = -[J^TJ+\lambda diag(J^TJ)]^{-1} J^Tf(x_k) \] 可以看出来\(\lambda\) 很小甚至为零时,增量计算方式和高斯牛顿迭代的方法是一致的;当\(\lambda\) 很大的时候,大到\(J^TJ\) 可以忽略的时候,就变成了: \[ \delta x_k = -\lambda J^Tf(x_k) \] 恩,长得和梯度下降的式子挺像的。目前的Bundle Adjustment貌似都是基于LM算法实现的。

雅克比矩阵

上面提到了很多次雅克比矩阵,甚至于每次迭代都需要计算一次雅克比矩阵,雅克比又是个啥?对于我们三维重建的问题,我们期望优化的参数有相机姿态\([R\ \ \ t]\) ,这里一共有6个未知数(为啥是6个?因为旋转矩阵 \(R\) 的自由度为3,事实上也可以用罗德里格斯公式转化为一个三维的向量\((r_1,r_2,r_3)\),位移向量\(t\) 是长度为3的向量\((t_1,t_2,t_3)\)),三维点的坐标\((x,y,z)\) 也就是一共就,9个参数。那么雅克比矩阵就一共有9列,每一列就是函数\(f(x)\) 对不同的参数进行求偏导,这一列的元素值就是这个偏导数在一组参数下的取值: \[ J = [\frac{\partial f}{\partial r_1} \ \ \frac{\partial f}{\partial r_2} \ \ \frac{\partial f}{\partial r_3} \ \ \frac{\partial f}{\partial t_1} \ \ \frac{\partial f}{\partial t_2} \ \ \frac{\partial f}{\partial t_3} \ \ \frac{\partial f}{\partial x} \ \ \frac{\partial f}{\partial y} \ \ \frac{\partial f}{\partial z}] \] 假设一共有m个三维点,n幅图像,那么雅克比矩阵就应该有\(m\times n\)行,总的雅克比矩阵可以: \[ J = \left[\begin{matrix}\ \frac{\partial f}{\partial r_1^1} &\frac{\partial f}{\partial r_2^1}&\frac{\partial f}{\partial r_3^1} &\frac{\partial f}{\partial t_1^1} &\frac{\partial f}{\partial t_2^1} &\frac{\partial f}{\partial t_3^1} &\frac{\partial f}{\partial x^1} &\frac{\partial f}{\partial y^1} &\frac{\partial f}{\partial z^1} \\ \frac{\partial f}{\partial r_1^2} &\frac{\partial f}{\partial r_2^2}&\frac{\partial f}{\partial r_3^2} &\frac{\partial f}{\partial t_1^2} &\frac{\partial f}{\partial t_2^2} &\frac{\partial f}{\partial t_3^2} &\frac{\partial f}{\partial x^1} &\frac{\partial f}{\partial y^1} &\frac{\partial f}{\partial z^1} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ \frac{\partial f}{\partial r_1^n} &\frac{\partial f}{\partial r_2^n}&\frac{\partial f}{\partial r_3^n} &\frac{\partial f}{\partial t_1^n} &\frac{\partial f}{\partial t_2^n} &\frac{\partial f}{\partial t_3^n} &\frac{\partial f}{\partial x^1} &\frac{\partial f}{\partial y^1} &\frac{\partial f}{\partial z^1} \\ \frac{\partial f}{\partial r_1^1} &\frac{\partial f}{\partial r_2^1}&\frac{\partial f}{\partial r_3^1} &\frac{\partial f}{\partial t_1^1} &\frac{\partial f}{\partial t_2^1} &\frac{\partial f}{\partial t_3^1} &\frac{\partial f}{\partial x^2} &\frac{\partial f}{\partial y^2} &\frac{\partial f}{\partial z^2} \\ \frac{\partial f}{\partial r_1^2} &\frac{\partial f}{\partial r_2^2}&\frac{\partial f}{\partial r_3^2} &\frac{\partial f}{\partial t_1^2} &\frac{\partial f}{\partial t_2^2} &\frac{\partial f}{\partial t_3^2} &\frac{\partial f}{\partial x^2} &\frac{\partial f}{\partial y^2} &\frac{\partial f}{\partial z^2} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ \frac{\partial f}{\partial r_1^n} &\frac{\partial f}{\partial r_2^n}&\frac{\partial f}{\partial r_3^n} &\frac{\partial f}{\partial t_1^n} &\frac{\partial f}{\partial t_2^n} &\frac{\partial f}{\partial t_3^n} &\frac{\partial f}{\partial x^2} &\frac{\partial f}{\partial y^2} &\frac{\partial f}{\partial z^2} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ \frac{\partial f}{\partial r_1^n} &\frac{\partial f}{\partial r_2^n}&\frac{\partial f}{\partial r_3^n} &\frac{\partial f}{\partial t_1^n} &\frac{\partial f}{\partial t_2^n} &\frac{\partial f}{\partial t_3^n} &\frac{\partial f}{\partial x^m} &\frac{\partial f}{\partial y^m} &\frac{\partial f}{\partial z^m} \\ \end{matrix}\right] \] 三维重建一般都有成千上万个三维点和几十上百幅图像,所以雅克比矩阵非常庞大,每次迭代都需要计算一个如此大的矩阵,计算量也是非常巨大的,好在\(J\) 是一个稀疏矩阵,也就是说有很多很多项都是0,比如当一个三维点在某一幅图像上没有投影点的时候,对应的那一行就为全为0了。

最后

其实写到这儿,Bundle Ajustment的主要内容应该差不多,但是也还有一些坑:

  • 应该后面还有一部分就是用解方程的方法计算\(\delta x_k\) ,但是这个具体怎么算我也不知道😂😂😂。。。
  • 上面的的推导默认\(x_{k+1} = x_k + \delta x_k\) ,但是旋转矩阵相加后比一定是旋转矩阵了,这就用到了其他的表示方式,比如李代数。。。