ORB_SLAM2代码阅读(5)——Bundle Adjustment

1. 说明

前几篇博客介绍了一下ORB_SLAM2的Tracking、LocalMapping 和 LoopClosing三大线程。本篇博客介绍一下 Bundle Adjustment（简称BA）。本来介绍BA可以单独写一篇文章，不必放在ORB_SLAM2代码阅读系列，但是由于ORB_SLAM2中的用到了这种技术，所以我也就把这部分放到了ORB_SLAM2代码阅读系列中。本文将从BA的物理意义、数学表达、求解手段、求解工具和BA在ORB_SLAM2中的应用这几方面来进行介绍。

2. Bundle Adjustment(BA)的物理意义

Bundle Adjustment 中文译为光束法平差。当然，它也有其他的中文译法，比如束调整、捆集调整或者捆绑调整等等。其中bundle，来源于bundle of light，其本意就是指的光束，这些光束指的是三维空间中的点投影到像平面上的光束。而平差则是测量学中的概念，指的是：由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。为了提高成果的质量，处理好这些测量中存在的误差问题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。

说的简单一点： BA的本质是一个优化模型，其目的是减小重投影误差 。

我们可以用一个模型来表述BA的过程。
这个图形经常在各种介绍BA的文章中提到，我也在此使用一下这个图。

从图中我们可以看到，长方体的各个顶点分别投影到了 $P_1$ 、$P_2$ 和 $P_3$代表的图像上。这就是一个普通的投影过程，并没有什么特殊的地方。但是在SLAM当中，前端的视觉里程计会计算出相邻两幅图像之间的变换关系和地图点的空间位置。放到上面这幅图中就是，我们可以计算出 $P_1$ 、$P_2$ 和 $P_3$之间的变换关系和长方体各个定点的空间坐标，但是，不可否认的是我们的计算结果和真实值之间是存在差异的。由于计算误差的存在，长方体各个顶点的空间坐标与真实坐标并不会完全重合。既然我们计算出的顶点坐标与真实坐标存在差异，那么将我们计算出的顶点位置再次用投影模型投影到图像上（二次投影，重投影）得到的投影坐标与顶点真实位置的投影（第一次投影）坐标之间不可避免的存在差异，这个误差我们称之为 重投影误差 。

从上面的过程中我们可以知道，造成重投影误差的原因是我们计算的相机之间的变换关系和地图点的空间位置不准确造成。所以如果我们能够减小重投影误差，那么我们就能优化相机的位姿和地图点的位置。这正是SLAM后端优化部分要做的事情。

那么，如何能够减小重投影误差就是BA要做的事情了。

3. BA的数学表达

投影过程可以分为以下几个步骤：

1. 首先，把世界坐标转换到相机坐标，这里将用到相机外参数$(R;t)$：
   $$P^{\prime }=Rp+t=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$
2. 然后，将$P^{\prime }$投至归一化平面，得到归一化坐标：
   $$P_c=[u_c;$$