期末大作业：设计SLAM系统_机器学习期末大作业

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

提示：这里可以添加本文要记录的大概内容：

例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、实验目的

1.实际设计一个视觉里程计
2.理解SLAM软件框架是如何搭建的
3.理解在视觉里程计设计中容易出现的问题，以及修补的方式

二、实验条件

前提：ubuntu-18.04.6-desktop-amd64
依赖：linux操作系统

三、实验过程

目标选择
实现一个双目视觉里程计在Kitti数据集中的运行效果。
这个视觉里程计由一个光流追踪的前端和一个局部BA的后端组成。双目只需单帧就可初始化，双目存在3D观测，实现效果比单目好。
程序：数据结构+算法

处理的最基本单元图像。在双目中是一对图像，为一帧。
对帧提取特征点。特征点是有很多2D的点。
图像之间寻找特征的关联。如果多次看到某个特征，就用三角化方法计算它的3D位置，即路标。
所以图像，特征，路标是这个系统的最基本的结构。其中路标，路标点，地图点都指代3D空间中的点，语义是一样的。
算法：负责提取特征点的算法，负责做三角化的算法，负责处理优化的算法。总的来说有两个模块：
前端。负责计算相邻图像的特征匹配，往前端中插入图像帧，负责提取图像中的特征点，然后与上一帧进行光流追踪，通过光流结果计算该帧的定位。必要时，应该补充新的特征点并做三角化。前端快速处理保证实时性，前端处理的结果将作为后端优化的初始值。
后端。后端是一个较慢的线程，它拿到前端初始化好的关键帧和路标点后，对他们进行优化，然后返回优化结果。后端应该控制优化问题在一定的规模内，不能随时间增长。
这样通过数据结构和算法确定了系统框架，前后端之间放了地图模块来处理他们之间的数据流动。前后段分别在各自的线程中处理数据，前端提取关键帧后，往梯度中添加新的数据；后端检测到地图更新时，运行一次优化，然后把旧的关键帧和地图点去掉，保持优化的规模。
后续的编程除了核心算法外，还需要一些小模块让系统更加方便：
需要一个相机类来管理相机的内外参和投影函数；
需要一个配置文件管理类，方便从配置文件中读取内容。配置文件中可以记录一些重要的配置参数；
算法是在Kitti数据集上运行的，所以需要一个单独的类来按Kitti的存储格式来读取数据；
需要一个可视化的模块来观察系统的运行状态，否则就是一连传数值；

2.主要实现思路：
（1）实现基本数据结构
先来实现帧，特征，路标点三个类。对于基本数据结构，通常设为struct，考虑到这些数据会被多个线程同时访问或修改，在关键部分我们需要加线程锁。
（2）前端
插入图像帧，提取图像特征，然后与上一帧进行光流追踪，通过光流结果计算该帧的位姿。必要时补充新的特征点并作三角化。前端处理的结果作为后端的初始值。
定义好基本的数据结构后，看来前端的功能，前端需要根据双目图像确定v该镇的位置。每帧都有左右目，进行三角化的时候如何选任意两张图像做三角化，考虑不一致，所以位我们先来确定前端的处理逻辑：
前端本身有初始化，正常追踪，追踪丢失三种状态；
在初始化状态中，根据左右目之间的光流匹配，寻找可以三角化的地图点，成功建立处是地图；
追踪阶段中，前端计算上一帧的特征点到当前帧的光流，根据光流结果计算图像位姿。该计算只使用左目图像，不使用右目；
如果追踪到的点较少，就判定当前帧为关键帧。对于关键帧，有以下几个点：

• 提取新的特征点。
• 找到这些点在右图中的对应点，用三角化建立新的路标点。
• 将新的关键帧和路标点加入到地图，并触发一次后端优化。
  如果追踪丢失，就重置前端系统，重新初始化
  （3）后端
  对前端处理后的数据（关键帧和路标点）进行优化，然后返回优化结果。后端应该控制优化问题的规模，不要让其对时间无限增长。
  为了方便前端与后端之间的数据流动，加入地图类。前后端在分别的线程中处理数据，前端负责在地图中添加路标点，后端检测到地图更新以后优化路标点，并将旧的路标点删除掉，保持一定的规模。
  后端相比前端的代码量较少，但是逻辑却更复杂。原来的代码中使用的是GFTT特征点提取方法，这种方法提取的精度较高，但缺点是速度慢，修改成OpenCV自带的FastFeature提取，运行速度会快很多，但是提取的特征点质量不高。
  3.核心代码：
  （1）帧Frame类：

/**
 * 帧
 * 每一帧分配独立id，关键帧分配关键帧ID
 */
