SLAM专题（10）- 最小化重投影误差与Bundle Adjustment （BA）

          Adjustment computation最早是由geodesy的人搞出来的。19世纪中期的时候，geodetics的学者就开始研究large scale triangulations（大型三角剖分）。20世纪中期，随着camera和computer的出现，photogrammetry(照相测量法)也开始研究adjustment computation，所以他们给起了个名字叫bundle adjustment。21世纪前后，robotics领域开始兴起SLAM，最早用的recursive bayesian filter（递归贝叶斯滤波），后来把问题搞成个graph然后用least squares方法解。

       这些东西归根结底就是Gauss大神“发明”的least squares method（最小二乘法）。当年天文学家Piazzi整天闲得没事看星星，在1801年1月1号早上发现了一个从来没观测到的星星，再接下来的42天里做了19次观测之后这个星星就消失了。当时的天文学家为了确定这玩意到底是什么绞尽了脑汁，这时候Gauss出现了，（最初）只用了3个观察数据，就用least squares算出了这个小行星的轨道，接下来天文学家根据Gauss的预测，也重新发现了这个小行星（虽然有小小的偏差），并将其命名为Ceres，也就是谷神星。Google的ceres-solver就是根据这个来命名的。[ref: How Gauss Determined the Orbit of Ceres]

         Bundle adjustment优化的是sum of reprojection error，即重投影误差和，这是一个（geometric distance）几何距离[为什么要minimize geometric distance可以参考[Hartley00]]，可以转换成一个least squares problem， 如果nosie是gaussian的话，那就是一个最大似然估计（maximum likelihood estimator），是这种情况下所能得到的最优解了。  这个reprojection error的公式是非线性的，所以这个least squares problem得用迭代法来求解：一般都是用Gauss-Newton 法或者LM算法迭代求解。bundle adjustmen由于是特定的形式，所以可以化成sparse matrix 稀疏矩阵的形式，这样计算量大大减小了。不论GN,LM,中间都要解一个Ax=b形式的linear system，一般情况下算法的效率就取决于解这个linear system的效率。所以说到底这些nonlinear least squares problem最后也就是解一个linear system。这个linear system你可以直接解，也可以用QR分解，乔姆斯基分解 ，或者奇异值分解法求解来解。

         然而，现实中并不是所有观测过程中的噪声都服从 gaussian noise的（或者可以说几乎没有），遇到有outlier的情况，这些方法非常容易挂掉，这时候就得用到robust statistics里面的robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，比较常用的有huber, cauchy等等。