【学习SLAM】Bundle Adjustment 光束法平差详解

首先引述来自维基百科的定义：假设我们有一个3D空间中的点，他被位于不同位置的多个摄像机看到，那么所谓的光束法平差（Bundle Adjustment），就是能够从这些多视角信息中提取出3D点的坐标以及各个摄像机的相对位置和光学信息的过程。
可能这么说有点不够具体，我们用比较通俗的数学公式来描述这个过程：
问题提出：假设我们有一个3D空间中的一个点 Xj，他可以被多个位于不同角度的摄像机看到，设第i个摄像机看到的第j个点坐标为xij,由3D点像二维相机平面转换的矩阵为Pi. 我们现在考虑这样的问题，给出一系列的的坐标xij，找到相机转换矩阵Pi使得PiXj=xij.

光束法：加入我们的图像含有噪声，那么PiXj=xij可能不能够准确的满足，所以我们采用极大似然估计（Maximum Likelihood ）的办法，假设测量噪声是高斯白噪声，我们希望估计投射矩阵P̂ i和对应的3D空间中的点X̂ j,这两个参量可以准确的满足P̂ iX̂ j=x̂ ij.这里的x̂ ij

是我们在图像中观测到的点。我们希望最小化所有的通过预测得到的3D空间中的点投影在在平面中的像点，与真实的平面中的像点之间的误差，也就是：

这里的cij代表指示参量，如果计算的像素在平面里面，cij=1 反之，cij=0。d(x,y)表示点x,y之间的几何距离。这种把所有的映射误差最小化的过程被称为光束法平差——就类似于调整位于相机中心和一系列的3D点之间的一串光束一样。

(这里我们要简单回顾下相机矩阵的概念，相机都要进行校正，其中以小孔模型为例，把以小孔中心为原点的三维坐标系中的点转化到同样以小孔中心为原点的二维平面的矩阵，我们称之为内部矩阵（intrinsic matrix）,这个矩阵只与fx,fy,cx,cy
等相机的内部参数有关，把相机坐标系转换到世界坐标系的矩阵称之为外部矩阵，一般涉及相机的旋转和平移（rotation and translation matrix）)。\color{red}{（注意我们一般在变量上加一个折号来表示观测量，如\hat{x},加波浪号表示预测量，如\tilde{x}），个人认为上面公式中出现的有关折号的参量表示应该改为波浪号}。
 

 

光束法平差模型：
在解析摄影测量中，将外方位元素（局外点）和模型点坐标（局内点）的计算放在一个整体内进行，此时称其为光束法。光束法平差是以共线方程式作为数学模型，像点的像平面坐标观测值是未知数的非线性函数，经过线性化后按照最小二乘法原理进行计算。该计算也是在提供一个近似解的基础上，逐次迭代来达到趋近于最佳值的。

1. 共线方程式的表达：
   设S为摄影中心，在世界坐标系下的坐标为;M为空间一点，在世界坐标系下的坐标为（X,Y,Z），m是M在影像上的构象，其像平面和像空间辅助坐标分别为（x，y，-f），，此时可知S、m、M三点共线。可得
   
   再根据像平面坐标和像空间辅助坐标的关系有
   
   由上面两式可解得共线方程式为
   
   其中x0、y0、f是影像内方位元素；表示像平面中心坐标和摄像机主距。

2. 共线方程式的线性化：
   该方程式一次项展开式为
   
   式中为共线方程函数近似值，为外方位元素改正数，为待定点的坐标改正数。
   在保证共线条件下有：
   
   此时，根据上面的共线方程式以及旋转矩阵可得到
   

3. 误差方程式的建立：
   据此可得到误差方程式为
   
   其中有：
   
   将误差方程式改写成矩阵形式可为
   
   也可简写成：
   
   在该式中有：
   

4. 法方程式的建立：
   根据平差原理可知其法方程式为
   
   只需列出误差方程式，权赋1；
   对于控制点，列出误差方程式，还要列出虚拟误差方程式，权赋P。
   虚拟误差方程式为
   
   列出各类点的误差方程式后，按照最小二乘法原理建立法方程式，即按为最小建立的法方程式为
   
   也可简写成：
   
   在根据上式进行展开消元可得改化法方程式为：
   
   或者
   
   根据上面的公式可以求解出外方位元素的改正值；下面的公式可以求解出点的坐标改正值。

5. 结果判定：
   将改正数和规定的限差相比较，若小于限差则迭代完成，否则用未知数的新值又作为近似值继续迭代，直至满足条件。
   由此可知，开始时提供的初始值越接近最佳值，解的收敛速度就愈快；所以通常的处理方法是先进行空间后方交会，求出像片的外方位元素，将其作为光束法平差时未知数的初始值。

 

按照测量平差理解，非线性方程，首先要线性化，即是全微分，然后才能进行误差传播。

按照计算机视觉，导数矩阵可以提供优化的方向，即是梯度方向，指导迭代，如机器学习的梯度下降法

SLAM 中2*6的雅克比见下图：

摄影测量中的误差方程矩阵如下：

注意这里的H等于   Z一拔，摄影测量是先平移后旋转，而计算机视觉是先旋转，后平移。

可以看出，两个矩阵几乎一样，不同有两处：1、角元素改正数的顺序不同 2、符号上的差异，见用圆圈圈起来的地方。

注：知道了导数矩阵，那么如何迭代获得最优值？

对于摄影测量，利用高斯牛顿，即是利用正规方程（就是法方程，笔者在Slam 与测量平差中介绍过）得到改正数，若改正数小于一个阈值，则退出，否则更新姿态元素进行下一次。

对于SLAM 则是，利用列-马（就是岭回归）或者梯度下降法。