视觉SLAM十四讲学习笔记——第十三讲 实践：设计SLAM系统

   1.如何运行示例代码

        首先是如何运行示例代码，这里遇到了很多问题：

（1）首先要下载Kitti数据集，并在config/default.yaml文件内修改路径。

（2）安装Glog、GTest、GFlags库，这部分比较简单，可能遇到的问题可以参考以下几个教程：

                        Ubuntu 16.04 系统 gflags & glog 安装_calvinpaean的博客-CSDN博客

                        安装glog 执行bash./ autogen时报错“没有这个文件”_蓝雨飞扬7的博客-CSDN博客

（3）Opencv版本，我最初用的是Opencv4,遇到了一些奇怪的bug,后来安装的Opencv3.4.5就解决了一部分奇怪的bug。

（4）修改app/run_kitti_stereo.cpp源码，主函数第一行为：

gflags::ParseCommandLineFlags(&argc, &argv, true);

（5）即使解决了所有问题，我还是不能用Clion直接运行程序，只能运行bin文件下的二进制程序。即在bin文件路径下直接命令行./run_kitti_stereo.cpp,虽然没解决Clion的问题，但至少能成功运行了。

2.主要头文件

        能够成功运行代码后再来关注是怎么实现的，对于工程框架即各个文件夹内容的作用在P347已经详细介绍了，这里主要看一下include/myslam文件下的一系列头文件：

（1）algorithm.h：三角化函数，与前文讲过的方法不同，使用了SVD算法，后续再介绍。

（2）backend.h：后端，使用滑动窗口法对固定数量帧优化。

（3）camera.h：相机类，主要包括相机内外参数及世界、相机和像素三个坐标系下的坐标变换。

（4）common_include.h：常用矩阵类型的定义。

（5）config.h：用于获取config/default.yaml配置文件信息，包括相机内参和数据集路径。

（6）dataset.h：用于读取数据集数据。

（7）feature.h：2D特征点类，主要包括持有帧、对应路标、2D坐标、是否异常等。

（8）frame.h：帧类，主要包括左右图像、位姿、是否为关键帧等。

（9）frontend.h：前端，估计当前帧的位姿，必要时触发后端优化。

（10）g2o_types.h：G2o图优化定义，两类节点（相机位姿和地图点），两类边（用于前端估计位姿的一元边和用于后端优化地图的二元边）。

（11）map.h：地图类，主要包括关键帧、地图点的添加和删除。

（12）mappoint.h：路标点类，三角化后的特征点三维坐标。

（13）viewer.h：负责整个过程的可视化。

（14）visual_odometry.h：整个视觉里程计的对外接口，可在主函数内直接调用。

3.主要算法框架图

        程序可以分为数据结构+算法，这里我们不再讨论数据结构，重点关注一下算法的实现，这里主要包括前端（frontend）+后端（backend），书中也有简单的程序框图，但很难显示所有细节。因此这里简单整理一下：

         这里只整理了前端的系统框图，没有后端，主要是因为我个人感觉后端的代码框架并不复杂，仍然是一个两类节点（相机位姿和路标节点）+多条二元边的优化问题（前文第九讲 后端优化(1)中详细分析过），并在此基础上增加了一步异常数据的筛选，代码复杂一点的部分都是在所构建的数据结构内读取相应信息的过程。整个过程使用滑动窗口法控制BA规模，都是前文有详细讲过的内容。

4.主要关注的问题

（1）三角化方法：

        前文提到过，三角化本质上也是一个最小二乘问题，并且在第12讲的应用中有使用最小二乘解的结论求解像素深度，但在此工程中又使用了另一种方法“基于SVD奇异值分解的三角化”，本质上是用SVD算法求解一个最小二乘问题。代码部分如下：

/**
 * 基于奇异值分解的线性三角测量
 * @param poses     poses,
 * @param points    points in normalized plane
 * @param pt_world  triangulated point in the world
 * @return true if success
 */
inline bool triangulation(const std::vector &poses,
                   const std::vector points, Vec3 &pt_world) {
    MatXX A(2 * poses.size(), 4);//4*4
    VecX b(2 * poses.size());//4*1
    b.setZero();
    for (size_t i = 0; i < poses.size(); ++i) {
        Mat34 m = poses[i].matrix3x4();//转化为3*4的矩阵形式
        A.block<1, 4>(2 * i, 0) = points[i][0] * m.row(2) - m.row(0);
        A.block<1, 4>(2 * i + 1, 0) = points[i][1] * m.row(2) - m.row(1);
    }
    auto svd = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
    pt_world = (svd.matrixV().col(3) / svd.matrixV()(3, 3)).head<3>();

    if (svd.singularValues()[3] / svd.singularValues()[2] < 1e-2) {
        // 解质量不好，放弃
        return true;
    }
    return false;
}

        理论推导过程可以参考下面这边博客：

                      https://blog.csdn.net/qq_42995327/article/details/118917141

