ORB-SLAM2应用练习：三维重建系统搭建 (4) [END]

系统设计

---

写了这么久，终于可以来到这一步了。按照我们最初的思路，我们已经准备好了跟踪轨迹的SLAM程序、读取图片的Camera类族（我喜欢这么称呼一大堆有血缘关系的类们），以及立体匹配算子的类族，现在开始构造系统的结构。

想想我们系统运作的流程，大概会是：

• 系统的初始化
• 相机的初始化与立体矫正中视差图到深度图映射矩阵的计算
• 图片的读取与预处理
• 立体匹配、计算视差、求深度图
• 根据深度图和原图，在3D空间中打印点云

另外，我希望在点云窗口可以以任意角度观察点云的分布，因此该窗口需要有一些交互动作的监听，所以我们很自然地将绘图放在一个单独的线程中去。根据以上的设计，我们的系统类声明如下：

#pragma once
#include "StereoMatching.h"
#include "CameraFactory.h"
#include 
#include 
#include 
using namespace std;

namespace scs
{
    class System
    {
    public:
        /**
         * @Title: System Constructor
         * @Description: 初始化系统参数，设置立体匹配使用的算法，设置相机类型，加载相机参数，启动画图线程。
         * @param: algorithm, 立体匹配算法名字
         * @param: cameraType, 相机类型
         * @param: dir, 相机参数文件路径(第[0]份存放相机之间的RT矩阵与SLAM要求的参数，第[1][2]份存放相机内参数)
         * @param: name, 相机参数对象在参数文件中的ID
        */
        System(StereoMatching::Type algorithm, CameraFactory::Type cameraType, const string dir[3], const string name[2]);

        ~System();

        /**
         * @Tiitle: run
         * @Description: 系统运行，读图、定位，并三维重建
         * @param: num, 设置处理的帧数量。该值为0时，则一直处理。
        */
        void run(int num = 0);

    private:
        /**
         * @Title: rectify
         * @Description: 相机矫正
        */
        void rectify();

        /**
         * @Title: draw
         * @Description: 绘画线程
        */
        void draw();

    private:
        StereoMatcher * matcher;
        Camera * cam[2];
        Mat R, T, Q, mapx[2], mapy[2];

        vector xyz, color;
        mutex mutexPushData;
        float mViewpointF, mViewpointX, mViewpointY, mViewpointZ, mPointSize;

        // 用于系统析构时，正确结束画图进程.
        bool isFinish, finish;
        mutex mutexIsFinish, mutexFinish;
    };
}

我简要介绍一下该类的接口。首先它处于scs的命名空间下。构造该类时需要四个参数，我的注释说的很清楚了，其中dir包含了SLAM所要求的参数文件路径，而name指的是相机参数在参数文件中的名字，具体可以看[相机类的抽象]这一篇文章对参数对象的介绍。

在构造函数中，除了使用这些参数文件初始化相机和SLAM系统，我们还会调用该类的rectify功能，计算出矫正相机图片的参数以及最重要的立体矫正中视差图到深度图映射矩阵。除此之外，绘图进程也是在该函数中被启动。构造函数代码如下：

scs::System::System(StereoMatching::Type algorithm, CameraFactory::Type cameraType, const string dir[3], const string name[2])
{
    matcher = StereoMatching::make(algorithm);
    for (int i = 0; i < 2; i++)
    {
        cam[i] = CameraFactory::make(cameraType);
        cam[i]->param.load(dir[i+1], name[i]);
        if (!cam[i]->open(i))
        {
            cerr << "CANNOT open camera " << i << endl;
            abort();
        }
    }

    // 需要考虑一些设置失败的情况.
    // TODO
    FileStorage fs(dir[0], FileStorage::READ);
    fs["R"] >> R;
    fs["T"] >> T;
    fs["Viewer.PointSize"] >> mPointSize;
    fs["Viewer.ViewpointX"] >> mViewpointX;
    fs["Viewer.ViewpointY"] >> mViewpointY;
    fs["Viewer.ViewpointZ"] >> mViewpointZ;
    fs["Viewer.ViewpointF"] >> mViewpointF;

    rectify();

    // 这里的参数文件还是遵于SLAM的要求，最好的做法是，SLAM系统的参数由本系统导入.
    // TODO
    slam.init("../Run/Vocabulary/ORBvoc.txt", dir[0], SLAM::eSensor::STEREO, false);

    isFinish = finish = false;
    thread(std::bind(&System::draw, this)).detach();
}

构造函数会根据立体匹配算法的名字和相机类型，使用算子工厂和相机工厂，生成相应的对象。矫正相机以及计算映射矩阵的rectify函数实现如下：

