在上一篇文章的基础上，这里说一下利用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; //如果深度值不为空，实现深度图向彩色图的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<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::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; }