一看标题就知道，又是一篇基础科普

整个机器人状态估计一般由视觉前端/后端优化与回环约束构成。

今天重点谈一下后端优化，很多是个人的一些感悟。

后端优化无论是使用卡尔曼滤波还是光束平差法BA，到最后都是一个非线性最小二乘问题。

卡尔曼滤波之前详细写过，就不赘述了。

写到这里的时候加几个有意思的知识点，关于距离：

1.欧氏距离：这个代表了两个点之间的真实距离，在坐标系中无论正负，取绝对值

2.马氏距离：满足同一分布且协方差矩阵为Σ的随机变量的差异程度，这里要注意的是，当Σ为单位矩阵时，欧氏距离就是马氏距离，当Σ为对角矩阵，马氏距离称为正规化欧氏距离

3.汉明距离：2个字串之间不同的数量，如1001和0011的汉明距离是“2”

4.误差二范数：A的转置共轭矩阵与矩阵A的积的最大特征根的平方根，表示空间中2个向量矩阵的直线距离。

后端优化BA能使用的基础是Hession矩阵的稀疏性(这个属于SLAM常识不赘述，看到这里不懂的话去看视觉SLAM十四讲)

BA基本的流程如下(因子图法-G2O)：

(1)选择节点与边的类型，确定参数化类型，通常点代表Frame/Pose/特征点的状态，边代表残差

(2)向图中加入实际的点与边

(3)选初值，开始迭代

(4)迭代中，计算对于当前值的Jacobian和Hession矩阵

(5)求解稀疏线性方程H△x=-b(标准增量方程)，得到梯度

(6)用GN或LM法继续迭代。如果结束则返回优化值

这个主流程是基于G2O的，CERES略有区别就不写了，其实也可以手写。。。哈哈

这里再讲几个关键迭代算法流程的关键异同(很重要)：

1.最速下降法：大跨步推进函数步数，迅速找到极值(函数最优值)，缺点是容易Zig TAG，就是容易造成路径打Z字，而且很容易过头

2.牛顿法：对目标函数进行标准一阶泰勒展开，然后计算Jacobian和Hession，效果很好，缺点是Hession矩阵算起来开销比较高

3.高斯-牛顿法：这个就是精髓了，将目标函数一阶泰勒展开后，利用高斯分布特性，将H近似为JJ转置，这样就只需要计算Jacobian。

4.LM/Dogleg：这一类算法类似阻尼牛顿法，就是在高斯牛顿法的过程中增加一个μ因子，使其在合适的条件下在高斯牛顿法和最速下降法之间切换，更好地达到最后优化的效果。

最后一般要加入一个柯西核或者胡博核(Huber Norm)函数，更好地处理outlier。

BA优化的核心就是这些了，其实多做一些之后就会发现虽然看起来复杂，但是在实际工程实践中还是比较简单的，真正的难题往往是多传感器融合后权重和约束的处理！