视觉SLAM笔记(45) 搭建 VO 框架


1. 确定程序框架

根据之前的内容,知道视觉里程计分单目、双目、 RGB-D 三大类
单目视觉相对复杂,而 RGB-D 最为简单,没有初始化,也没有尺度问题

由简入繁,先从 RGB-D 做起
为了方便做实验,将使用数据集而非实际的 RGB-D 相机

在编写一个小规模的库时,通常会建立一些文件夹
把源代码、头文件、文档、测试数据、配置文件、日志等等分类存放,
这样会显得很有条理

如果一个库内容很多,还会把代码分解各个独立的小模块,以便测试。
可以参照 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 之类的库中会用到它

以上就是目录结构,如图所示

在这里插入图片描述
相比于之前每一章内都零零散散地放着的 main.cpp,这种做法显得更有条理
接下来,会在这些目录里不断地添加新文件,渐渐形成一个完整的程序


2. 确定基本数据结构

为了让程序跑起来,要设计好数据单元,以及程序处理的流程
那么,在一个 SLAM 程序中,抽象出几条基本概念:

  1. :一个帧是相机采集到的图像单位
    它主要包含一个图像(RGB-D 情形下是一对图像),还有特征点、位姿、内参等信息
    在视觉 SLAM 中会谈论关键帧(Key-frame)
    由于相机采集的数据很多,存储所有的数据显然是不现实的
    通常的做法是把某些认为更重要的帧保存起来,并认为相机轨迹就可以用这些关键帧来描述
    关键帧如何选择是一个很大的问题,而且基于工程经验,很少有理论上的指导
    这里也会使用一个关键帧选择方法

  2. 路标:路标点即图像中的特征点
    当相机运动之后,还能估计它们的 3D 位置
    通常,会把路标点放在一个地图当中,并将新来的帧与地图中的路标点进行匹配,估计相机位姿

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

  1. 配置文件:在写程序中会经常遇到各种各样的参数
    比如相机的内参、特征点的数量、匹配时选择的比例等等
    可以把这些数写在程序中,但那不是一个好习惯
    会经常修改这些参数,但每次修改后都要重新编译一遍程序
    当它们数量越来越多时,修改就变得越来越困难
    所以,更好的方式是在 外部定义一个配置文件,程序运行时读取该配置文件中的参数值
    这样,每次只要修改配置文件内容就行了,不必对程序本身做任何修改

  2. 坐标变换:会经常需要在坐标系间进行坐标变换
    例如世界坐标到相机坐标、相机坐标到归一化相机坐标、归一化相机坐标到像素坐标等等
    定义一个类把这些操作都放在一起将更方便些

所以下面就来定义帧、路标这几个概念,在 C++ 中都以类来表示
尽量保证一个类有单独的头文件和源文件,避免把许多个类放在同一个文件中
然后,把函数声明放在头文件,实现放在源文件里(除非函数很短,也可以写在头文件中)
参照 Google的命名规范,同时考虑尽量以初学者也能看懂的方式来写程序
由于程序是偏向算法而非软件工程的,所以不讨论复杂的类继承关系、接口、模板等等
而更应该关注 算法的正确实现,以及是否便于扩展
会把数据成员设置为公有,尽管这在 C++ 软件设计中是应该避免的
当然也可以把它们改成 private 或 protected 接口,并添加设置和获取接口

在过程较为复杂的算法中,会把它分解成若干步骤,例如特征提取和匹配应该分别在不同的函数中实现
这样,当想修改算法流程时,就不需要修改整个运行流程,只需调整局部的处理方式即可

现在,开始写 VO,这是刚开始的阶段
一共写五个类:

  • Frame :帧
  • Camera :相机模型
  • MapPoint :为特征点/路标点
  • Map :管理特征点
  • Config :提供配置参数

它们的关系如图所示:

在这里插入图片描述
现在只写它们的数据成员和常用方法,而在后面用到更多内容时,再另行添加新的内容


3. Camera 类

Camera 类最简单,先来实现它
存储相机的内参和外参,并完成相机坐标系、像素坐标系、和世界坐标系之间的坐标变换
当然,在世界坐标系中需要一个相机的(变动的)外参,以参数的形式传入
在源文件中,Camera 类的头文件位于 /include/myslam/camera.h

#ifndef CAMERA_H // 防止头文件重复引用
#define CAMERA_H

#include "myslam/common_include.h"

namespace myslam
{
    // 针孔/RGBD 相机模型
    class Camera
    {
    public:
        typedef std::shared_ptr<Camera> Ptr;
        float   fx_, fy_, cx_, cy_, depth_scale_;  // 内参

        Camera();   // 定义 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)
        {}

