SLAM学习之路---深度图序列合成场景点云

一、概述

上一篇博客绘制了相机的轨迹，那么有了相机轨迹之后能干什么呢？本篇博客将通过相机轨迹对点云进行拼接合成一个完整的室内场景。合成一个场景需要很多个点云，而这些点云则是通过深度相机扫描得到的一系列深度图序列转换得到的。

在 深度图转换成点云 这篇博客中，使用了 http://redwood-data.org/indoor/dataset.html 网站上的深度图转换成点云。下载了深度图序列文件和相机的轨迹文件，当时还不知道相机轨迹文件的作用是什么，看着网站上的介绍也是云里雾里的，后来过了很久才知道相机轨迹的作用。本文仍然利用这个网站上的数据，通过先将深度图序列转成点云，然后通过相机轨迹文件来拼接点云，最后绘制出相机的轨迹。使用的数据在下图中用红色空选出：

二、深度图转换成点云

在之前的那篇博客中也记录了怎么将深度图转成点云，但是现在看来感觉过程有点繁琐，而且又是Python又是QT的，搞得很麻烦，所以在这里重新梳理一遍。

深度图转点云的过程可以看作是相机成像（当然是对深度成像而不是RGB图像）的逆过程。既然是逆过程，那如果知道了深度图成像的过程，那么逆过程自然就知道了。

深度图成像的过程和RGB图像的成像过程是一样的（都是小孔成像模型），只不过深度图成像得到的像素是单通道的，每个像素存储的是对应点的深度；而RGB则是三通道的，每个像素有三个通道，每个通道分别是RGB三种颜色值。

设空间中有一点$P$， 其在相机坐标系下（以相机光心为原点，在相机后面看，$X$轴朝右，$Y$轴朝下，$Z$轴朝镜头向外）的坐标为$(x,y,z)$，通过小孔成像后，其在成像平面坐标系下（照片几何中心为原点，同样$X$轴朝右，$Y$轴朝下）的坐标为$(a,b)$，同时在像素坐标系下（以照片左上角为原点，$X$轴向右，$Y$轴向下）的坐标为$(u,v)$，此时成像平面坐标系和像素坐标系处于同一个平面，成像平面坐标系下的位置$(a,b)$到像素坐标系下的位置$(u,v)$有如下关系：
$$(a+c_x,b+c_y)=(u,v)$$

小孔成像模型（也是大多数单目相机成像模型）如下图所示，$O_1$为相机坐标系原点，$O_2$为成像平面坐标系原点，$O_3$为像素坐标系原点
【待插入示意图】

