视觉SLAM从理论到实践：设计前端（1）

作者：高翔，张涛，刘毅，颜沁睿。
本文节选自图书《视觉SLAM十四讲：从理论到实践》

本文完全由实践部分组成，实际书写一个视觉里程计程序。你会管理局部的机器人轨迹与路标点，并体验一下一个软件框架是如何组成的。在操作过程中，我们会遇到许多问题：相机运动过快、图像模糊、误匹配……都会使算法失效。要让程序稳定运行，我们需要处理以上的种种情况，这将带来许多工程实现方面的、有益的讨论。

主要目标：

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

搭建VO框架

知晓砖头和水泥的原理，并不代表能够建造伟大的宫殿。
在笔者深爱的《我的世界》游戏中，玩家拥有的只是一些色彩、纹理不同的方块。其性质极其简单，而玩家所要做的只是把这些方块放在空地上而已。理解一个方块至为简单，但实际拿起它们时，初学者往往只能搭建简单的火柴盒房屋，而有经验、有创造力的玩家则可用这些简单的方块建造民居、园林、楼台亭榭，乃至城市，如图1 。

图1 左下是笔者的练习作品。右下来自Epicwork团队作品：《圆明园》

在SLAM中，我们认为工程实现和理解算法原理应该至少是同等重要的，甚至更应强调如何书写实际可用的程序。算法的原理，就像一个个方块一样，我们可以清楚明确地讨论它们的原理和性质，但仅仅理解了一个个方块并不能使你建造真正的建筑：它们需要大量的尝试、时间和经验，我们鼓励读者朝更为实际的方向努力——当然这往往是十分复杂的。就像在《我的世界》里那样，你需要掌握各种立柱、墙面、屋顶的结构，墙面的雕花，几何形体角度的计算，这些远远不像讨论每个方块的性质那样简单。

SLAM的具体实现亦是如此，一个实用的程序会有很多的工程设计和技巧（Trick），还需要讨论每一步出现问题之后该如何处理。原则上讲，每个人实现的SLAM都会有所不同，多数时候我们并不能说哪种实现方式就一定是最好的。但是，我们通常会遇到一些共同的问题：“怎么管理地图点”“如何处理误匹配”“如何选择关键帧”，等等。我们希望读者能对这些可能出现的问题产生一些直观的感觉——我们认为这种感觉是非常重要的。

所以，出于对实践的重视，本章我们将带领读者领略一下搭建SLAM框架的过程。就像建筑那样，我们要讨论柱间距、门面宽高比等琐碎但重要的问题。SLAM工程是复杂的。即使我们只保留核心的部分，也会占用大量的篇幅，使本书变得过于繁冗。不过，请注意，尽管完成之后的工程是复杂的，但是中间的“由简到繁”的过程，是值得详细讨论、有学习价值的。所以，我们要从简单的数据结构出发，先来做一个简单的视觉里程计，再慢慢地把一些额外的功能加进来。换言之，我们要把从简单到复杂的过程展现给读者看，这样你才会明白一个库是如何像雪人那样慢慢堆起来的。
本讲的代码放在slambook/project中。由于随着开发过程不断前进，我们会对工程做一些删改，因此它的内容也会发生变化。所以我们会把中间的代码也保留在目录中，以版本号命名，以便读者随时查看、模仿。

确定程序框架

根据前两讲的内容，我们知道视觉里程计分单目、双目、RGB-D三大类。单目视觉相对复杂，而RGB-D最为简单，没有初始化，也没有尺度问题。本着由简入繁的指导思想，我们先从RGB-D做起。为了方便读者做实验，我们将使用数据集而非实际的RGB-D相机（因为不能保证读者人手一台RGB-D相机）。

首先，我们来了解一下Linux程序的组织方式。在编写一个小规模的库时，我们通常会建立一些文件夹，把源代码、头文件、文档、测试数据、配置文件、日志等分类存放，这样会显得很有条理。如果一个库内容很多，我们还会把代码分解成各个独立的小模块，以便测试。读者可以参照OpenCV或g2o的组织方式，看看一个大中型库是如何组织的。例如，OpenCV有core、imgproc、features2d等模块，每个模块分别负责不同的任务。g2o则有core、solvers、types等若干模块。不过在小型程序里，我们也可以把所有的东西糅在一起，称为SLAM库。

现在我们要写的SLAM库是一个小型库，目标是帮读者将本书用到的各种算法融会贯通，书写自己的SLAM程序。挑选一个工程目录，在其下面建立如下文件夹来组织代码文件：