        // 坐标变换:世界,相机,像素
        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. 用命名空间 namespace myslam 将类定义包裹起来
    因为是自己写的 slam,所以命名空间就叫 myslam 了
    命名空间可以防止不小心定义出别的库里同名的函数,也是一种比较安全和规范的做法

  2. 由于宏定义和命名空间在每个文件中都会写一遍
    把一些常用的头文件放在一个 common_include.h 文件中
    这样就可以避免每次书写一个很长的一串 include

  3. 把智能指针定义成 Camera 的指针类型
    因此以后在传递参数时,只需用 Camera::Ptr 类型即可

  4. Sophus::SE3 来表达相机的位姿

在源文件中,给出 Camera 方法的实现 /src/camera.cpp

#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);
    }
}

完成了像素坐标系、相机坐标系和世界坐标系间的坐标变换


4. Frame 类

由于 Frame 类是基本数据单元,在许多地方会用到它
但现在初期设计阶段,还不清楚以后可能新加的内容
所以这里的 Frame 类只提供基本的数据存储和接口
如果之后有新增的内容,就继续往里添加

在源文件中,Frame 类的头文件位于 /include/myslam/frame.h

#ifndef FRAME_H
#define FRAME_H

#include "myslam/common_include.h"
#include "myslam/camera.h"

namespace myslam
{
    class MapPoint;
    class Frame
    {
    public:
        typedef std::shared_ptr<Frame> Ptr;
        unsigned long                  id_;            // 帧的id
        double                         time_stamp_;    // 记录的时间
        SE3                            T_c_w_;         // 从世界到相机的转换
        Camera::Ptr                    camera_;        // 针孔/RGBD相机模型
        Mat                            color_, depth_; // 颜色和深度图像

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

        // 创建 Frame
        static Frame::Ptr createFrame();

        // 寻找给定点对应的深度
        double findDepth(const cv::KeyPoint& kp);

        // 获取相机光心
        Vector3d getCamCenter() const;

        // 判断某个点是否在视野内
        bool isInFrame(const Vector3d& pt_world);
    };
}

#endif // FRAME_H

在 Frame 中,定义了 ID、时间戳、位姿、相机、图像这几个量,这应该是一个帧当中含有的最重要的信息
在方法中,提取了几个重要的方法:创建 Frame、寻找给定点对应的深度、获取相机光心、判断某个点是否在视野内等等
在源文件中,给出 Frame 方法的实现 /src/frame .cpp

#include "myslam/frame.h"

namespace myslam
{
    Frame::Frame()
        : id_(-1), time_stamp_(-1), camera_(nullptr)
    {

    }

    Frame::Frame(long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth)
        : id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth)
    {

    }

    Frame::~Frame()
    {

    }

    // 创建 Frame
    Frame::Ptr Frame::createFrame()
    {
        static long factory_id = 0;
        return Frame::Ptr(new Frame(factory_id++));
    }

    // 寻找给定点对应的深度
    double Frame::findDepth(const cv::KeyPoint& kp)
    {
        int x = cvRound(kp.pt.x);
        int y = cvRound(kp.pt.y);
        ushort d = depth_.ptr<ushort>(y)[x];
        if (d != 0)
        {
            return double(d) / camera_->depth_scale_;
        }
        else
        {
            // 检查附近的地点
            int dx[4] = { -1,0,1,0 };
            int dy[4] = { 0,-1,0,1 };
            for (int i = 0; i < 4; i++)
            {
                d = depth_.ptr<ushort>(y + dy[i])[x + dx[i]];
                if (d != 0)
                {
                    return double(d) / camera_->depth_scale_;
                }
            }
        }
        return -1.0;
    }

    // 获取相机光心
    Vector3d Frame::getCamCenter() const
    {
        return T_c_w_.inverse().translation();
    }

    // 判断某个点是否在视野内
    bool Frame::isInFrame(const Vector3d& pt_world)
    {
        Vector3d p_cam = camera_->world2camera(pt_world, T_c_w_);
        if (p_cam(2, 0) < 0)
            return false;
        Vector2d pixel = camera_->world2pixel(pt_world, T_c_w_);
        return pixel(0, 0) > 0 && pixel(1, 0) > 0
            && pixel(0, 0) < color_.cols
            && pixel(1, 0) < color_.rows;
    }
}

5. MapPoint 类

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

在源文件中,MapPoint 类的头文件位于 /include/myslam/mappoint.h

#ifndef MAPPOINT_H
#define MAPPOINT_H