由小孔成像模型示意图中的相似三角形关系可知：
$$\frac{f}{z}=\frac{a}{x}=\frac{b}{y}\Rightarrow \frac{f}{z}=\frac{u-c_x}{x}=\frac{v-c_y}{y}$$
因为现在是通过深度图转成点云，也就是知道了$(u,v,d)$（$d$为$(u,v)$这个位置的像素存储的深度值，且$d=z$）和相机的内参 { c x , c y , f } \{c_x, c_y, f\} {cx​,cy​,f}（$f$为相机焦距）来求$(x,y,z)$，所以将上式整理得：
$$\{\begin{array}{l}z=d\\ x=(u-c_x)/f\cdot z\\ y=(v-c_y)/f\cdot z\end{array}$$
通过上面的公式就能够将深度图上的每个像素映射到三维空间中的每个点了。

但是，此时三维空间的点都是在相机坐标系下的位置，而在扫描深度图序列过程中，相机是在时刻运动的，所以通过上述方式将深度图序列转成的点云都是以相机光心为原点的坐标系下的相对位置，每个点云的坐标系原点都是重合的。要想将这些点云拼接合成一个场景，就需要通过相机的轨迹将每帧深度图得到的点云进行刚体变换，从而得到世界坐标系下的位置。

三、刚体变换

三维空间中的刚体变换包含旋转和平移两种，其中旋转通过一个$3\times 3$的旋转矩阵$R$（特殊正交群$SO(3)$）来表示，平移通过$3$维平移向量$tr$表示，可以将旋转和平移使用一个统一的变换矩阵$T$（特殊欧式群$SE(3)$）表示：
$$T=\left[\begin{array}{cc}R& tr\\ \vec{0}& 1\end{array}\right]$$
则相机坐标系下的$P$可以通过变换矩阵转换到世界坐标系下的$P^{\prime }$：
$$P^{\prime }=T\cdot P=\left[\begin{array}{cc}R& tr\\ \vec{0}& 1\end{array}\right][x,y,z,1{]}^t$$
上面的公式中，将$P$转成了齐次坐标形式（4维）从而符合运算。

这样，就能够将每一帧深度图通过 第二步 所得到的相机坐标系下的点云转换到世界坐标系下的点云了，同时也就完成了点云的拼接（注意，这里虽然完成了拼接，但是相邻深度图间存在大量重复，导致点云内有大量冗余点，本文暂不考虑如何去除重复点）。

四、实验结果

相机轨迹如下图所示，$(X,Y,Z)$轴分别用$(R,G,B)$三种颜色表示，如何绘制相机轨迹见SLAM学习之路—绘制相机轨迹这篇博客。

点云转换和拼接结果如下，右边蓝色为每帧深度图所得到的点云，左边绿色为点云拼接结果，由于机器性能原因，左边为每隔15帧拼接一次得到的结果，所以看起来不连续，右边为连续深度图序列得到的点云结果。

五、代码

代码中所用到的 "livingroom1-traj.txt"为 一、概述 中下载的 TXT 文件，“livingroom1-depth-clean” 为下载的 PNG 深度图序列 文件夹。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/ply_io.h>
#include <io.h>

//绘制相机轨迹
void DrawTrajector(const std::vector <Eigen::Isometry3d> &poses)
{
	pcl::visualization::PCLVisualizer visulizer;
	pcl::PointXYZ last_O_pos;
	for (int i = 0; i < poses.size(); i++)
	{
		pcl::PointXYZ O_pose;
		O_pose.x = poses[i].translation()[0];
		O_pose.y = poses[i].translation()[1];
		O_pose.z = poses[i].translation()[2];
		if (i > 0)
			visulizer.addLine(last_O_pos, O_pose, 255, 255, 255, "trac_" + std::to_string(i));

		pcl::PointXYZ X;
		Eigen::Vector3d Xw = poses[i] * (0.1 * Eigen::Vector3d(1, 0, 0));
		X.x = Xw[0];
		X.y = Xw[1];
		X.z = Xw[2];
		visulizer.addLine(O_pose, X, 255, 0, 0, "X_" + std::to_string(i));
		
		pcl::PointXYZ Y;
		Eigen::Vector3d Yw = poses[i] * (0.1 * Eigen::Vector3d(0, 1, 0));
		Y.x = Yw[0];
		Y.y = Yw[1];
		Y.z = Yw[2];
		visulizer.addLine(O_pose, Y, 0, 255, 0, "Y_" + std::to_string(i));

		pcl::PointXYZ Z;
		Eigen::Vector3d Zw = poses[i] * (0.1 * Eigen::Vector3d(0, 0, 1));
		Z.x = Zw[0];
		Z.y = Zw[1];
		Z.z = Zw[2];
		visulizer.addLine(O_pose, Z, 0, 0, 255, "Z_" + std::to_string(i));

		last_O_pos = O_pose;
	}
	visulizer.spin();
}

//深度图转换成点云
void depthImgtoPointCloud(const cv::Mat &img, pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud, const Eigen::Isometry3d &Twr)
{
	double cx = 319.5, cy = 239.5, f = 525.0;
	for (int i = 0; i < img.rows; i++)
	{
		for (int j = 0; j < img.cols; j++)
		{
			double z = img.at<unsigned short>(i, j) / 1000.; //此处也可以不除以1000，尺度相关
			double x = (j - cx) / f * z;
			double y = (i - cy) / f * z;
			Eigen::Vector3d global_pos = Twr * Eigen::Vector3d(x, y, z);
			pointcloud->push_back(pcl::PointXYZ(global_pos[0], global_pos[1], global_pos[2]));
		}
	}
}

//找到目录下所有深度图
std::vector<std::string> findAllDepthImg(std::string path, std::string mode)
{
	std::vector<std::string> files;
	_finddata64i32_t file;
	intptr_t HANDLE;
	std::string Onepath = path + mode;
	HANDLE = _findfirst64i32(Onepath.c_str(), &file);
	if (HANDLE == -1L)
	{
		cout << "can not match the folder path" << endl;
		system("pause");
	}
	do {
		files.push_back(file.name);
	} while (_findnext64i32(HANDLE, &file) == 0);
	_findclose(HANDLE);
	return files;
}


int main()
{
	std::vector<Eigen::Isometry3d> poses;
	ifstream fin("./livingroom1-traj.txt"); 		//读取相机轨迹文件
	int count = 0;
	std::vector<std::string> imgs;
	imgs = findAllDepthImg("./livingroom1-depth-clean/", "*.png"); 

	pcl::visualization::PCLVisualizer visualizer;
	int v1 = 0;
	visualizer.createViewPort(0, 0, 0.5, 1.0, v1);
	visualizer.setBackgroundColor(255, 255, 255, v1);
	visualizer.addText("Merge Result", 5, 5, 20, 255, 0, 0, "v1", v1);
	int v2 = 0;
	visualizer.createViewPort(0.5, 0, 1, 1, v2);
	visualizer.setBackgroundColor(255, 255, 255, v2);
	visualizer.addText("Single Depth-Cloud", 5, 5, 20, 255, 0, 0, "v2", v2);

	for(auto img_name : imgs)
	{
		count++;
		double temp;
		Eigen::Isometry3d Twr;
		fin >> temp >> temp >> temp;
		fin >> Twr(0, 0) >> Twr(0, 1) >> Twr(0, 2) >> Twr(0, 3);
		fin >> Twr(1, 0) >> Twr(1, 1) >> Twr(1, 2) >> Twr(1, 3);
		fin >> Twr(2, 0) >> Twr(2, 1) >> Twr(2, 2) >> Twr(2, 3);
		fin >> Twr(3, 0) >> Twr(3, 1) >> Twr(3, 2) >> Twr(3, 3);
		pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
		cv::Mat img = cv::imread("./livingroom1-depth-clean/" + img_name, -1);
		depthImgtoPointCloud(img, pointcloud, Twr);

		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ch1(pointcloud, 0, 255, 0);
		if(count % 15 == 0 || count == 1) //机器性能影响，所以每15帧拼接一次
			visualizer.addPointCloud(pointcloud, ch1, img_name + "v1", v1);
		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> ch2(pointcloud, 0, 0, 255);
		visualizer.removeAllPointClouds(v2);
		visualizer.addPointCloud(pointcloud, ch2, img_name + "v2", v2);
		visualizer.spinOnce();
		if (count == 1) system("PAUSE");
		poses.emplace_back(Twr);
	}
	DrawTrajector(poses);

	return 0;
}