（2）左右视角的使用：

        此工程是一个使用双目相机的视觉SLAM，但如何使用双目相机的图像信息呢？事实上，只有较少的状态下使用右侧图像信息：初始化地图、更新关键帧并三角化更新路标点；而左侧图像信息，是从始至终都在使用的，所有的跟踪过程都是用左侧图像来估计当前相机位姿的初始值。

（3）LK光流：

        前端使用的是LK光流来匹配关键点，这在第八讲 视觉里程计内有详细讨论，“本质上是替代图像间特征点匹配的过程，使用“灰度不变”的假设确定关键点的对应关系，因此使用时仅需要对第一幅图像提取关键点，不需要计算描述子，显著缩短运算时间。在完成匹配后，使用特征点法相同的方式估计摄像机运动参数。至于使用哪种方法，要根据是否已知图像点的深度信息来选择。”代码中则是直接使用Opencv自带的LK光流算法实现的。

（4）图优化后去除异常数据：

        G2o图优化我们都非常熟悉了，但这一次相比于之前的应用，在完成优化后进行了一步异常数据的判断，无论在前端（两帧图像间的位姿估计）还是后端（求解BA问题）都有标记异常数据的过程。筛选方法比较简单，设定一个阈值（固定的或者动态的）根据各个边的损失函数判断数据是否异常，这样有助于提高系统的稳定性，减少累计误差。

（5）如何控制BA规模：

        第十讲 后端（2）设计到两种控制BA规模的方法，这里使用的是“滑动窗口法”。但是滑动窗口的选择较为简单，大致可以理解为保存最近的7帧（这里的“近”我认为指的是距离上的近），但实际上在剔除旧关键帧时还是有一定的依据的，在map.cpp文件下，先找到距离当前帧最近和最远的两帧，若最近的小于阈值，则优先剔除，否则剔除最远的：

void Map::RemoveOldKeyframe() {
    if (current_frame_ == nullptr) return;
    // 寻找与当前帧最近与最远的两个关键帧
    double max_dis = 0, min_dis = 9999;
    double max_kf_id = 0, min_kf_id = 0;
    auto Twc = current_frame_->Pose().inverse();
    for (auto& kf : active_keyframes_) {
        if (kf.second == current_frame_) continue;
        auto dis = (kf.second->Pose() * Twc).log().norm();
        if (dis > max_dis) {
            max_dis = dis;
            max_kf_id = kf.first;
        }
        if (dis < min_dis) {
            min_dis = dis;
            min_kf_id = kf.first;
        }
    }

    const double min_dis_th = 0.2;  // 最近阈值
    Frame::Ptr frame_to_remove = nullptr;
    if (min_dis < min_dis_th) {
        // 如果存在很近的帧，优先删掉最近的
        frame_to_remove = keyframes_.at(min_kf_id);
    } else {
        // 删掉最远的
        frame_to_remove = keyframes_.at(max_kf_id);
    }

    LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_;
    // remove keyframe and landmark observation
    active_keyframes_.erase(frame_to_remove->keyframe_id_);
    for (auto feat : frame_to_remove->features_left_) {
        auto mp = feat->map_point_.lock();
        if (mp) {
            mp->RemoveObservation(feat);
        }
    }
    for (auto feat : frame_to_remove->features_right_) {
        if (feat == nullptr) continue;
        auto mp = feat->map_point_.lock();
        if (mp) {
            mp->RemoveObservation(feat);
        }
    }

    CleanMap();
}

（6）其他：

        除了这些重要的算法外，高博还设计了一套完备的数据结构，并在必要的地方添加了“锁”，防止数据访问时出错；单独为后端开了一个线程，在需要时开始线程运行后端的优化程序；此外还有可视化数据界面、数据文件读取以及一系列编程技巧等等。可以说哪怕只是实现一个简单可靠的视觉SLAM系统仍是一个十分复杂的问题，这里笔者也只是重点关注了书中曾讲到过的重要的SLAM算法，其他部分的内容由于能力有限没能仔细研究清楚。下一篇再分享前端和后端两个主要的源代码的注释。