系统设计
写了这么久,终于可以来到这一步了。按照我们最初的思路,我们已经准备好了跟踪轨迹的SLAM程序、读取图片的Camera类族(我喜欢这么称呼一大堆有血缘关系的类们),以及立体匹配算子的类族,现在开始构造系统的结构。
想想我们系统运作的流程,大概会是:
- 系统的初始化
- 相机的初始化与立体矫正中视差图到深度图映射矩阵的计算
- 图片的读取与预处理
- 立体匹配、计算视差、求深度图
- 根据深度图和原图,在3D空间中打印点云
另外,我希望在点云窗口可以以任意角度观察点云的分布,因此该窗口需要有一些交互动作的监听,所以我们很自然地将绘图放在一个单独的线程中去。根据以上的设计,我们的系统类声明如下:
#pragma once
#include "StereoMatching.h"
#include "CameraFactory.h"
#include <string>
#include <mutex>
#include <vector>
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<Mat> 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[