SLAM专题（专题（10））- 最小化重投影误差与 最小化重投影误差与Bundle Adjustment （（BA）） 在SFM（structure from motion）的计算中BA（Bundle Adjustment）作为最后一步优化具有很重要的作用，在近几年兴起的基于图的SLAM（simultaneous localization and mapping）算法里面使用了图优化替代了原来的滤波器，这里所谓的图优化其实也是指BA。其实很多经典的文献对于BA都有深深浅浅的介 绍，如果想对BA的全过程做一个全面的更深层次的了解，推荐阅读 Bundle Adjustment —A Modern Synthesis. 目录目录 一. 历史由来 二. Bundle Adjustment到底是什么？ 平差也就正好表述了为什么需要BA以及BA这个优化过程到底是怎么进行的。 2.1 BA模型： 2.2 BA的数学模型 2.3 计算—非线性优化 （1）最速下降法 （2）Newton型方法 （3）Gauss-Newton方法 （4）LM（Levenberg-Marquadt）方法 2.4 解方程 (1)稠密矩阵的最小二乘解 (2)稀疏矩阵的Cholesky分解 一一. 历史由来 历史由来 Adjustment computation最早是由geodesy的人搞出来的。19世纪中期的时候，geodetics的学者就开始研究large scale triangulations（大型三角剖 分）。20世纪中期，随着camera和computer的出现，photogrammetry(照相测量法)也开始研究adjustment computation，所以他们给起了个名字叫bundle （递归贝叶斯滤波），后来把问题搞成个graph然后用然后用least adjustment。21世纪前后，robotics领域开始兴起SLAM，最早用的 squares方法解。 方法解。 最早用的recursive bayesian filter（递归贝叶斯滤波），后来把问题搞成个 这些东西归根结底就是Gauss大神大神“发明发明”的的least squares method（最小二乘法）。 这些东西归根结底就是 （最小二乘法）。当年天文学家Piazzi整天闲得没事看星星，在1801年1月1号早上 发现了一个从来没观测到的星星，再接下来的42天里做了19次观测之后这个星星就消失了。当时的天文学家为了确定这玩意到底是什么绞尽了脑汁，这时 候Gauss出现了，（最初）只用了3个观察数据，就用least squares算出了这个小行星的轨道，接下来天文学家根据Gauss的预测，也重新发现了这个小行 星（虽然有小小的偏差），并将其命名为Ceres，也就是谷神星 就是根据这个来命名的。[ref: How Gauss Determined the Orbit of Ceres] ，也就是谷神星。Google的的ceres-solver就是根据这个来命名的 优化的是sum of reprojection error，即重投影误差和，这是一个（ Bundle adjustment优化的是 geometric distance可以参考[Hartley00]]，可以转换成一个 likelihood estimator）），是这种情况下所能得到的最优解了。 这个这个reprojection error的公式是非线性的，所以这个 求解：一 求解：一般都是用 算量大大减小了。不论GN,LM,中间都要解一个Ax=b形式的形式的linear system，一般情况下算法的效率就取决于解这个linear system的效率。所以说到底这 算量大大减小了。 些nonlinear least squares problem最后也就是解一个linear system。这个linear system你可以直接解，也可以用QR分解，乔姆斯基分解 解法求解来解。 解法求解来解。 的公式是非线性的，所以这个least squares problem得用得用迭代法来 迭代法来 特定的形式，所以可以化成sparse matrix 稀疏矩阵的形式，这样计 稀疏矩阵的形式，这样计 可以转换成一个least squares problem， 如果nosie是gaussian的话，那就是一个最大似然估计（ 算法迭代求解。bundle adjustmen由于是特定的形式，所以可以化成 ，即重投影误差和，这是一个（geometric distance）几何距离 般都是用Gauss-Newton 法或者法或者LM算法迭代求解 ）几何距离[为什么要minimize 最大似然估计（maximum 分解，乔姆斯基分解 ，或者奇异值分 ，或者奇异值分 然而，现实中并不是所有观测过程中的噪声都服从 gaussian noise的（或者可以说几乎没有），遇到有 然而，现实中并不是所有观测过程中的噪声都服从 时候就得用到robust statistics里面的robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，比较常用的有huber, cauchy等等。 的（或者可以说几乎没有），遇到有outlier的情况，这些方法非常容易挂掉 的情况，这些方法非常容易挂掉，这 二二. Bundle Adjustment到底是什么？ 到底是什么？ 博客https://blog.csdn.net/OptSolution/article/details/64442962 详细介绍了BA和SLAM图优化方法。 BA 译为光束法平差，或者 译为光束法平差，或者束调整束调整、、捆集调整 捆集调整。。 所谓bundle，来源于bundle of light，其本意就是指的光束光束，这些光束指的是三维空间中的点投影到像平面上的光束 建的，因此称为光束法 建的，因此称为光束法，强调光束也正是描述其优化模型是如何建立的。剩下的就是平差，那什么是平差呢？ 三维空间中的点投影到像平面上的光束，而重投影误差正是利用这些光束来构 而重投影误差正是利用这些光束来构 测量误差总是不可避免的。为了提高成果的质量，处理好这些测量中存在的误差问 观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。有了多余观测，势必在观测结果之间产生矛盾，测量平差的目的就 消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度。测量平差采用的原理就是“最小二乘法 测量平差 测量平差：由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的 题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进行多余观测。 在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度 平差也就正好表述了为什么需要BA以及以及BA这个优化过程到底是怎么进行的。 平差也就正好表述了为什么需要 这个优化过程到底是怎么进行的。 最小二乘法”。 2.1 BA模型：模型： BA的本质是一个优化模型，其目的是最小化重投影误差 的本质是一个优化模型，其目的是最小化重投影误差. 

补充：补充： 作者：hhaowang