返回 登录
0

视觉SLAM从理论到实践:设计前端(1)

阅读6033

作者:高翔,张涛,刘毅,颜沁睿。
本文节选自图书《视觉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入群交流。
图片描述

评论