从零开始学习VIO笔记 --- 第五讲：滑动窗口算法实践【逐行手写求解器】

一. 知识回顾

1.1 最小二乘求解器

1. 信息矩阵 H 不满秩，那求解的时候如何操作呢？

• 使用 LM 算法，加阻尼因子使得系统满秩，可求解，但是求得的结果可能会往零空间变化。
• 添加先验约束，增加系统的可观性。比如 g2o tutorial 中对第一个pose 的信息矩阵加上单位阵 H[11]+ = I ；

2. orbslam，svo 等等求 mono BA 问题时，fix 一个相机 pose 和一个特征点，或者 fix 两个相机 pose，也是为了限定优化值不乱飘。那代码如何实现 fix 呢?

• 添加超强先验，使得对应的信息矩阵巨大（如，1015），就能使得∆x = 0;
• 设定对应雅克比矩阵为 0，意味着残差等于 0. 求解方程为(0 + λI) ∆x = 0，只能 ∆x = 0;

1.2 单目 Bundle Adjustment 求解代码讲解

上一讲的作业中，也有BA 求解； 但这儿使用图优化模型构建最小二乘问题，再优化（类似 g2o框架）

---

滑动窗口算法

概要

---

关键步骤

---

关键问题

---

VINS-Mono 中的滑动窗口算法

二. 代码

类似 g2o 框架求解 ； （g2o的基本应用—见博主另外一篇文章，应用在曲线拟合）

本节代码包含两部分： 单目 Bundle Adjustment ，（第四讲：滑动窗口【作业】，这一节中的作业有一个 单目BA的信息矩阵计算，是高斯牛顿的求解方式，然而本节是使用g2o进行优化）； 简易滑动窗口算法实践

2.1 单目 Bundle Adjustment

1. 使用高斯牛顿非线性优化的方式求解单目BA ： 构建信息矩阵 $H$，用每一个残差项分别对相机位姿和特征点求导 进行构建；（本部分参考 第四讲：滑动窗口【作业】）

2. 使用图优化的方式求解单目BA ： 则是构建图模型，为三元边，分别连接 逆深度参数化的特征点，第0和第j帧同时观测到这个特征点的两帧相机位姿顶点，共有三个顶点；（本质上也是使用非线性优化的方式去求解的，通过遍历边构造H矩阵，使用LM算法求解）

定义边

/**
 * 此边是视觉重投影误差，此边为三元边，与之相连的顶点有：
 * 路标点的逆深度InveseDepth、第一次观测到该路标点的source Camera的位姿T_World_From_Body1，
 * 和观测到该路标点的mearsurement Camera位姿T_World_From_Body2。
 * 注意：verticies_顶点顺序必须为InveseDepth、T_World_From_Body1、T_World_From_Body2。
 */
class EdgeReprojection : public Edge {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeReprojection(const Vec3 &pts_i, const Vec3 &pts_j)
        : Edge(2, 3, std::vector<std::string>{"VertexInverseDepth", "VertexPose", "VertexPose"}) {
        pts_i_ = pts_i;
        pts_j_ = pts_j;
    }

    /// 返回边的类型信息
    virtual std::string TypeInfo() const override { return "EdgeReprojection"; }

    /// 计算残差
    virtual void ComputeResidual() override;

    /// 计算雅可比
    virtual void ComputeJacobians() override;

    void SetTranslationImuFromCamera(Eigen::Quaterniond &qic_, Vec3 &tic_);

private:
    //Translation imu from camera
    Qd qic;
    Vec3 tic;

    //measurements
    Vec3 pts_i_, pts_j_;
};

2.2 简易滑动窗口算法实践

(后续再跟 工程，详细完成此部分) 与与本节作业第二题对应

通过计算联合高斯分布从而得到协方差矩阵的 （见第四讲！）