视觉SLAM十四讲学习笔记——第九讲 后端优化（1）

        经过前端（视觉里程计）估计得到的轨迹和地图由于存在累计误差，在长时间内是不准确的。因此希望构建一个针对全局的更大规模的优化问题，得到最优的轨迹和地图，这里主要有两种解决思路：

（1）基于马尔可夫性假设的卡尔曼滤波器：马氏性假设可以简单地理解为“当前时刻状态只与上一时刻有关”。针对SLAM问题（非线性）的卡尔曼滤波器给出了单次线性近似下的最大后验估计，或者说是优化过程中一次迭代的结果。

（2）非线性优化：非线性优化考虑了当前及之前所有时刻的状态，同时优化所有时刻的相机位姿以及各个特征点的空间位置。使用光束法平差（BA）求得最优解。

        一般情况下认为非线性优化算法比滤波器占有明显优势，因此重点关注BA求解与图优化。

1.问题分析

        首先针对全局的BA本质上仍是一个最小化重投影误差的问题：结合任意时刻的相机外部参数（位姿）、内部参数及拍摄到的特征点的三维坐标，计算其投影到图像像素坐标系下的坐标，最小化与实际提取结果之间的误差。所涉及的变量均为前端估计得到的，在这个过程中同时调整相机参数及路标（特征点）坐标。

         确定优化问题后，一般我们可以使用常见的优化算法直接求解，计算雅克比矩阵和海塞矩阵求解增量方程，但是由于考虑了过去的所有状态和路标，因此方程的维度极大，很难满足实时性，求解过程中需要进行优化。这里考虑到H矩阵的稀疏性，利用Schur消元（也可以称为边缘化）显著降低计算量，加速运算。不过在实际应用中Ceres和g2o库都可以帮助我们完成这一过程。

        最后为削弱异常数据对整个优化过程的影响，提高鲁棒性，提出来几种常用的鲁棒核函数，确保每条边的误差不会大的没边而掩盖其他的边，使优化结果更稳健。

2.示例代码分析

        首先是数据读取，这里选择创建了一个BALProblem类，来管理读取到的数据。

/// 从文件读入BAL dataset
class BALProblem {
public:
    /// 加载文件数据
    explicit BALProblem(const std::string &filename, bool use_quaternions = false);

    ~BALProblem() {
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }

    /// 存储数据到.txt
    void WriteToFile(const std::string &filename) const;

    /// 存储点云文件
    void WriteToPLYFile(const std::string &filename) const;
    /// 归一化
    void Normalize();