视觉SLAM十四讲--第13讲 实践：设计SLAM系统（最详细的代码调试运行步骤）

选题1：Kitti 的双目视觉里程计
   Kitti 的 Odometry 提供了左右双目的视觉图像（以及激光，但本作业不使用激光数据），并提供了标定信息。它一共含有若干个 Sequence，其中一部分 Sequence 的真实轨迹是开放的，另一部分则是隐藏的，作为测试使用。在 Kitti 官网可以上传你对测试部分的轨迹估计，系统会计算与真实轨迹的差异，并给出评分。
   现在我们已经介绍了所有关于视觉 SLAM 方面的内容，你可以基于已有算法，实现一个双目的视觉里程计了。Kitti 官网 odometry 分类下提供了双目相机的所有信息。请你根据已学知识，实现双目视觉里程计，然后运行任何一个sequence，并与标准轨迹比较。
说明：
   虽然已经学完了视觉SLAM的相关知识点，但是当想要去自己编程实现，用c++来实现一整个SLAM系统的时候，我完全不知道该怎么下手了。因此本次的作业代码是基于高博课本第13讲的代码上进行改动的。虽然高博课本上的SLAM系统，已经很简单很基础的了，但是，我自己在读代码、走流程的时候依然觉得有点难啃。里边的C++模板、头文件的书写、多线程的实现、线程锁的使用以及可视化的实现都很精妙。不过这个SLAM系统没有回环检测，也没有用一些trick，特征提取什么的用的也是OpenCV自带的方法。还有很多可以调整的地方。

一、 代码目录介绍及运行步骤

代码目录：

如何运行

1. 在ch9目录下创建一个build文件夹;
2. 编译ch9下的CMakeList.txt文件;
3. 目录切回到ch9下，执行如下命令：

./bin/run_kitti_stereo

二、 代码调试问题

1. 准备工作

• 首先一些包是必须安装的，如gtest、gflags、glog、eigen、g2o、CSparse、Sophus、pangolin、OpenCV等。
  gtest、gflags、glog的安装，推荐源码编译安装，这里可以看我另外一篇博客，如下图所示，有介绍安装步骤：
  

• 然后是修改数据集的路径：将其改为自己的数据集路径位置。
  修改文件config/default.yaml中 dataset_dir: 路径位置
  

• 修改C++的版本和OpenCV的版本指定，将opencv调成不指定版本的，
  修改ch13/CMakeLists.txt中代码如下所示：
  修改OpenCV部分
  

• 添加fmt库，修改ch13/CMakeLists.txt中代码如下所示：
  

2. 遇到的问题及解决办法

• 如果运行run_kitti_stereo时出现如下错误
  或者出现了/usr/bin/ld:找不到-lglut…recipe for target ‘…/lib/libmyslam.so’ failed的错误
  
  或者是编译之后不报错 ，但是lib下没有libmyslam.so文件，这是因为缺少包，执行如下命令，安装freeglut即可

sudo apt-get install freeglut3-dev

• 如果遇到下边的index_sequence错误，此问题易解决，之前遇到过。
  解决办法，将ch13/CMakeLists.txt中的c++11标准改为c++ 14，如下所示：
  
• 如果编译是出现…/…/lib/libmyslam.so：对‘std::locale fmt::v8::detail::locale_ref::getstd::locale() const’未定义的引用的错误：这是因为编译文件中没有链入fmt库，添加方法看本文的准备工作部分。
  
• 如果遇到 relocation R_X86_64_PC32 against symbol `stderr@@GLIBC_2.2.5’ can not be used when making a shared object; recompile with -fPIC的错误，这是由于链接库中使用了libfmt.a与libmyslam.so，编译时动态库与静态库不能混用。.a是静态库，.so是动态库，具体问题具体分析，有的是因为gflags安装是安装的静态库，如下图所示，而我的问题是fmt是静态库，需要在编译时（cmake … -DGFLAGS_NAMESPACE=google -DCMAKE_CXX_FLAGS=-fPIC …）稍加注意，改成动态库就行了。
  
  改成动态库的具体操作步骤、解决办法：
  重新编译安装fmt包:

git clone  https://ghproxy.com/https://github.com/fmtlib/fmt.git
cd fmt/
mkdir build
cd build
cmake .. -DGFLAGS_NAMESPACE=google -DCMAKE_CXX_FLAGS=-fPIC ..
make -j8
sudo make install

• 然后再重新编译就好了，大功告成。
  
• 编译运行成功后，运行时又出现了新错误，WARNING: Logging before InitGoogleLogging() is written to STDERR
  I20210712 22:42:13.808815 12504 visual_odometry.cpp:44] VO is running Segmentation fault (core dumped)：
  
  解决办法：
  修改app/run_kitti_stereo.cpp，注释掉assert这句代码，将vo->Init()提取出来，重新编译运行：
  
• 另外，值得注意的一点是，如果是命令行运行代码，需要在/ch13目录下执行如下代码：

 ./bin/run_kitti_stereo 

• 如果运行目录直接切到了bin下边，执行./run_kitti_stereo，会出现WARNING: Logging before InitGoogleLogging() is written to STDERR E20211204 11:31:53.583640 10794 config.cpp:9] parameter file ./config/default.yaml does not exist. I20211204 11:31:53.583715 10794 visual_odometry.cpp:44] VO is running 段错误 (核心已转储)的错误，如下图所示：
  

• 如果使用CLion进行编译，出现如下错误“ERROR: unknown command line flag ‘gtest_color’”，这是因为clion中少加了关于测试的参数。
  
  两种解决办法：1. 按照slambook2书中的指示，在命令行中运行 bin/run_kitti_stereo就可以了; 2. 加这个参数不就可以了嘛，怎么加，我还在想办法。
  

3. 成功运行

我测试过，经过上述步骤，在CLion下，或者自己创建build文件夹，编译运行都可正常运行。

这里，我要感谢一下我参考的一些博客的作者，有时虽不能帮我解决问题，但是在他们的文章下进行评论时，能得到及时的回复，使我备受鼓舞。
参考链接：

• 第十三讲 实践：设计SLAM系统
• 第13讲实践：设计slam系统遇到问题总结
• slam14讲第13讲问题解决
• make报错：relocation R_X86_64_PC32 against symbol `stderr@@GLIBC_2.2.5

