ORB-SLAM2稠密点云重建:RGBD室内[1]

接上一篇.

步骤2：了解PCL

在 PCL文档中简单了解其基本用法和工具。我在其官方教程中学习并使用了PCL的一些滤波工具。

步骤3：准备工作

首先，根据ORB-SLAM2的git主页，安装好ORB-SLAM2（注意openCV"干净"安装，即如果你要重装openCV,要完整卸载之前版本，具体百度），选择你的数据集跑RGB-D的例子。此时可以得到 KeyFrameTrajectory.txt。我使用了TUM数据集上的rgbd_dataset_freiburg1_room，因为它拥有闭环检测，比较精确。我在D盘创建了“SLAM”文件夹，把数据集放进去了。然后把KeyFrameTrajectory.txt也复制进文件夹，重命名为RGBD_RoomTraj.txt。我把association文件也复制进去，重命名为RGBD_RoomAssociations.txt。至此，程序输入数据就准备完成了。

步骤4：声明主函数

我把主函数称为JoinPC（意思为JoinPointCloud，也许起得不好，将就吧）。

用法：

JoinPC("D:\\SLAM\\RGBD_RoomTraj.txt","D:\\SLAM\\RGBD_RoomAssociations.txt","D:\\SLAM\\rgbd_dataset_freiburg1_room");

原型：

void JoinPC(string camTransFile,string associateFile,string dataMainPath)

步骤5：新建VS项目，添加高翔博士的代码

新建VS C/C++控制台项目，配置好OpenCV和PCL（我使用openCV2411和PCL1.8.1）。添加高翔博士第四讲中的slamBase.h和slamBase.cpp。

步骤6：修改ParameterReader，新建参数文件

对于我使用的TUM数据集room建立对应相机的参数文件，参数我在TUM上找到了，下面贴出参数文件内容：

detector=ORB
descriptor=ORB
good_match_threshold=10

# camera
camera.cx=318.6;
camera.cy=255.3;
camera.fx=517.3;
camera.fy=516.5;
camera.scale=5000.0;

我重命名其为RGBDparameters.txt，放入VS项目代码的同目录下。

由于要使用这个文件，所以对高博的ParameterReader类进行修改:

class ParameterReader
{
public:
	ParameterReader(string filename = "RGBDparameters.txt")
...

然后就可以用了。向主函数填充代码：

void JoinPC(string camTransFile,string associateFile,string dataMainPath) {
	ParameterReader pd;
	// 相机内参
	CAMERA_INTRINSIC_PARAMETERS camera;
	camera.fx = atof(pd.getData("camera.fx").c_str());
	camera.fy = atof(pd.getData("camera.fy").c_str());
	camera.cx = atof(pd.getData("camera.cx").c_str());
	camera.cy = atof(pd.getData("camera.cy").c_str());
	camera.scale = atof(pd.getData("camera.scale").c_str());

步骤7：新建两个类，读取关键帧文件信息，association文件信息

新建类XCTool，并添加3个东西：XCKey类读取关键帧信息，XCAssociationKey类读取关联信息，FindDFileByRGB函数顾名思义。

XCTool.h:

#pragma once
#include <string>
#include <vector>
using std::string;
using std::vector;
class XCTool
{
public:

};

class XCKey {
public:
	string frameID;
	double tx, ty, tz;
	double qx, qy, qz, qw;
};

class XCAssociationKey {
public:
	string rgb, full_rgb, d, full_d;
};

class XCKITTIKey {
public:
	double r00, r01, r02, r10, r11, r12, r20, r21, r22;
	double tx, ty, tz;
};

string FindDFileByRGB(vector<XCAssociationKey>& aKeyVec, string rgb);

XCTool.cpp:

#include "stdafx.h"
#include "XCTool.h"

string FindDFileByRGB(vector<XCAssociationKey>& aKeyVec, string rgb) {
	for (auto& iter : aKeyVec) {
		if (iter.rgb == rgb) {
			return iter.d;
		}
	}
	abort();
}

向主函数添加代码，目前是：

void JoinPC(string camTransFile,string associateFile,string dataMainPath) {
	ParameterReader pd;
	// 相机内参
	CAMERA_INTRINSIC_PARAMETERS camera;
	camera.fx = atof(pd.getData("camera.fx").c_str());
	camera.fy = atof(pd.getData("camera.fy").c_str());
	camera.cx = atof(pd.getData("camera.cx").c_str());
	camera.cy = atof(pd.getData("camera.cy").c_str());
	camera.scale = atof(pd.getData("camera.scale").c_str());

	ifstream fcamTrans(camTransFile);
	vector<XCKey> keyVec;
	while (!fcamTrans.eof()) {
		XCKey tkey;
		fcamTrans >> tkey.frameID;
		fcamTrans >> tkey.tx;
		fcamTrans >> tkey.ty;
		fcamTrans >> tkey.tz;
		fcamTrans >> tkey.qx;
		fcamTrans >> tkey.qy;
		fcamTrans >> tkey.qz;
		fcamTrans >> tkey.qw;
		keyVec.push_back(tkey);
	}

	ifstream fAssociation(associateFile);
	vector<XCAssociationKey> assoKeyVec;
	while (!fAssociation.eof()) {
		XCAssociationKey takey;
		fAssociation >> takey.rgb;
		fAssociation >> takey.full_rgb;
		fAssociation >> takey.d;
		fAssociation >> takey.full_d;
		assoKeyVec.push_back(takey);
	}

	vector<string> rgbPathVec, dPathVec;
	for (int i = 0; i < keyVec.size(); i++) {
		rgbPathVec.push_back(dataMainPath + "\\rgb\\" + keyVec[i].frameID + ".png");
		dPathVec.push_back(dataMainPath + "\\depth\\" + FindDFileByRGB(assoKeyVec,keyVec[i].frameID)+".png");
	}

这样，keyVec中保存了关键帧信息。rgbPathVec, dPathVec就保存了rgb图路径和深度图路径，是一一对应的。

ORB-SLAM2稠密点云重建:RGBD室内[1]

猜你喜欢