win下使用realsense进行数据修正并获取三维坐标

在上一篇文章的基础上，这里说一下利用realsense获取经过修正过的数据流，即color_aligned_to_depth和depth_aligned_to_color；同时利用realsense获取目标实际三维坐标。

首先环境设置、主要流程和上一篇一样。这里说一下，最开始的时候我以为读取color_aligned_to_depth，就要在设置数据流的时候，设置color_aligned_to_depth，但是后来才知道，数据流设置不变，只是读取的时候选择想要的数据流就可以了。此外，在读取depth_aligned_to_color数据流的时候，会报错。关于这个问题，我看了git上说明，好像是个bug。所以这里我们不能直接读取深度向彩色修正的数据流，但是我们可以绕过这个问题，换种方式同样获得向彩色修正过的深度数据。

因为不能够直接读取depth_aligned_to_color的深度数据，所以这里，我通过读取深度图中的点，转换为三维坐标，然后再转换为彩色图中的点，这样就实现了深度图向彩色图转换的对应关系，依据这种关系生成depth_aligned_to_color的深度图。

代码如下：

#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int main() try
{
	// 创建context对象，用来控制所有设备。
	rs::context ctx;
	printf("There are %d connected RealSense devices.\n", ctx.get_device_count());
	if (ctx.get_device_count() == 0) return EXIT_FAILURE;

	// 这里只有一个设备。
	rs::device * dev = ctx.get_device(0);
	printf("\nUsing device 0, an %s\n", dev->get_name());
	printf("    Serial number: %s\n", dev->get_serial());
	printf("    Firmware version: %s\n", dev->get_firmware_version());

	//设备设置数据流只有两种，彩色和深度。
	dev->enable_stream(rs::stream::depth, 640, 480, rs::format::z16, 30);
	dev->enable_stream(rs::stream::color, 640, 480, rs::format::bgr8, 30);

	dev->start();

	// 这里dev->get_depth_scale()返回深度值和实际尺度的比例，0.001。
	const uint16_t one_meter = static_cast(1.0f / dev->get_depth_scale());

	// 获取深度、彩色数据流的内参，同时深度向彩色数据流转换的外参。
	rs::intrinsics depth_intrin = dev->get_stream_intrinsics(rs::stream::depth);
	rs::extrinsics depth_to_color = dev->get_extrinsics(rs::stream::depth, rs::stream::color);
	rs::intrinsics color_intrin = dev->get_stream_intrinsics(rs::stream::color);
	float scale = dev->get_depth_scale();

	bool flag = true;
	while (flag)
	{
		dev->wait_for_frames();

		// 读取数据。
		uint16_t * depth_frame = (uint16_t *)(dev->get_frame_data(rs::stream::depth));
		uchar * color_frame = (uchar*)(dev->get_frame_data(rs::stream::color));
		uchar * color_to_depth_frame = (uchar*)(dev->get_frame_data(rs::stream::color_aligned_to_depth));

		// 把数据保存为opencv的Mat数据格式。
		Mat depthImg(480, 640, CV_16UC1, depth_frame);
		Mat colorImg(480, 640, CV_8UC3, color_frame);
		Mat color_aligned_to_depth(480, 640, CV_8UC3, color_to_depth_frame);

		// 这里进行深度图向彩色图的修正。
		Mat depth_aligned_to_color(480, 640, CV_16UC1, Scalar(0));
		for (int y = 0; y < 480; y++)
		{
			for (int x = 0; x < 640; x++)
			{
				ushort depthVal = depthImg.at(y, x);
				float depth_in_meter = depthVal*scale;

				//如果深度值不为空，实现深度图向彩色图的align
				if (depthVal != 0)
				{
					// 这里实现了深度图中的点转换为三维坐标；再转换为彩色图中坐标。
					rs::float2 depth_pixel = { (float)x, (float)y };
					rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meter);//此处一定要为float型。
					rs::float3 color_point = depth_to_color.transform(depth_point);
					rs::float2 color_pixel = color_intrin.project(color_point);

					int n = 0, m = 0;
					m = cvFloor(color_pixel.x);
					n = cvFloor(color_pixel.y);

					// 要保证深度图向彩色图转换过去的点在合理范围内才会被保存。
					if (n >= 0 && n<480 && m >= 0 && m<640)
					{
						depth_aligned_to_color.at(n, m) = depthVal;
					}
				}
			}
		}

		imshow("color", colorImg);
		imshow("depth", depthImg);
		imshow("depth_aligned_to_color", depth_aligned_to_color * 10);
		imshow("color_aligned_to_depth", color_aligned_to_depth);
		waitKey(10);

	}

	return EXIT_SUCCESS;
}
catch (const rs::error & e)
{
	// Method calls against librealsense objects may throw exceptions of type rs::error
	printf("rs::error was thrown when calling %s(%s):\n", e.get_failed_function().c_str(), e.get_failed_args().c_str());
	printf("    %s\n", e.what());
	return EXIT_FAILURE;
}