void scs::System::rectify()
{
    Mat Rl, Rr, Pl, Pr;
    stereoRectify(
        cam[0]->param.getIntrinsicMatrix(),
        cam[0]->param.getDistortion(),
        cam[1]->param.getIntrinsicMatrix(),
        cam[1]->param.getDistortion(),
        cam[0]->param.getResolution(),
        R, T, Rl, Rr, Pl, Pr, Q);

    cout
        << "cam1 = " << cam[0]->param.getIntrinsicMatrix() << endl
        << "cam2 = " << cam[1]->param.getIntrinsicMatrix() << endl
        << "disp 1 = " << cam[0]->param.getDistortion() << endl
        << "disp 2 = " << cam[1]->param.getDistortion() << endl
        << "R = " << R << endl
        << "T = " << T << endl
        << "Q = " << Q << endl;

    for (int i = 0; i < 2; i++)
        initUndistortRectifyMap(
            cam[i]->param.getIntrinsicMatrix(),
            cam[i]->param.getDistortion(),
            Rl, Pr,
            cam[i]->param.getResolution(),
            CV_16SC2, mapx[i], mapy[i]);
}

立体矫正使用了opencv自带的函数，函数的用法都是我从网上学来的，其中最重要的矩阵就是Q矩阵了。

一旦系统初始化完毕，我们便可以调用run服务开始读取图片并计算点云和轨迹了。我的思路是：为了不让CPU瞬间爆炸，run接口提供一个可选参数，指定读取多少帧图片；如果该参数为0，则让系统持续读图。run函数的流程大概是：

• 从相机读图
• 对图像进行矫正
• SLAM作跟踪并返回该帧相机位姿
• 计算视差图并求出深度图
• 根据相机位姿，将点云转换到全局坐标下
• 给画图进程添加新的帧

根据以上设计，run函数的实现为：

void scs::System::run(int num)
{
    Mat img[2], disp, disp8, tpose;
    bool slamInit = false;

    for (int i = 0; i < num || num == 0; i++)
    {
        for (int j = 0; j < 2; j++)
        {
            if (!cam[j]->image(img[j])) continue;
        //  因为我使用的数据是已经矫正过的KITTI数据，且我也没有它们相机之间的外参，因此我注释了这一句。
        //  remap(img[j], img[j], mapx[j], mapy[j], INTER_LINEAR);
        }

        // 这里需要多加一些图像非空的判断，以及彩图和灰度图的判断，以增强系统鲁棒性.
        // 这里做简单处理，若在这里出问题，可以考虑是以上的因素造成的.
        // TODO
        tpose = slam.track(img[0], img[1]); 

        if (tpose.data != nullptr)
        {
            if (!slamInit) slamInit = true;

            // 通过视差图，算出深度图.
            disp = matcher->getDisparityImage(img[0], img[1]);
            reprojectImageTo3D(disp, disp8, Q, true);

            // 根据定位信息，将点云坐标转换到全局坐标下.
            for(int i = 0; i < disp8.rows; i++)
                for (int j = 0; j < disp8.cols; j++)
                {
                    Vec3f & p = disp8.at(i, j);
                    Vec3f tmp = p;
                    for (int k = 0; k < 3; k++)
                        p[k] = tpose.at<float>(k, 0) * tmp[0] + tpose.at<float>(k, 1) * tmp[1] + tpose.at<float>(k, 2) * tmp[2];
                }

            unique_lock lock(mutexPushData);
            xyz.push_back(disp8);
            color.push_back(img[0]);
        }
        else if (!slamInit) i--;
    }
}

注意到，对画图进程的帧进行更新时，为了避免冲突，使用mutex加锁进行同步。最后介绍一下系统类的绘图进程。

我选择pangolin帮我绘制点云，以及添加交互动作，它是对opengl的一层封装。如何在window下使用pangolin，我在[Pangolin的配置]一文中谈到过，它的库文件都被我放在我配置的SLAM工程目录下了，对于本系统的搭建来说，也需要用到SLAM，需要的同学请往下看，链接我放在文后了。

该类的绘图函数实现如下：

void scs::System::draw()
{
    pangolin::CreateWindowAndBind("3D Reconstruct", 1024, 768);

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Issue specific OpenGl we might need
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, mViewpointF, mViewpointF, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(mViewpointX, mViewpointY, mViewpointZ, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(0), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    pangolin::OpenGlMatrix Twc;
    Twc.SetIdentity();

    while (1)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        glClearColor(0.125f, 0.6171875f, 1.0f, 1.0f);
        glPointSize(mPointSize);
        glBegin(GL_POINTS);

        {
            unique_lock lock(mutexPushData);
            for (int i = 0, size = xyz.size(); i < size; i++)
            {
                bool oneChannel = color[i].channels() == 1;
                for(int y = 0; y < xyz[i].rows; y++)
                    for (int x = 0; x < xyz[i].cols; x++)
                    {
                        Vec3f p = xyz[i].at<Vec3f>(y, x);
                        if (oneChannel)
                        {
                            double c = color[i].at(y, x);
                            c *= 0.00390625;
                            glColor3d(c, c, c);
                        }
                        else
                        {
                            Vec3d c = color[i].at<Vec3b>(y, x);
                            c *= 0.00390625;
                            glColor3d(c[2], c[1], c[0]);
                        }           
                        glVertex3f(p[0], p[1], p[2]);
                    }
            }
        }

        glEnd();
        pangolin::FinishFrame();
        unique_lock lock(mutexFinish);
        if (finish)
        {
            unique_lock lock(mutexIsFinish);
            isFinish = true;
            return;
        }
    }
}