1. bin 用来存放可执行的二进制文件。
2. include/myslam 存放SLAM模块的头文件，主要是.h文件。这种做法的理由是，当把包含目录设到include，引用自己的头文件时，需要写include “myslam/xxx.h”，这样不容易和别的库混淆。
3. src 存放源代码文件，主要是.cpp文件。
4. test 存放测试用的文件，也是.cpp文件。
5. lib 存放编译好的库文件。
6. config 存放配置文件。
7. cmake_modules 第三方库的cmake文件，在使用g2o之类的库时会用到它。

以上就是我们的目录结构，如图2所示。相比于之前每一讲内零零散散地放着的main.cpp，这种做法显得更有条理。接下来，我们会在这些目录里不断地添加新文件，逐渐形成一个完整的程序。

图2 工程项目的目录

确定基本数据结构

为了让程序跑起来，我们要设计好数据单元，以及程序处理的流程。这好比构成房屋的一个个的柱子和砖块。那么，在一个SLAM程序中，有哪些结构是最基本的呢？我们抽象出以下基本概念：

1. 帧：帧是相机采集到的图像单位。它主要包含一个图像（RGB-D情形下是一对图像）。此外，还有特征点、位姿、内参等信息。
   在视觉SLAM中我们会谈论关键帧（Key-frame）。由于相机采集的数据很多，存储所有的数据显然是不现实的。否则，如果相机放在桌上不动，程序的内存占用也会越来越高直至无法接受。通常的做法是把某些我们认为更重要的帧保存起来，并认为相机轨迹可以用这些关键帧来描述。关键帧如何选择是一个很大的问题，而且基于工程经验，很少有理论上的指导。在本书中我们会使用一个关键帧选择方法，但读者亦可考虑自己提出新的方式。
2. 路标：路标点即图像中的特征点。在相机运动后，我们还能估计它们的3D位置。通常，会把路标点放在一个地图当中，并将新来的帧与地图中的路标点进行匹配，估计相机位姿。

帧的位姿与路标的位置估计相当于一个局部的SLAM问题。除此之外，我们还需要一些工具，让程序写起来更流畅。例如：

1. 配置文件：在写程序过程中你会经常遇到各种各样的参数，比如，相机的内参、特征点的数量、匹配时选择的比例，等等。你可以把这些数写在程序中，但那不是一个好习惯。你会经常修改这些参数，但每次修改后都要重新编译一遍程序。当其数量越来越多时，修改就变得越来越困难。所以，更好的方式是在外部定义一个配置文件，程序运行时读取该配置文件中的参数值。这样，每次只要修改配置文件内容就行了，不必对程序本身做任何修改。
2. 坐标变换：你会经常需要在坐标系间进行坐标变换，例如，世界坐标到相机坐标、相机坐标到归一化相机坐标、归一化相机坐标到像素坐标，等等。定义一个类把这些操作都放在一起将更方便。

下面我们就来定义帧、路标这几个概念，在C++中都以类来表示。我们尽量保证一个类有单独的头文件和源文件，避免把许多个类放在同一个文件中。然后，把函数声明放在头文件，实现放在源文件中（除非函数很短，也可以写在头文件中）。我们参照Google的命名规范，同时考虑尽量以初学者也能看懂的方式来写程序。由于我们的程序是偏向算法而非软件工程的，所以不讨论复杂的类继承关系、接口、模板等，而更关注算法的正确实现，以及是否便于扩展。我们会把数据成员设置为公有，尽管这在C++软件设计中是应该避免的，如果读者愿意，也可以把它们改成private或protected接口，并添加设置和获取接口。在过程较为复杂的算法中，我们会把它分解成若干步骤，例如特征提取和匹配应该分别在不同的函数中实现，这样，当我们想修改算法流程时，就不需要修改整个运行流程，只需调整局部的处理方式即可。

现在，让我们开始写VO。我们把这个版本定为0.1版，表示这是刚开始的阶段。我们一共写5个类：Frame为帧，Camera为相机模型，MapPoint为特征点/路标点，Map管理特征点，Config提供配置参数。它们的关系如图3所示。我们现在只写它们的数据成员和常用方法，而在后面用到更多内容时再行添加。

图3 基本类的关系示意图

Camera类最简单，我们先来实现它。

Camera类

Camera类存储相机的内参和外参，并完成相机坐标系、像素坐标系和世界坐标系之间的坐标变换。当然，在世界坐标系中你需要一个相机的（变动的）外参，我们以参数的形式传入。

#ifndef CAMERA_H
#define CAMERA_H
#include "myslam/common_include.h"
namespace myslam
{
// Pinhole RGB-D camera model
class Camera
{
public:
    typedef std::shared_ptr<Camera> Ptr;
    float   fx_, fy_, cx_, cy_, depth_scale_;  // Camera intrinsics 

