常用算法

1. Bundle Adjustment (BA)

    Bundle adjustment优化的是sum of reprojection error(重投影错误之和)，这是一个geometric distance（几何距离），问题可以formulate成一个least squares problem， 如果nosie是gaussian的话，那就是一个maximum likelihood estimator，是这种情况下所能得到的最优解了。

     机器人导航中，2D的特征reproject回三维域内，和真正的3D点的位置会有偏差。但是在物理意义上，3D点和投射到摄像机的2D特征点是同一个点。所以这个误差出现在计算3D点时摄像机自身旋转矩阵和位移向量上。

     Bundle Adjustment的作用是，通过least square等算法，去最小化这个偏差，以此得到机器人移动和方向的精确值。这在物理意义上是最精确的，是Visual SLAM问题的state-of-art解决方法。

     其实least square problem一般都是用Gauss-Newton 法或者LM（Levenberg-Marquardt）算法迭代求解。Bundle Adjustment本质也是LM算法。由于是特定的形式，所以可以化成sparse matrix 的形式，这样计算量大大减小了。

     Bundle Adjustment（BA）是非线性的平差问题，要修正投影关系使得投影之后的点与观测到的点之间的欧式距离最小的问题。

     为什么选用BA?

     这个reprojection error的公式是非线性的，所以这个least squares problem得用iterative method来求解。最简单的是Newton， 但是要算Hessian，并不是很好算，所以pass。接下来是Gauss-Newton，用J^T J 来近似Hessian，但是convergence速度不给力，也pass。再下来是Levenberg-Marquardt，是一个damping method，改一个lambda来控制到底是偏向steepest descent还是偏向Gauss-Newton，如果算出来更渣的一步就不接受，改个lambda重算，这样一来做了很多无用功，所以也pass。再下来还有Powell's dogleg，是一个trust region method，在这个小区间内搞一个新的function来近objective function，不论好坏都走一步，但是步幅不会超过所谓的trust region，再根据表现调整这个region，总的来说在large scale问题上比Levenberg-Marquardt的表现要好。再往后问题再大就得用前面所说的算法的sparse版本，再大下去得换conjugate gradient方法。    

    无论GN,LM,DL，中间都要解一个Ax=b形式的linear system，一般情况下算法的效率就取决于解这个linear system的效率。所以说到底这些nonlinear least squares problem最后也就是解一个linear system。这个linear system你可以直接inverse，也可以用QR，或者Choleskey，或者Schur complement trick来解，爱谁谁。说到这个Choleskey decomposition，当初就是为了Geodetic mapping而发明的。

2. ICP: Iterative Closest Point

3.