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

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

0. 内容

这讲来讲后端
之前的最小二乘的方法属于批量方法,较为简单,另一种方法是递归方法。
深蓝学院-视觉SLAM课程-第7讲笔记_第1张图片

1. 递归方法

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

深蓝学院-视觉SLAM课程-第7讲笔记_第2张图片
运动方程对自己的位姿有一个大概的估计,但是存在不确定性,随着时间推移,不确定性会累积,而观测方程观测到路标点,对之前的估计进行矫正,不确定性会减小很多。

深蓝学院-视觉SLAM课程-第7讲笔记_第3张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第4张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第5张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第6张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第7张图片
假设一阶马尔科夫性则会得到EKF等滤波器方法
不假设的话会得到非线性优化的方法。

深蓝学院-视觉SLAM课程-第7讲笔记_第8张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第9张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第10张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第11张图片
上面K的不需要计算后延协方差的方式使用了SMW的第4个公式

深蓝学院-视觉SLAM课程-第7讲笔记_第12张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第13张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第14张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第15张图片

2. 批量方法

深蓝学院-视觉SLAM课程-第7讲笔记_第16张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第17张图片
对稀疏矩阵做一个简单的维数验证:
雅可比:88矩阵块
深蓝学院-视觉SLAM课程-第7讲笔记_第18张图片
雅可比的转置 J T \bm J^T JT,同样也是8
8矩阵块,每个块也要转置
深蓝学院-视觉SLAM课程-第7讲笔记_第19张图片
H = J T J H=\bm J^T\bm J H=JTJ
深蓝学院-视觉SLAM课程-第7讲笔记_第20张图片
和下面的临接矩阵除了对角线外,结构一样
深蓝学院-视觉SLAM课程-第7讲笔记_第21张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第22张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第23张图片
高斯消元,H第二行左乘 − E C − 1 -EC^{-1} EC1加到第一行来消掉右上角的E。
深蓝学院-视觉SLAM课程-第7讲笔记_第24张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第25张图片
这里核心意思是把计算量转移到了求解 Δ x c \Delta x_c Δxc上,最主要的就是就是对C进行求逆,

深蓝学院-视觉SLAM课程-第7讲笔记_第26张图片
边缘化小结:
深蓝学院-视觉SLAM课程-第7讲笔记_第27张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第28张图片

2.1. BA和递归的对比

深蓝学院-视觉SLAM课程-第7讲笔记_第29张图片

深蓝学院-视觉SLAM课程-第7讲笔记_第30张图片
新的关键帧来到时,要把老的关键帧Marg掉,老的关键帧会影响先验分布
问题是:如果回环之后,老的信息不太容易用起来。

3. Pose Graph位姿图

深蓝学院-视觉SLAM课程-第7讲笔记_第31张图片
由于路标点的数量很多,CPU很难实现实时优化,所以策略

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

深蓝学院-视觉SLAM课程-第7讲笔记_第32张图片
深蓝学院-视觉SLAM课程-第7讲笔记_第33张图片

4. 实践

4.1 Ceres BA书写

照着例程运行,安装meshlab

sudo apt-get install meshlab

这个是初始点云
深蓝学院-视觉SLAM课程-第7讲笔记_第34张图片
这个是优化后的
深蓝学院-视觉SLAM课程-第7讲笔记_第35张图片
噪声点明显少了很多。

4.2 g2o BA

跑代码和4.1差不多。

4.3 Pose Graph SE3

跑出来大概这样子:
深蓝学院-视觉SLAM课程-第7讲笔记_第36张图片

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 J_r^{-1} Jr1结果差不多:

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

深蓝学院-视觉SLAM课程-第7讲笔记_第37张图片

做作业!

你可能感兴趣的:(SLAM,slam)