从TUM数据集生成点云以及真实位姿

目标：根据rgb和depth图像生成点云，并且获取点云真实位姿GT

数据集地址：Computer Vision Group - Dataset Download

数据集格式：

1、rgb.txt 和 depth.txt 记录了各文件的采集时间和对应的文件名。

2、rgb/ 和 depth/目录存放着采集到的 png 格式图像文件。彩色图像为八位三通道,深

度图为 16 位单通道图像。文件名即采集时间。

3、groundtruth.txt 为外部运动捕捉系统采集到的相机位姿,格式为(time, t x , t y , t z , q x , q y , q z , q w )

请注意彩色图、深度图和标准轨迹的采集都是独立的,轨迹的采集频率比图像高很多。在使用数据之前,需要根据采集时间,对数据进行一次时间上的对齐,以便对彩色图和深度图进行配对。原则上,我们可以把采集时间相近于一个阈值的数据,看成是一对图像。并把相近时间的位姿,看作是该图像的真实采集位置。

问题说明：

1. rgb图像和depth图像的时间戳不一样，即命名格式不一样，需要使用associate.py文件进行相关联，关联后的图像个数小于原始图像个数。

python associate.py rgb.txt depth.txt >  rgbd_dataset_ associations.txt

1. 关联后的rgb-depth在groundtruth.txt也要找对应的时间戳，这样才能得到对应的真值位姿。groundtruth.txt里的时间戳远远多于rgb-depth的时间戳.

python associate.py groundtruth.txt rgbd_dataset_ associations.txt >  GTpose.txt

出错：系统生成.txt是utf-16的，但是TUM里的.txt文件是utf-8的，所以要注意文件格式问题将utf-16改成utf-8格式

3、生成点云的同时将GTpose.txt里的位姿提取出来(这里也要注意读取的是utf-8格式文件)

//参考：https://www.cnblogs.com/gary-guo/p/6542141.html#commentform
//2019-05-05
//实现了将深度图像和RGB彩色图像转换成RGB点云
//*********注意数据集深度图与rgb图像的对应关系***********
// c++标准库 
#include <iostream>
#include <string>
// opencv库
#include <opencv2/opencv.hpp>
// PCL库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_types.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>//loadPolygonFileOBJ所属头文件；
#include <pcl/io/io.h>

#include <pcl/io/ply_io.h>   
using namespace std;

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 相机内参
const double camera_factor = 5000;
const double camera_cx = 319.5;
const double camera_cy = 239.5;
const double camera_fx = 525.0;
const double camera_fy = 525.0;
void readCameraTrajectory_head(string camTransFile, vector<string>& poses, vector<string>& rgb, vector<string>& depth)
{
	ifstream fcamTrans(camTransFile);
	if (!fcamTrans.is_open())
	{
		cerr << "trajectory is empty!" << endl;
		return;
	}
	else
	{
		string str;
		while ((getline(fcamTrans, str)))
		{
			

			if (str.at(0) == '#') {
				cout << "str" << str << endl;
				continue;
			}
			//1305031102.175304 rgb/1305031102.175304.png 1305031102.160407 depth/1305031102.160407.png 1305031102.175800 1.3405 0.6266 1.6575 0.6574 0.6126 -0.2949 -0.3248
			istringstream strdata(str);
			double a1, b1, c, t1, t2, t3, q1, q2, q3, q4;
			string a2,b2,T;
			strdata >> a1 >> a2 >> b1 >> b2 >> c>>t1 >> t2 >> t3 >> q1 >> q2 >> q3 >> q4;
			cout<<a1<<" "<<b1<<" "<< t1 << " " << t2 << " " << t3 << " " << q1 << " " << q2 << " " << q3 << " " << q4 << endl;
			rgb.push_back(to_string(a1));
			depth.push_back(to_string(b1));
			T = to_string(t1) + " " + to_string(t2) + " " + to_string(t3) + " " + to_string( q1) + " " + to_string(q2) + " " + to_string(q3) + " " + to_string(q4);
			//cout << T << endl;
			poses.push_back(T);
		}
	}
}
// 主函数
int main(int argc, char** argv)
{
	vector<Eigen::Matrix4d> GT_poses;
	vector<string> GT, rgb_pose,depth_pose;

	string ground_Path = "M:\\build_code+data\\data\\TUM\\rgbd_dataset_freiburg1_xyz\\GTpose2.txt";
	string rgb_Path = "M:\\build_code+data\\data\\TUM\\rgbd_dataset_freiburg1_xyz\\rgb\\";
	string depth_PosePath = "M:\\build_code+data\\data\\TUM\\rgbd_dataset_freiburg1_xyz\\depth\\";
	ofstream myout("M:\\build_code+data\\data\\TUM\\rgbd_dataset_freiburg1_xyz\\GT_head.txt", ios::in | ios::out | ios::app);
	readCameraTrajectory_head(ground_Path, GT, rgb_pose, depth_pose);


	for (int i = 0; i < 790; i = i + 10) {

	
	//读取rgb图像和depth图像，并转化为点云
	//图像矩阵
	cv::Mat rgb, depth;
	//使用cv::imread()来读取图像
	//rgb图像是8UC3的彩色图像
	cout << rgb_pose[i] << endl;
	cout << depth_pose[i] << endl;
	rgb = cv::imread(rgb_Path+rgb_pose[i]+".png");
	//depth是16UC1的单通道图像，注意flags设置为-1，表示读取原始数据不做修改
	depth = cv::imread(depth_PosePath + depth_pose[i]  + ".png", -1);

	//点云变量
	//使用智能指针，创建一个空点云。这种指针用完会自动释放
	PointCloud::Ptr cloud(new PointCloud);

	//遍历深度图
	for (int m = 0; m < depth.rows; m++) {
		for (int n = 0; n < depth.cols; n++) {
			//获取深度图中(m,n)处的值
			ushort d = depth.ptr<ushort>(m)[n];
			//d可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			//d存在值，则向点云增加一个点
			PointT p;
			//计算这个点的空间坐标
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = (m - camera_cy) * p.z / camera_fy;
			//从rgb图像中获取它的颜色
			//rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];
			//把p加入到点云中
			cloud->points.push_back(p);
		}
	}
	//设置并保存点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size=" << cloud->points.size() << endl;
	cloud->is_dense = false;
	
	pcl::PLYWriter writer;
	//writer.write("M:\\build_code+data\\data\\TUM\\rgbd_dataset_freiburg1_xyz\\ply\\" + to_string(i) + ".ply", *cloud);  //保存文件
	myout << "VERTEX_SE3:QUAT " << i << " " << GT[i] << endl;;
	清除数据并保存
	cloud->points.clear();
	cout << "Point cloud"<<i<<" saved." << endl;
	}
	return 0;
}