namespace myslam
{
    class Frame;
    class MapPoint
    {
    public:
        typedef shared_ptr<MapPoint> Ptr;
        unsigned long      id_;         // ID
        Vector3d    pos_;               // 世界坐标系上的坐标
        Vector3d    norm_;              // 观察方向法线
        Mat         descriptor_;        // 描述符匹配
        int         observed_times_;    // 观察时间
        int         correct_times_;     // 正确时间

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

        // 建立MapPoint
        static MapPoint::Ptr createMapPoint();
    };
}

#endif // MAPPOINT_H

在源文件中,给出 MapPoint 方法的实现 /src/mappoint.cpp

#include "myslam/common_include.h"
#include "myslam/mappoint.h"

namespace myslam
{

    MapPoint::MapPoint()
        : id_(-1), pos_(Vector3d(0, 0, 0)), norm_(Vector3d(0, 0, 0)), observed_times_(0), correct_times_(0)
    {

    }

    MapPoint::MapPoint(long id, Vector3d position, Vector3d norm)
        : id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0)
    {

    }

     // 建立MapPoint
    MapPoint::Ptr MapPoint::createMapPoint()
    {
        static long factory_id = 0;
        return MapPoint::Ptr(
            new MapPoint(factory_id++, Vector3d(0, 0, 0), Vector3d(0, 0, 0))
        );
    }
}

6. Map 类

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

在源文件中,Map 类的头文件位于 /include/myslam/map.h

#ifndef MAP_H
#define MAP_H

#include "myslam/common_include.h"
#include "myslam/frame.h"
#include "myslam/mappoint.h"

namespace myslam
{
    class Map
    {
    public:
        typedef shared_ptr<Map> Ptr;
        unordered_map<unsigned long, MapPoint::Ptr >  map_points_;        // 所有路标点
        unordered_map<unsigned long, Frame::Ptr >     keyframes_;         // 所有关键帧

        Map() {}

        void insertMapPoint(MapPoint::Ptr map_point);                     // 插入路标点
        void insertKeyFrame(Frame::Ptr frame);                            // 插入关键帧
    };
}

#endif // MAP_H

Map 类中实际存储了各个关键帧和路标点,既需要随机访问,又需要随时插入和删除,
因此使用散列(Hash)来存储它们

在源文件中,给出 Map 方法的实现 /src/map.cpp

#include "myslam/map.h"

namespace myslam
{
    void Map::insertKeyFrame(Frame::Ptr frame)
    {
        cout << "Key frame size = " << keyframes_.size() << endl;
        if (keyframes_.find(frame->id_) == keyframes_.end())
        {
            keyframes_.insert(make_pair(frame->id_, frame));
        }
        else
        {
            keyframes_[frame->id_] = frame;
        }
    }

    void Map::insertMapPoint(MapPoint::Ptr map_point)
    {
        if (map_points_.find(map_point->id_) == map_points_.end())
        {
            map_points_.insert(make_pair(map_point->id_, map_point));
        }
        else
        {
            map_points_[map_point->id_] = map_point;
        }
    }
}

7. Config 类

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

在源文件中,Config 类的头文件位于 /include/myslam/config.h

#ifndef CONFIG_H
#define CONFIG_H

#include "myslam/common_include.h" 

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

        Config() {} 
    public:
        ~Config(); 

        // 设置一个新的配置文件
        static void setParameterFile(const std::string& filename);

        // 访问参数值
        template< typename T >
        static T get(const std::string& key)
        {
            return T(Config::config_->file_[key]);
        }
    };
}

#endif // CONFIG_H

把构造函数声明为私有,防止这个类的对象在别处建立,它只能在 setParameterFile 时构造
实际构造的对象是 Config 的智能指针: static shared_ptr<Config>config_
用智能指针的原因是可以自动析构,省得再调一个别的函数来做析构

在文件读取方面,使用 OpenCV 提供的 FileStorage
它可以读取一个 YAML 文件,且可以访问其中任意一个字段
由于参数实质值可能为整数、浮点数或字符串,所以通过一个模板函数 get,来获得任意类型的参数值

在源文件中,给出 Config 方法的实现 /src/config.cpp

#include "myslam/config.h"

namespace myslam
{
    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 类后,可以在任何地方获取参数文件里的参数

例如,当想要定义相机的焦距 fx 时,按照以下几个操作步骤即可:

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

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

至此,定义了 SLAM 程序的基本数据结构,书写了若干个基本类
可以调用 cmake 编译这个工程,尽管它还没有实质性的功能


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(44) RGB-D 的直接法
视觉SLAM笔记(43) 直接法
视觉SLAM笔记(42) 光流法跟踪特征点
视觉SLAM笔记(41) 光流
视觉SLAM笔记(40) 特征点的缺陷


发布了217 篇原创文章 · 获赞 292 · 访问量 289万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览