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

        上文提到考虑全局的后端优化计算量非常大，因此在计算增量方程时，借助H矩阵的稀疏性加速运算。但是随着时间的推移，累积的相机位姿和路标数量还是会导致计算量过大，以上一节的示例代码数据为例：16张图像，共提取到22106个特征点，这些特征点共出现了83718次。对于一个20Hz更新速度，上述的数据量甚至还不到1s的内容，因此在求解大规模定位建图问题时，一定要控制BA的规模。这里主要有两种解决思路：

（1）滑动窗口法：固定优化N个关键帧，这N个关键帧同时观测到了M个路标点，因此可以建立一个规模适中的BA问题。应用过程中要注意保持BA窗口的稀疏性。

（2）位姿图：从示例数据可以观察到，特征点的数量远大于相机位姿的数量。位姿图忽略路标点的优化仅关心相机位姿之间的联系。此外，如果有额外的传感器（IMU），那么位姿图也是融合Pose测量结果的方法（VIO）。

        事实上，前端需要实时响应视频的速度，例如每秒30帧；而后端优化可以慢一些运行，只需要在优化完成时返回给前端即可。通常不会对后端提出很高的速度要求，但也不能太慢。

        示例代码仅给出了用g2o实现的位姿图优化，这部分代码也比较简单了，这部分的公式推导部分主要用来第四章李代数的内容，雅可比矩阵计算的部分可以取近似值也可以直接取单位阵：

typedef Matrix Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(const SE3d &e) {
    Matrix6d J;
    J.block(0, 0, 3, 3) = SO3d::hat(e.so3().log());
    J.block(0, 3, 3, 3) = SO3d::hat(e.translation());
    J.block(3, 0, 3, 3) = Matrix3d::Zero(3, 3);
    J.block(3, 3, 3, 3) = SO3d::hat(e.so3().log());
    // J = J * 0.5 + Matrix6d::Identity();
    J = Matrix6d::Identity();    // try Identity if you want
    return J;
}

        至于可视化的优化结果，可以使用g2o_viewer打开.g2o文件。我印象里这个g2o_viewer好像不需要额外安装，安装好g2o库就可以了，使用时在result.g2o的文件目录下打开命令行，输入：

g2o_viewer result.g2o

        就可以看到运行结果了。