struct Frame {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<Frame> Ptr;
    unsigned long id_ = 0;           //  帧id
    unsigned long keyframe_id_ = 0;  // 关键帧id
    bool is_keyframe_ = false;       // 是否为关键帧
    double time_stamp_;              // 时间戳，暂不使用
    SE3 pose_;                       // Tcw 形式位姿，用李代数表示()
    std::mutex pose_mutex_;          // Pose数据锁
    cv::Mat left_img_, right_img_;   // stereo images
    //装填左图特征点指针的容器
    std::vector<std::shared_ptr<Feature>> features_left_;
    //左图特征点在右图中的对应特征点，如果没有相应的特征，则设置为nullptr
    std::vector<std::shared_ptr<Feature>> features_right_;
   public:  // data members
    Frame() {}
    Frame(long id, double time_stamp, const SE3 &pose, const Mat &left,
          const Mat &right);
    // 帧的位姿会被前端后端同时设置或者访问，为了避免竞争问题，在set和get函数中加锁
    //取出帧的位姿，并保证线程安全
    SE3 Pose() {
        std::unique_lock<std::mutex> lck(pose_mutex_);
        return pose_;
    }
    //设置帧的位姿
    void SetPose(const SE3 &pose) {
        std::unique_lock<std::mutex> lck(pose_mutex_);
        pose_ = pose;
    }
    // 设置关键帧并分配并键帧id
    void SetKeyFrame();
    // 工厂构建模式，分配id 
    static std::shared_ptr<Frame> CreateFrame();
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

（2）特征Feature：

/**
 * 2D 特征点
 * 在三角化之后会被关联一个地图点
 */
struct Feature {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<Feature> Ptr;
    //weak_ptr不能直接定义智能指针对象，因此这里是用weak_ptr持有这个frame的shared_ptr智能指针的值
    std::weak_ptr<Frame> frame_;         // 持有该feature的frame
    cv::KeyPoint position_;              // 2D提取位置
    std::weak_ptr<MapPoint> map_point_;  // 关联地图点
    bool is_outlier_ = false;       // 是否为异常点
    bool is_on_left_image_ = true;  // 标识是否提在左图，false为右图
   public:
    Feature() {}
    Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp)
        : frame_(frame), position_(kp) {}
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

（3）路标点MapPoint：

/**
 * 路标点类
 * 特征点在三角化之后形成路标点
 */
struct MapPoint {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<MapPoint> Ptr;
    unsigned long id_ = 0;              // 路标点ID
    bool is_outlier_ = false;           // 是否是异常点
    Vec3 pos_ = Vec3::Zero();        //路标点世界坐标，初始值为0
    std::mutex data_mutex_;
    int observed_times_ = 0;         //记录路标点被帧观测的次数
    // 用链表存储 观测到这个路标点的所有特征点
    std::list<std::weak_ptr<Feature>> observations_;
    MapPoint() {}
    MapPoint(long id, Vec3 position);
    Vec3 Pos() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return pos_;
    }
    void SetPos(const Vec3 &pos) {
        std::unique_lock<std::mutex> lck(data_mutex_);
        pos_ = pos;
    };
    // 增加新的观测到这个路标点，并且特征点数量+1
    void AddObservation(std::shared_ptr<Feature> feature) {
        std::unique_lock<std::mutex> lck(data_mutex_);
        observations_.push_back(feature);
        observed_times_++;
    }
    //移除异常特征点
    void RemoveObservation(std::shared_ptr<Feature> feat);
    /*
    std::unique_lock<std::mutex> lck(data_mutex_);
    for (auto iter = observations_.begin(); iter != observations_.end();
         iter++) {
        if (iter->lock() == feat) {
            observations_.erase(iter);
            feat->map_point_.reset();
            observed_times_--;
            break;
           }
    }    
    */
     // 取出特征点存储的链表
    std::list<std::weak_ptr<Feature>> GetObs() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return observations_;
    }
    // 工厂模式构建路标点
    static MapPoint::Ptr CreateNewMappoint();
    /*
    static long factory_id = 0;
    MapPoint::Ptr new_mappoint(new MapPoint);
    new_mappoint->id_ = factory_id++;
    return new_mappoint;
    */
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

四、实验结果及分析

两个线程：
.前端：插入帧，提取特征、光流追踪、三角化，必要时补充新的特征点，前端处理结果作为后端的初始值（非线性优化的初值至关重要，这是常说的），前端要对每帧进行处理，需要较高的效率，否则容易“应接不暇”。
.后端：后端可以是一个较慢的线程，对前端之前的发出的数据（关键帧和路标点）进行优化，返回优化结果，最终改善建图的整体效果。后端应该控制优化问题的规模，不要让其对时间无限增长。
为了方便前端与后端之间的数据流动，加入地图类，前端负责在地图中添加路标点，后端负责将地图优化路标点，并将旧的路标点删除掉，保持一定的规模。
为了方便编码实现，还需要添加几个辅助的类：
相机类：管理内外参，因为内外参都由它管理，自然地，投影函数也由它管理
配置文件管理类：方便从配置文件中读取内容，配置文件中记录一些重要参数，方便我们调整。
Kitti数据集读取类
可视化模块：观察系统状态

总结

这次大作业总体过程比较多，让我在操作过程中遇到了不少的困难，但好在通过资料的查找，我现在对SLAM设计相关知识有了一定的理解，虽然电脑系统和新的系统有些不兼容，但最后我还是找到了方法逐步克服。要想通过代码实现功能，就要了解其原理，只有知其然并知其所以然才算是真正懂了这门课，也就不愧对这么多天坐在电脑前查资料、敲代码。
这是一次非常考验和锻炼自己动手能力和对电脑熟悉程度的实验，专业课多了之后时间越来越紧了，也就越来越需要更高的效率去处理自己的电脑。我最大的收获是在ubuntu系统中掌握了更多终端的使用代码，这些加大了运行的方便度。无论是ubuntu、SLAM还是轨道信号这门课程，甚至是通信工程这个专业，我都对它们有了更深的理解。
这门课程是真正意义上将理论与实践结合的课程，让我彻底了解到原来这些看起来极其复杂的代码能够运用到实际生活中，这样的知识竟然在现实中有这么实用的地方。通过视觉里程计自己模拟路程，让我感到了科技改变生活的伟大。无论是地图的构建、机器人的定位，还是在生活中更多的智能化技术，都让我更有动力去探究。希望在之后的学习过程中能得到越来越多的知识。