VSLAM基础（七）————光束平差法Bundle Adjustment

上一篇我们讲了线性的超定方程的解法，这节我们就接着讲讲slam里非线性方程的优化方法。

一、BA光束平差法

偷个图：

首先到底啥是BA呢？

在slam前端中，我们通过匹配特征点、对极约束、三角化就可以恢复出相机姿态（R，t）与三维物点坐标（用X表示），这两数据是我们通过观测到的像点坐标（用Z表示）求得。

但是实际情况中往往存在大量的噪声，点与点不是精确地对应甚至出现一些错误匹配。 这就导致我们预测求出的R，t，X存在误差，我们需要在已知的观测值下找到一个方法能够调整优化我们求出的R，t，X使得尽可能的靠近真实值。

光束平差法由Bundle Adjustment翻译得来： 
            对场景中任意三维点P，由从每个视图所对应的的摄像机的光心发射出来并经过图像中P对应的像素后的光线，都将交于P这一点，对于所有三维点，则形成相当多的光束（bundle）；实际过程中由于噪声等存在，每条光线几乎不可能汇聚与一点，因此在求解过程中，需要不断对待求信息进行调整（adjustment），来使得最终光线能交于点P。

具体来说：

可以想象存在很多条光线，其中每一条连接了相机光心，三维物点，物点在相片上的投影，这个模型（为了简单化暂时不考虑畸变的影响）我们通过之前所讲过的P矩阵描述，即Z = PX，因为误差的存在PX并不绝对与Z相等，此时我们就是要调整（R，t，X）使得代价 最小。因为从每一个位姿处观察物点都可以视为一个光束，则BA就是优化整体所有光束代价最小时的（R，t，X）。如下式