三、 代码理解

核心算法结构

数据结构：

基本数据结构及关系如下图所示：
图像、特征和路标是本系统最基本的数据结构：

• 处理的基本数据单元就是图像
• 对每一帧图像进行特征点的提取
• 在图像中寻找相同的特征点，然后用三角化计算它的3D位置，及路标。

算法

分析系统有两个重要的模块：

• 前端：负责提取图像中的特征、光流追踪、三角化。必要时，补充新的特征点做三角化。前端处理结果作为后端的初始值，前端要对每帧进行处理，需要较高的效率。
• 后端：后端优化不需要实时的进行，是一个较慢的过程。对前端之前的发出的数据（关键帧和路标点）进行优化，返回优化结果，最终改善建图的整体效果。后端应该控制优化问题的规模，不要让其对时间无限增长。

为了方便前端与后端之间的数据流动，加入地图类，前端负责在地图中添加路标点，后端负责将地图优化路标点，并将旧的路标点删除掉，保持一定的规模。

除了核心算法，为了后续的编码实现，我们还需要一些小的辅助模块让系统更方便：

• 相机类：来管理相机的参数和投影函数
• 配置文件类：方便灵活修改配置
• 数据类：读取相应格式的数据
• 可视化模块类：可以观察系统的运行状态

代码理解及修改

代码理解

前端

如何选取关键帧？如果追踪到的点较少，就判定当前帧为关键帧。

前端的处理逻辑：

• 前端本身有初始化、正常追踪、追踪丢失三种状态
• 在初始化状态中，根据左右目之间的光流匹配，寻找可以三角化的地图点，成功时建立初始地图。
• 追踪阶段中，前端计算上一帧的特征点到当前帧的光流，根据光流结果计算图像位姿。该计算只使用左目图像，不用右目。
• 关键帧的选取
• 如果追踪丢失，重置前端系统，重新初始化。

前端处理流程大致确定了，代码如下：

bool Frontend::AddFrame(myslam::Frame::Ptr frame) {
    current_frame_ = frame;

    switch (status_) {
        case FrontendStatus::INITING:
            StereoInit();
            break;
        case FrontendStatus::TRACKING_GOOD:
        case FrontendStatus::TRACKING_BAD:
            Track();
            break;
        case FrontendStatus::LOST:
            Reset();
            break;
    }

    last_frame_ = current_frame_;
    return true;
}

后端

后端相比前端的代码量较少，但是逻辑却更复杂。

代码修改

修改特征提取方法

原来的代码中使用的是GFTT特征点提取方法，这种方法提取的精度较高，但缺点是速度慢，修改成OpenCV自带的FastFeature提取，运行速度会快很多，但是提取的特征点质量不高。代码修改如下：

修改src/frontend.cpp

Frontend::Frontend() {
  //  gftt_ =
   //     cv::GFTTDetector::create(Config::Get<int>("num_features"), 0.01, 20);
    gftt_ =
            cv::FastFeatureDetector::create(Config::Get<int>("num_features") );
    num_features_init_ = Config::Get<int>("num_features_init");
    num_features_ = Config::Get<int>("num_features");
}

修改include/myslam/frontend.h

    // utilities
//    cv::Ptr<cv::GFTTDetector> gftt_;  // feature detector in opencv
    cv::Ptr<cv::FastFeatureDetector> gftt_;  // feature detector in opencv

查看整体建图效果

按照如下方式修改src/viewer.cpp，可以查看整体的建图效果。

// 注意是viewer.cpp中的函数
void Viewer::UpdateMap() {
    std::unique_lock<std::mutex> lck(viewer_data_mutex_);
    assert(map_ != nullptr);
    active_keyframes_ = map_->GetActiveKeyFrames();
    // 这样可以显示所有地图点，同时也能够看出没有回环检测，累计误差很大
    // active_landmarks_ = map_->GetActiveMapPoints();
    active_landmarks_ = map_->GetAllMapPoints();   // 改为all mappoints，显示整体地图
    map_updated_ = true;
}

运行效果图

四、总结

  在完成上述系统的代码调试之后，我又重新读课本上的代码介绍，再读源代码。现在也算了解了SLAM过程中的每个环节，学到现在才觉得自己开始入门SLAM了。对我自己而言理一理SLAM的整体过程也能收获很多， 接下来将会重点放在后端优化部分的调整，后期会尝试将回环检测模块加入本系统。继续加油～

参考资料链接整理：

• 重读《视觉SLAM十四讲》ch13实践设计SLAM系统
• 一个基于深度学习回环检测模块的简单双目 SLAM 系统