ORB-SLAM2应用练习：三维重建系统搭建 (2)

来源：互联网发布：金手指手机炒股软件编辑：程序博客网时间：2024/06/06 19:58

相机类的抽象

上一个博客讲的是如何封装ORB-SLAM2，使其成为一项定位的服务，此举简单地解决了三维重建系统中每一帧的定位问题。该系统的输入是一组图像序列，目前来说，我们图像的输入是来自于磁盘，但或许以后系统实现实时的时候，图像的来源也可以是相机，所以我决定采用不失扩展性的方法来编写程序。

首先新建一个文件：Camera.h

为了让代码可以扩充各种各样的相机，我们先要约定好相机的标准。按经验，相机一般有三种操作：打开，关闭，读取一张图片。因此我们创建一个抽象的Camera类，作为其子类的接口约束：

    class Camera    {    public:        virtual bool open(int index) = 0;               virtual void close() = 0;        virtual bool image(Mat & im) = 0;        virtual ~Camera() {}       };

一般来说，相机的打开会指定相机编号，因此open函数传入一个整数参数，作为相机编号。image函数表示从相机获得一张图片，并返回是否获得成功的标志。需要注意的是，该抽象类的析构函数需要是virtual的，以便使其子类能够正确析构。

但这样一个相机类也未免比较简单。在实际使用中，我们往往需要知道相机的一些参数，如其内矩阵、畸变向量，甚至是特殊相机的特殊参数，这些不仅在OBR-SLAM2中有用到，在三维重建的时候也是需要的。因此我们为Camera增设一个类，Parameter：

    class Parameter    {    public:        /**         * @Title: load / save         * @Description: Set/save camera parameter by .yaml file.         * @param: dir, path of the file.         * @param: cam, name of the Parameter object in the setting file.        */        virtual void load(const string & dir, const string & cam);        virtual void save(const string & dir, const string & cam);        void setIntrinsicMatrix(double fx, double fy, double cx, double cy);        void setDistortionVector(double k1, double k2, double p1, double p2, double k3 = 0.0);        void setResolution(int width, int height);        Mat getIntrinsicMatrix() const { return intrinsic; }        Mat getDistortion() const { return distortion; }        Size getResolution() const { return resolution; }    protected:        // These write and read functions must be defined for the serialization in FileStorage to work        friend void write(FileStorage & fs, const string & dir, const Parameter & x);        friend void read(const FileNode & node, Parameter & x, const Parameter & default_value);        Mat intrinsic, distortion;        Size resolution;    }

ORB-SLAM2是通过.yaml文件进行参数设置的，我打算仿照它的那种做法，因此，我的相机参数设置也是通过.yaml来操作的，具体实现在load函数与read函数：

void Camera::Parameter::load(const string & dir, const string & cam){    FileStorage fs(dir, FileStorage::READ);    fs[cam] >> (*this);}void read(const FileNode & node, Camera::Parameter & x, const Camera::Parameter & default_value){    if (node.empty())        x = default_value;    else    {        double fx, fy, cx, cy, k1, k2, p1, p2, k3, width, height;        node["fx"] >> fx;        node["fy"] >> fy;        node["cx"] >> cx;        node["cy"] >> cy;        node["k1"] >> k1;        node["k2"] >> k2;        node["p1"] >> p1;        node["p2"] >> p2;        node["k3"] >> k3;        node["width"] >> width;        node["height"] >> height;        x.setIntrinsicMatrix(fx, fy, cx, cy);        x.setDistortionVector(k1, k2, p1, p2, k3);        x.setResolution(width, height);    }}

用opencv处理.yaml文件的类FileStorage来实现，但具体过程其会调用read这个重载函数。注意到，它不是属于Parameter类的函数，而是它的友元，其意义是我们仍认为它是Parameter的组成成分，但是为了使FileStorage类正常工作（读写自定义类对象，没有我们说明白，怎么可能正常工作呢？），我们需要让read去重载全局域中的函数。

我们从参数文件中，读取到焦点、光轴和分辨率信息，使用以下函数将其设置在类中：

void Camera::Parameter::setIntrinsicMatrix(double fx, double fy, double cx, double cy){    intrinsic = Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1;}void Camera::Parameter::setDistortionVector(double k1, double k2, double p1, double p2, double k3){    distortion = Mat_<double>(5, 1) << k1, k2, p1, p2, k3;}void Camera::Parameter::setResolution(int width, int height){    resolution.width = width;    resolution.height = height;}

除此之外，在Parameter类中我也顺便提供了参数写出的接口：

void Camera::Parameter::save(const string & dir, const string & cam){    FileStorage fs(dir, FileStorage::WRITE);    fs << cam << (*this);}void write(FileStorage & fs, const string & dir, const Camera::Parameter & x){    fs << "{"        << "fx" << x.intrinsic.at<double>(0, 0)        << "fy" << x.intrinsic.at<double>(1, 1)        << "cx" << x.intrinsic.at<double>(0, 2)        << "cy" << x.intrinsic.at<double>(1, 2)        << "k1" << x.distortion.at<double>(0, 0)        << "k2" << x.distortion.at<double>(1, 0)        << "p1" << x.distortion.at<double>(2, 0)        << "p2" << x.distortion.at<double>(3, 0)        << "k3" << x.distortion.at<double>(4, 0)        << "width" << x.resolution.width        << "height" << x.resolution.height        << "}";}

Parameter作为相机的组成成分之一，它应该作为Camera类的内部类存在。另外，为了区分其他框架的函数接口，我给我的三维重建系统设计到的类、函数，封装在命名空间scs中，因此完整的Camera.h如下所示：

#pragma once#include <opencv2/opencv.hpp>#include <string>using std::string;using cv::Mat;using cv::Size;using cv::FileStorage;using cv::FileNode;namespace scs{    class Camera    {    public:        class Parameter        {        public:            /**             * @Title: load / save             * @Description: Set/save camera parameter by .yaml file.             * @param: dir, path of the file.             * @param: cam, name of the Parameter object in the setting file.            */            virtual void load(const string & dir, const string & cam);            virtual void save(const string & dir, const string & cam);            void setIntrinsicMatrix(double fx, double fy, double cx, double cy);            void setDistortionVector(double k1, double k2, double p1, double p2, double k3 = 0.0);            void setResolution(int width, int height);            Mat getIntrinsicMatrix() const { return intrinsic; }            Mat getDistortion() const { return distortion; }            Size getResolution() const { return resolution; }        protected:            // These write and read functions must be defined for the serialization in FileStorage to work            friend void write(FileStorage & fs, const string & dir, const Parameter & x);            friend void read(const FileNode & node, Parameter & x, const Parameter & default_value);            Mat intrinsic, distortion;            Size resolution;        } param;    public:        virtual bool open(int index) = 0;        virtual void close() = 0;        virtual bool image(Mat & im) = 0;        virtual ~Camera() {}       };} /// Namespace scs

Camera类的实现太长就不贴上来了，在本系列博客结束的时候，我会把它上传到资源里边去。

现在，这个Camera类只是一个抽象类，它还没有实际作用，只作为接口标准，我现在需要派生一个特定的相机来实例化。下面我就举一个例子，这个相机称之为ImageReader，顾名思义，它就是个从磁盘读图像的相机。其它类型的相机，如JAI、Pylon、Balser，或者是普通的可以使用opencv函数打开的相机，实现的基本步骤跟下面这个类的都差不多：

namespace scs{    class ImageReader : public Camera    {       public:        bool open(int index) override;        void close() override {}        bool image(Mat & im) override;        void reset() { iterator = start; }    private:        string dir, suffix;        int width, start, end, iterator;    };}

注意到，该类对Camera类中的三个纯虚函数作了override，其他内容则是ImageReader特殊的部分。对于一个从磁盘读取图像的相机来说，close函数似乎没有什么作用，因此直接是个空的。在磁盘上的图像序列（我们平常说的数据集），一般在命名方式上有一定规律：即有特定的前缀或后缀，使用固定场宽的连续数字来标识，在这里我参考KITTI的数据集假定，图像的命名诸如000000.png的格式，因此我的ImageReader类中有那几个成员变量：

dir：指定数据集的路径
suffix：指定图像格式，jpg或png等等
width：数字场宽
start：图像开始编号
end：最后一张的编号
iterator：标识现在到了第几张图像

跟iterator相关的操作——reset函数用于复位iterator，使ImageReader从头开始读图。

基于以上介绍可知，open函数就是用来设置ImageReader的特殊参数的，image函数则根据这些参数从磁盘读图，它们的实现为：

// ImageReader.cpp#include "ImageReader.h"#include <iostream>using namespace cv;using namespace std;bool scs::ImageReader::open(int index){    cout        << "Open Camera " << index << ": " << endl        << "- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - " << endl        << "Input |   dir   |   suffix   |   width   |   start   |   end   |" << endl        << ">>>: ";    cin >> dir >> suffix >> width >> start >> end;    reset();    return true;}bool scs::ImageReader::image(Mat & frame){    char name[256];    if (iterator > end)    {        reset();        frame = Mat();    }    sprintf_s(name, "%s/%0*d.%s", dir.c_str(), width, iterator++, suffix.c_str());    frame = imread(name);    return frame.data != nullptr;}

这样，一个简单的可实例化的相机类就实现了，往后我们可以为不同厂家的相机都编写一个继承于Camera类的类，对复杂的相机SDK做一个封装，供用户实现。

但太多的相机类，对于用户来说也许也比较眼花缭乱，于是我们希望进一步封装，使用户仅关心Camera这个类就好了，因此我们引进“相机工厂”，为我们制造各种各样的相机：

// CameraFactory.h#pragma once#include "Camera.h"namespace scs{    class CameraFactory    {    public:        enum Type        {            ImageReader        };    public:        static Camera * make(CameraFactory::Type type);    };}

现在，用户要使用相机时，仅需要知道相机在CameraFactory中的型号就好了，各种各样烦人的头文件（如ImageReader.h，JAI.h, Pylon.h，Balser.h等等）全都封装了起来，这一步如同封装ORB-SLAM2一般。该工厂类的实现是：

// CameraFactory.cpp#include "CameraFactory.h"#include "ImageReader.h"namespace scs{    Camera * CameraFactory::make(CameraFactory::Type type)    {        switch (type)        {        case scs::CameraFactory::ImageReader:            return new scs::ImageReader;        default:            return nullptr;        }    }}

各种相机的头文件将会在CameraFactory.cpp中出现，但一旦封装起来，导出dll之后，那些头文件用户就不需要在意了。

好了，到这里，我们需要测试一下我们的相机能不能正常工作。将main.cpp的内容改为如下代码：

#include "CameraFactory.h"#include <opencv2/opencv.hpp>using namespace cv;using namespace scs;int main(){    Camera * cam = CameraFactory::make(CameraFactory::Type::ImageReader);    cam->open(0);    Mat frame;    int c = 0;    while (c != 27 && cam->image(frame))    {        imshow("Frame", frame);        c = waitKey(10);    }    return 0;}

编译之后，成功的话，就可以看到输入参数的提示了。这份代码有个问题需要注意的是，cam作为一个指针，接受工厂的对象之后，使用完需要自己释放，这样多少有些不方便。我们可以使用C++11中的智能指针对其包装，来实现自动释放对象。

阅读全文

0 0