    Camera();
    Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :
    fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale )
    {}

    // coordinate transform: world, camera, pixel
    Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w );
    Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w );
    Vector2d camera2pixel( const Vector3d& p_c );
    Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); 
    Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 );
    Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );
};
}
#endif // CAMERA_H

说明如下（由上往下）：

1. 在这个简单的例子中，我们给出了防止头文件重复引用的ifndef宏定义。如果没有这个宏，在两处引用此头文件时将出现类的重复定义。所以，在每个程序头文件里都会定义这样一个宏。
2. 我们用命名空间namespace myslam将类定义包裹起来（因为是我们自己写的SLAM，所以命名空间就叫myslam了）。命名空间可以防止我们不小心定义出别的库里同名的函数，也是一种比较安全和规范的做法。由于宏定义和命名空间在每个文件中都会写一遍，所以我们只在这里稍加介绍，后面将略去。
3. 我们把一些常用的头文件放在common_include.h文件中，这样就可以避免每次书写很长的一串include。
4. 我们把智能指针定义成Camera的指针类型，因此以后在传递参数时使用Camera::Ptr类型即可。
5. 我们用Sophus::SE3来表达相机的位姿。Sophus库在李代数一讲介绍过。

在源文件中，给出Camera方法的实现：

#include "myslam/camera.h"
namespace myslam
{

Camera::Camera()
{
}

Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w )
{
    return T_c_w*p_w;
}

Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
{
    return T_c_w.inverse() *p_c;
}

Vector2d Camera::camera2pixel ( const Vector3d& p_c )
{
    return Vector2d (
        fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) + cx_,
        fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) + cy_
    );
}

Vector3d Camera::pixel2camera ( const Vector2d& p_p, double depth )
{
    return Vector3d (
        ( p_p ( 0,0 )-cx_ ) *depth/fx_,
        ( p_p ( 1,0 )-cy_ ) *depth/fy_,
        depth
    );
}

Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w )
{
    return camera2pixel ( world2camera ( p_w, T_c_w ) );
}

Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth )
{
    return camera2world ( pixel2camera ( p_p, depth ), T_c_w );
}
}

读者可以对照一下这些方法是否和第5讲的内容一致。它们完成了像素坐标系、相机坐标系和世界坐标系间的坐标变换。

Frame类

下面来考虑Frame类。Frame类是基本数据单元，在许多地方会用到，但现在是初期设计阶段，我们还不清楚以后可能新加的内容。所以这里的Frame类只提供基本的数据存储和接口。如果之后有新增的内容，再继续往里添加。

class Frame
{
public:
    typedef std::shared_ptr<Frame> Ptr;
    unsigned long  id_; // id of this frame
    double time_stamp_; // when it is recorded
    SE3 T_c_w_; // transform from world to camera
    Camera::Ptr camera_; // Pinhole RGB-D Camera model 
    Mat color_, depth_; // color and depth image 

public: // data members 
    Frame();
    Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
    ~Frame();

    // factory function
    static Frame::Ptr createFrame(); 

    // find the depth in depth map
    double findDepth( const cv::KeyPoint& kp );

    // Get Camera Center
    Vector3d getCamCenter() const;

    // check if a point is in this frame 
    bool isInFrame( const Vector3d& pt_world );
};

在Frame中，我们定义了ID、时间戳、位姿、相机、图像这几个量，这应该是一个帧当中含有的最重要的信息。在方法中，我们提取了几个重要的方法：创建Frame、寻找给定点对应的深度、获取相机光心、判断某个点是否在视野内，等等。它们的实现是比较平凡的，请读者参考frame.cpp了解这些函数的具体实现。

MapPoint类

MapPoint表示路标点。我们将估计它的世界坐标，并且会拿当前帧提取到的特征点与地图中的路标点匹配，来估计相机的运动，因此还需要存储它对应的描述子。此外，我们会记录一个点被观测到的次数和被匹配的次数，作为评价其好坏程度的指标。

class MapPoint
{
public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long      id_; // ID
    Vector3d    pos_;       // Position in world
    Vector3d    norm_;      // Normal of viewing direction 
    Mat         descriptor_; // Descriptor for matching 
    int         observed_times_;    // being observed by feature matching algo.
    int         correct_times_;     // being an inliner in pose estimation

    MapPoint();
    MapPoint( long id, Vector3d position, Vector3d norm );

    // factory function
    static MapPoint::Ptr createMapPoint();
};

同样，读者可以浏览src/map.cpp查看其实现。目前为止我们只考虑这些数据成员的初始化问题。

Map类

