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 <librealsense/rs.hpp>#include <cstdio>#include <iostream>#include <opencv2/opencv.hpp>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<uint16_t>(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<ushort>(y, x);float depth_in_meter = depthVal*scale;//如果深度值不为空，实现深度图向彩色图的alignif (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<ushort>(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::errorprintf("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;}