RealSense D435 输出世界坐标系的csv文件

第一篇博客，最近在实习，一直在做机器视觉的研究。今天简单的介绍一个RealsenseD435的应用。

RealsenseD435是一款深度摄像头，可以快读的输出深度图像。首先了解一下什么事世界坐标系，相机坐标系，和图像坐标系，这里给大家推荐一篇博客。https://blog.csdn.net/waeceo/article/details/50580607

在Viewer里我们可以很轻松地得到相机坐标系下的深度图。但是在应用中我们有时候会需要世界坐标系下的深度图，这种时候就需要进行坐标系转换。

而且Realsense只能输出.ply的深度图，我(其实是boss)更想得到csv结果，以方便后续利用点云进行数据处理。

那么不多废话，如何得到世界坐标系下的csv三维坐标图呢？

std::ofstream outFile_original;
std::stringstream csv_file_original;
csv_file_original << "Original rs-save-to-disk-output-" << count_image << "-metadata.csv";
outFile_original.open(csv_file_original.str(), std::ios::app);
outFile_original << "X" << ';' << "Y" << ';' << "Z" << ';' << "scalar_X" << ';' << "scalar_Y" << ';' << "scalar_Z" << std::endl;

这是csv文件的表头，方便我们用pointcloud读取文件进行操作。

然后遍历深度图每一个pixel，根据公式求出该点的三维世界坐标系。

这是我的读取深度图，然后利用深度值得到点的三维坐标（相机坐标系）转换得到世界坐标系

static void Hauteur_Y(const rs2_intrinsics& intr, const rs2::depth_frame& frame, pixel u, std::ofstream *outFile, double H, double D, double angle, float wpoint[3],double dis_max,double dis_min)

{
	float upixel[2]; // From pixel
	float upoint[3]; // From point (in 3D)
	float xpoint[3];
	float extrin[4][4] = { { 1,0,0,0 },{ 0,cos(angle),sin(angle),H },{ 0,-sin(angle),cos(angle),0 },{ 0,0,0,1 } };
	upixel[0] = u.first;
	upixel[1] = u.second;
	auto udist = frame.get_distance(upixel[0], upixel[1]);
	rs2_deproject_pixel_to_point(upoint, &intr, upixel, udist);
	float b[4] = { upoint[0],upoint[1],upoint[2],1 };
	float out[4];
	for (int i = 0; i < 4; i++)
	{
		float s = 0;
		for (int k = 0; k < 4; k++)
			s += extrin[i][k] * b[k];
		out[i] = s;

	};
	if (udist < dis_max && udist > dis_min)
	{
		*outFile << out[0] << ';' << out[2] << ';' << -out[1] << ';' << out[0] << ';' << out[2] << ';' << -out[1] << ';' << "\n";
		wpoint[0] = out[0];
		wpoint[1] = out[2];
		wpoint[2] = -out[1];
	}
}

 

输入相机参数，根据相机位置进行坐标变换。

 

 

获得的深度图

 

 

根据深度图生成的世界坐标系下的三维点云。

 

 

完成！

本小白做的六个月机器视觉的实习，第一次写博客有很多不到位的地方请大家谅解，有什么疑问可以留言私信给我，之后还会继续写Realsense的一些总结（才不是为了写报告！）

xD