Map类管理着所有的路标点，并负责添加新路标、删除不好的路标等工作。VO的匹配过程只要和Map打交道即可。当然Map也会有很多操作，但现阶段我们只定义主要的数据结构。

class Map
{
public:
    typedef shared_ptr<Map> Ptr;
    unordered_map<unsigned long, MapPoint::Ptr >  map_points_;        // all landmarks
    unordered_map<unsigned long, Frame::Ptr >     keyframes_;         // all key-frames

    Map() {}

    void insertKeyFrame( Frame::Ptr frame );
    void insertMapPoint( MapPoint::Ptr map_point );
};

Map类中实际存储了各个关键帧和路标点，既需要随机访问，又需要随时插入和删除，因此我们使用散列（Hash）来进行存储。

Config类

Config类负责参数文件的读取，并在程序的任意地方都可随时提供参数的值。所以我们把Config写成单件模式（Singleton）。它只有一个全局对象，当我们设置参数文件时，创建该对象并读取参数文件，随后就可以在任意地方访问参数值，最后在程序结束时自动销毁。

class Config
{
private:
    static std::shared_ptr<Config> config_; 
    cv::FileStorage file_;

    Config () {} // private constructor makes a singleton
public:
    ~Config();  // close the file when deconstructing 

    // set a new config file 
    static void setParameterFile( const std::string& filename ); 

    // access the parameter values
    template< typename T >
    static T get( const std::string& key )
    {
        return T( Config::config_->file_[key] );
    }
};

说明如下：

1. 我们把构造函数声明为私有，防止这个类的对象在别处建立，它只能在setParameterFile时构造。实际构造的对象是Config的智能指针：static shared_ptr
   config_。用智能指针的原因是可以自动析构，省得我们再调一个别的函数来做析构。
2. 在文件读取方面，我们使用OpenCV提供的FileStorage类。它可以读取一个YAML文件，且可以访问其中任意一个字段。由于参数实质值可能为整数、浮点数或字符串，所以我们通过一个模板函数get来获得任意类型的参数值。

下面是Config的实现。注意，我们把单例模式的全局指针定义在此源文件中了：

void Config::setParameterFile( const std::string& filename )
{
    if ( config_ == nullptr )
        config_ = shared_ptr<Config>(new Config);
    config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );
    if ( config_->file_.isOpened() == false )
    {
        std::cerr<<"parameter file "<<filename<<" does not exist."<<std::endl;
        config_->file_.release();
        return ;
    }
}
Config::~Config()
{
    if ( file_.isOpened() )
        file_.release();
}
shared_ptr<Config> Config::config_ = nullptr;

在实现中，我们只要判断一下参数文件是否存在即可。定义了这个Config类后，我们可以在任何地方获取参数文件里的参数。例如，当想要定义相机的焦距时，按照如下步骤操作即可：

1. 在参数文件中加入：“Camera.fx: 500”。
2. 在代码中使用：
   myslam::Config::setParameterFile(“parameter.yaml”);
   double fx = myslam::Config::get (“Camera.fx”);
   就能获得的值了。

当然，参数文件的实现方法绝对不止这一种。我们主要从程序开发上的便利角度来考虑这个实现，读者当然也可以用更简单的方式来实现参数的配置。

至此，我们定义了SLAM程序的基本数据结构，书写了若干个基本类。这好比是造房子的砖头和水泥。你可以调用cmake编译这个0.1版，尽管它还没有实质性的功能。接下来我们来考虑把前面讲过的VO算法加到工程中，并做一些测试来调整各算法的性能。注意，笔者会刻意地暴露某些设计的问题，所以你看到的实现不见得就是最好的（或者足够好的）。

点击阅读下半部分：

视觉SLAM从理论到实践：设计前端（2）

编者按：本文节选自图书《视觉SLAM十四讲：从理论到实践》，系统介绍了视觉SLAM（同时定位与地图构建）所需的基本知识与核心算法，既包括数学理论基础，又包括计算机视觉的算法实现。此外，还提供了大量的实例代码供读者学习研究，从而更深入地掌握这些内容。

---

在线直播 | 人工智能核心技术解析与应用实战峰会由CSDN学院倾力打造，力邀一线公司技术骨干做深度解读。本期直播（5月13日）邀请来自阿里巴巴、思必驰、第四范式、一点资讯、58集团、PercepIn等在AI领域有着领先技术研究的一批专家，他们将针对人脸识别、卷积神经网络、大规模分布式机器学习系统搭建、推荐系统、自然语言处理及SLAM在机器人领域应用等热点话题进行分享。限时特惠199元即可听6位技术专家的在线分享，欢迎报名参加，加微信csdncxrs入群交流。