BA 光束平差法---十四讲7.7.3

  按照正常（线性）求解过程，特征提取-匹配-PnP 即先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。
  把相机和三维点放在一起进行优化的问题，统称为Bundle Adjustment（BA）。

过程如下，设空间点P_i=[X_i，Y_i，Z_i]^T；投影坐标u_i=[u_i，v_i]^T。(注意这两个值是真值，实际由于观测噪声，以及相机位姿未知，我们无法得到最准确的值）

像素位置与空间点位置关系如下：

s_i u_i=KTP_i

由于于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此将所有误差求和，构建最小二乘问题，需求误差最小时候的相机位姿

其中：

误差项的物理含义是，是将 3D 点按照当前估计的位姿进行投影得到的位置，与观测位置相比较得到的误差，所以称之为重投影误差。

注：理解该式子物理含义的时候可以如此理解，但实际计算的时候，u_i为匹配得到的数据，P为测量或计算得到。其中P也可用BA优化。

如图所示，通过特征匹配，知道p₁;p₂是同一个点P的投影，但是不知道相机位姿。使用初值计算时，P的投影p₂^~与实际观测得到的p₂之间有一定的距离。于是调整相机位姿，使这个距离变小。实际是多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常不会精确为0。

最小二乘问题构建成功，则可以按照第六讲非线性优化内容进行求解。

SLAM基础知识梳理