看过ORB-SLAM2源码的同学，就应该知道我这一段代码很多都是来自它的绘图进程的。注意到，该进程在画点云的时候，使用了刚才追加帧加锁的同一个互斥量，这里就是应该保护的临界区。当系统退出的时候，我们需要终结掉画图进程，因此在析构函数中便有如下实现保证画图进程正确退出：

scs::System::~System()
{
    {
        unique_lock lock(mutexFinish);
        finish = true;
    }

    while (1)
    {
        this_thread::sleep_for(chrono::milliseconds(1000));
        unique_lock lock(mutexIsFinish);
        if (isFinish) break;
    }
}

到这里，我们的系统算是写完了，但是有一些问题没有解决：

• 系统少了一些鲁棒性的检查
• 对SLAM失败等情况没有做处理
• 对点云没有做去重操作

前两者的解决留给读者和未来笔者，当真的有一天需要把这玩意拿到某个台面上的时候，相信未来的我会去改进的。对于最后一点，我想如果我没有走向相关研究的道路，恐怕是不会去解决它的了。如今该系统作为一个小练习，也不纠结那么多啦，现在来看看我们的系统运作地怎么样。

运行

---

添加main.cpp如下所示：

#include "System.h"
#include 
#include 
#include 
using namespace std;
using namespace scs;

/* Input: 
G:/Data/Natural/KitData/sequences/03/image_0 png 6 0 300
G:/Data/Natural/KitData/sequences/03/image_1 png 6 0 300
*/
int main()
{
    string dir[3], name[2];
    for (int i = 0; i < 3; i++) dir[i] = "../Run/Setting/KITTI03_For_3D_Restruct.yaml";
    for (int i = 0; i < 2; i++) name[i] = string("Cam") + char(1 + i + '0');

    System sys(StereoMatching::SGBM, CameraFactory::ImageReader, dir, name);
    sys.run(1);
    int n;
    cin >> n;
    return 0;
}

PS：最后的cin语句是作为卡住主线程之用，请别介意这种诡异的写法。

配置好相关的库，如SLAM与pangolin，将vs的生成模式调为库对应的版本，如果用的是我的工程的话，那么就是Release x64. 配置相应的运行环境，然后编译生成可执行文件，运行。

可以看到我使用相机是ImageReader，其实就是个从本地读图的家伙，我使用的数据集是KITTI上的标准数据集，已经经过矫正了（我会把参数文件和数据集的一两帧放到最后的工程文件夹下），运行的时候，相机提醒我输入图片的路径与其他信息（如main.cpp上的注释那样），输入后便可以看到3D重建后的效果了：

这实际上是重建一帧的结果，你们可以自己写好代码，或者下载我的代码后，尝试多跑几帧看看，我的电脑真的受不了太多帧。该窗口的左键和右键都能移动观察的视角，一开始要是没看到点云，请用右键旋转四周找找看。

总结

---

无论如何，这个系统终于实现好了，虽然存在不少问题，比如说点云的处理是相当缓慢的，这应该可以通过某种技术进行加速，另外由于track的误差与没有实现去重，显示多帧的时候会明显看到重影的现象。希望看了我的代码和文章的朋友能够指点我可以怎么去改进该系统，其中如果有错误的地方，也麻烦帮我指出！

我把代码放在这里：http://download.csdn.net/download/yfic000/9968752
本来想着不要设置积分，但没想到最低积分只能是1，也没办法了。这份资源我就没有将它打包成工程的模样，这个工作就留给你们自己完成吧，相信难度不会太大。
另外，ORB-SLAM2在window上的配置的工程可以在这里下载：http://download.csdn.net/download/yfic000/9913200
不会配置工程的同学，可以参考参考这个工程中的属性设置和属性表等信息，其相关的博文为：ORB-SLAM2在window下的配置

欢迎大家一起交流学习，本系列博文到此就结束了。

---

修改：贴上了代码链接，同时发现本文以及代码中有一处错误，在计算每帧点云的全局坐标处，使用了错误的公式，在文中已经改正，但是由于资源上传时还没来得及发现错误，所以本资源还略有问题，似乎CSDN的资源无法重新修改，也不想删了再发了，特此注记，诸位还是以文章为主。——2017-09-07