ORB-SLAM2應用練習：三維重建系統搭建 (2)

相機類的抽象

---

上一個博客講的是如何封裝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中的智能指針對其包裝，來實現自動釋放對象。