深蓝学院-视觉SLAM课程-第7讲笔记

课程Gitbug地址：https://github.com/wrk666/VSLAM-Course/tree/master

0. 内容

这讲来讲后端
之前的最小二乘的方法属于批量方法，较为简单，另一种方法是递归方法。

1. 递归方法

前端是实时的估计，抓取关键帧和特征点匹配，后端计算量较大，不要求实时，但是计算量也需要限定在一定的程度内。
保持当前状态，来新信息之后就更新，

运动方程对自己的位姿有一个大概的估计，但是存在不确定性，随着时间推移，不确定性会累积，而观测方程观测到路标点，对之前的估计进行矫正，不确定性会减小很多。

假设一阶马尔科夫性则会得到EKF等滤波器方法
不假设的话会得到非线性优化的方法。

上面K的不需要计算后延协方差的方式使用了SMW的第4个公式

2. 批量方法

对稀疏矩阵做一个简单的维数验证：
雅可比：88矩阵块

雅可比的转置${\mathbf{J}}^T$，同样也是88矩阵块，每个块也要转置

$H={\mathbf{J}}^T\mathbf{J}$

和下面的临接矩阵除了对角线外，结构一样

高斯消元，H第二行左乘$-E{C}^{-1}$加到第一行来消掉右上角的E。

这里核心意思是把计算量转移到了求解$\Delta x_c$上，最主要的就是就是对C进行求逆，

边缘化小结：

2.1. BA和递归的对比

新的关键帧来到时，要把老的关键帧Marg掉，老的关键帧会影响先验分布
问题是：如果回环之后，老的信息不太容易用起来。

3. Pose Graph位姿图

由于路标点的数量很多，CPU很难实现实时优化，所以策略

1. 如滑动窗口那样，丢弃一些历史数据；
2. 如位姿图那样舍弃对路标点的优化，只保留Pose之间的边。

4. 实践

4.1 Ceres BA书写

照着例程运行，安装meshlab

sudo apt-get install meshlab

这个是初始点云

这个是优化后的

噪声点明显少了很多。

4.2 g2o BA

跑代码和4.1差不多。

4.3 Pose Graph SE3

跑出来大概这样子：

4.4 Pose Graph Lie_algebra

编译时有个小bug：

./pose_graph_g2o_lie ../sphere.g2o
Sophus ensure failed in function 'void Sophus::SO3Base::normalize() [with Derived = Sophus::SO3]', file '/usr/local/include/sophus/so3.hpp', line 273.
Quaternion (   0.706662 4.32706e-17    0.707551 -4.3325e-17) should not be close to zero!

查了一下，原因是：
顶点类VertexSE3LieAlgebra的读入函数virtual bool read(istream& is)缺少返回值，会报以上错误。
解决方法：在函数virtual bool read(istream& is)中加入return true;

virtual bool read(istream &is) override {
    double data[7];
    for (int i = 0; i < 7; i++)
        is >> data[i];
    Quaterniond q(data[6], data[3], data[4], data[5]);
    q.normalize();
    setMeasurement(SE3d(q, Vector3d(data[0], data[1], data[2])));
    for (int i = 0; i < information().rows() && is.good(); i++)
        for (int j = i; j < information().cols() && is.good(); j++) {
            is >> information()(i, j);
            if (i != j)
                information()(j, i) = information()(i, j);
        }
    return true;
}

用单位阵代替$J_r^{-1}$结果差不多：

//        Matrix6d J = JRInv(SE3d::exp(_error));
        Matrix6d J = MatrixXd::Identity(6, 6);
        // 尝试把J近似为I？

做作业！