devbox
IT小白
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

C++ api调用realsense d435相机，将坐标转换到相机坐标系_realsense 编程

作者：IT小白 | 2024-02-08 21:17:42

赞

踩

realsense 编程

第一步，当然是包含头文件啦

  1. #include <librealsense2/rs.hpp>
  2. #include<librealsense2/rsutil.h>

第二步，初始化RealSense相机的上下文，能查看多个相机，以及相机的状态和信息。

  1. auto devs = ctx.query_devices();  // Get device list
  2. int device_num = devs.size();
  3. std::cout << "获取相机设备号：" << device_num << std::endl; // Device amount
  4.

第三步，创建数据管道和数据流参数文件，对数据流参数进行设置，包括启动深度流还是彩色流，分辨率等。

  1. //创建数据管道和参数文件
  2. rs2::pipeline pipe;
  3. rs2::config cfg;
  4. cfg.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
  5. cfg.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);

        在使用Intel RealSense相机进行编程时，首先需要创建一个rs2::pipeline对象，并使用该对象启动相机的数据流。在启动数据流后，相机将根据配置的参数生成相应的数据流，例如深度、彩色或红外流，并将这些数据传输到计算机中。

  • RS2_STREAM_DEPTH：指定启用的流类型为深度流，即获取相机输出的深度图像。
  • 640和480：指定深度图像的分辨率为640x480。
  • RS2_FORMAT_Z16：指定深度图像的像素格式为Z16，即每个像素用16位整数表示深度值。
  • 30：指定深度流的帧率为30帧

第四步，创建设备和一个流配置的组合，给接口提供访问流配置，设别信息和相应传感器的选项。

最最重要的是，我们能通过该接口直接拿到相机的内参。

  1.     //start()函数返回数据管道的profile
  2.     rs2::pipeline_profile profile = pipe.start(cfg);

        在获取数据流之前，需要首先通过调用rs2::pipeline::start()方法启动数据流，并使用返回的rs2::pipeline_profile对象来访问流的配置和设备信息。通过rs2::pipeline_profile对象可以获取相机的设备序列号、固件版本、传感器列表以及每个传感器的选项和特性。同时，还可以通过rs2::pipeline_profile对象访问深度流、彩色流或其他流的分辨率、帧速率、格式等配置参数，以便进行后续的数据处理和分析。

获取深度图内参，

  1.     //获取内参
  2.     auto stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
  3.     const auto intrinsics = stream.get_intrinsics();

先获取深度流的视频流配置信息，以便进一步使用这些信息对深度数据进行处理和分析，比如获取深度内参。

具体而言，程序首先调用rs2::pipeline_profile::get_stream()方法获取深度流的流配置信息，然后使用as<rs2::video_stream_profile>()方法将其转换为rs2::video_stream_profile类型的对象。这样可以方便地访问深度流的分辨率、内参，帧率、像素格式等配置参数。

RealSense相机的内参包括深度图内参和彩色图内参，下面给出一个获取两种内参的简单示例：

  1. rs2::config cfg;
  2. cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
  3. rs2::pipeline pipe;
  4. auto profile = pipe.start(cfg).get_active_profile();
  5.  
  6. auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
  7. auto depth_intrinsics = depth_stream.get_intrinsics();
  8.  
  9. auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
  10. auto color_intrinsics = color_stream.get_intrinsics();

这里回到我们的代码中来，stream.get_intrinsics();函数获取深度内参，内参矩阵是相机的重要参数之一，描述了相机的内部几何结构，包括相机的焦距、主点位置和像素尺寸等信息。在计算深度图像和三维点云时，需要使用深度相机的内参矩阵对深度值进行校正和转换，以得到准确的三维坐标。

  1.     rs2::align align_to_color(RS2_STREAM_COLOR);
  2.     rs2::frameset frameset;

rs2::align类是一个帮助类，用于将不同传感器的帧对齐，以便于将它们组合在一起进行处理。在该程序中，通过RS2_STREAM_COLOR参数指定将深度帧对齐到彩色帧。这样，对齐后的深度帧和彩色帧具有相同的时间戳和空间参考系，可以方便地进行联合处理。

rs2::frameset类是一个帧集合类，用于获取一组关联的帧。在该程序中，rs2::frameset对象用于存储从相机获取的多个帧，包括彩色图像、深度图像、红外图像等。通过获取这些帧，可以进行基于深度数据的三维重建、物体检测、姿态估计等应用。同时，通过使用帧集合类，还可以实现多帧数据的同步采集、保存和回放等功能。

将深度帧对齐到彩色帧和将彩色帧对齐到深度帧的区别：

将深度帧对齐到彩色帧的操作是一种常见的操作，目的是将深度数据和彩色图像在时间和空间上进行对齐，以便于将它们组合在一起进行处理。由于深度相机和彩色相机采集数据的方式不同，因此在时间和空间上可能存在一定的偏差。通过对齐后，可以得到时间戳和空间参考系完全一致的深度帧和彩色帧，从而方便地进行联合处理。在实际应用中，将深度帧对齐到彩色帧通常更为常见，因为彩色图像通常具有更高的分辨率和更丰富的信息，可以提供更准确的场景信息，有助于深度信息的校正和增强。

将彩色帧对齐到深度帧的操作相对较少见，通常在需要进行基于深度数据的像素级别处理时使用。由于深度图像和彩色图像的分辨率和采集方式不同，因此在像素级别的处理中可能会出现误差。通过将彩色帧对齐到深度帧，可以根据深度数据对彩色图像进行校正，从而提高像素级别处理的精度。例如，在进行虚拟现实应用时，需要将虚拟物体的图像合成到真实场景中。此时，需要根据深度图像的信息将虚拟物体正确地投影到真实场景中，从而实现逼真的效果。为了实现此目的，需要将彩色图像对齐到深度图像。

第五步，通过创建的管道获取数据流。

  1. frameset = pipe.wait_for_frames();
  2. frameset = align_to_color.process(frameset);

可以说前面四不全部是准备工作。到第五步才相机开始正式工作。

这段代码是使用RealSense相机的C++ API获取深度和彩色图像，并将深度帧对齐到彩色帧。具体来说，代码分为两个步骤：

  1. pipe.wait_for_frames()：等待获取一帧包含深度和彩色图像的帧集。
  2. align_to_color.process(frameset)：将获取到的深度帧对齐到彩色帧，返回一个包含对齐后深度和彩色帧的新帧集。

拿到对齐后的视频流后，需要从里面拿到对齐后的深度流和彩色流，并获取

  1.             rs2::video_frame color_frame = frameset.get_color_frame();
  2.             rs2::depth_frame depth_frame = frameset.get_depth_frame();
  3.             // 获取彩色图像宽
  4.             const int w = color_frame.as<rs2::video_frame>().get_width(); 
  5.             const int h = color_frame.as<rs2::video_frame>().get_height();

为了方便对图像进行处理，需要将彩色帧转成成OpenCV中的cv::Mat格式，这里转换，需要注意一下，因为opencv的存在，opencv中，它默认读取和显示图像时的通道顺序是BGR而不是RGB，因此还需要进行格式转换。

  1.             cv::Mat frame(cv::Size(w, h), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
  2.             cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);

  1. 创建一个OpenCV中的cv::Mat对象，用于存储彩色帧的像素数据。其中，cv::Size(w, h)指定了图像的宽度和高度，CV_8UC3表示图像的数据类型为8位无符号整数，3个通道，color_frame.get_data()返回彩色帧的数据指针，cv::Mat::AUTO_STEP表示自动计算每行像素的字节数，从而得到图像数据的步长（stride）。

  2. 将RealSense相机获取的彩色帧数据存储到cv::Mat对象中。由于RealSense相机获取的彩色帧数据格式为RGB8，而OpenCV中的cv::Mat默认使用BGR8格式，因此需要进行颜色通道的交换，即将R通道和B通道的数据交换位置。这可以通过OpenCV提供的cv::cvtColor()函数实现，具体代码如下：

到这里图像已经准备好了，你可以把它用起来了，做比如yolov5目标检测等，目标检测得到结果后，为了结果用起来，还需要进行像素坐标系到现实坐标系的转换，Yolov5等算法直接输出的位置结果是基于像素坐标系的，不能直接使用，需要进行转换。

第六步，使用rs2_deproject_pixel_to_point函数进行坐标系转换

rs2_deproject_pixel_to_point函数是RealSense C++ API中用于将像素坐标投影为相机坐标系中的3D点的函数。使用该函数需要提供相机内参和深度值，函数原型如下：

rs2_deproject_pixel_to_point(float* out, const rs2_intrinsics *intrin, const float pixel[2], float depth);

其中，out是一个指向包含3D点坐标的float型数组的指针；intrin是一个指向相机内参结构体的指针；pixel是一个包含像素坐标（x，y）的float型数组，depth是深度值。

下面是一个使用rs2_deproject_pixel_to_point函数的示例：

  1. // 创建一个深度帧和一个内参结构体
  2. rs2::depth_frame depth_frame = frames.get_depth_frame();
  3. rs2::video_stream_profile depth_profile = depth_frame.get_profile().as<rs2::video_stream_profile>();
  4. rs2_intrinsics intrin = depth_profile.get_intrinsics();
  5.  
  6. // 获取深度值和像素坐标
  7. float depth_value = depth_frame.get_distance(x, y);
  8. float pixel[2] = { (float)x, (float)y };
  9.  
  10. // 将像素坐标投影到3D坐标
  11. float point[3];
  12. rs2_deproject_pixel_to_point(point, &intrin, pixel, depth_value);

该示例中，我们首先获取深度帧和内参结构体，然后从深度帧中获取深度值和像素坐标。最后，我们调用rs2_deproject_pixel_to_point函数将像素坐标投影到3D坐标，并将结果保存在point数组中。

rs2_deproject_pixel_to_point函数的输出结果是一个包含三个浮点数的数组，表示像素点在3D空间中的坐标。如果需要打印输出这个结果，可以使用以下代码：

  1. float point[3];
  2. rs2_deproject_pixel_to_point(point, &intrinsics, pixel, depth);
  3.  
  4. std::cout << "Point in 3D space: (" << point[0] << ", " << point[1] << ", " << point[2] << ")" << std::endl;

其中，intrinsics是相机的内参矩阵，pixel是待转换的像素坐标，depth是对应像素点的深度值。以上代码将输出格式为(x, y, z)的点坐标。请注意，打印输出结果前需要先确保rs2_deproject_pixel_to_point函数的返回值为true，表示转换成功。

函数的输出结果是一个点在3D空间中的坐标。具体而言，该函数接收一个像素坐标和对应的深度值作为输入，然后使用相机的内参矩阵对这个像素点进行反投影，得到该点在相机坐标系下的坐标。最后，根据相机坐标系和世界坐标系之间的转换关系，将相机坐标系下的坐标转换为世界坐标系下的坐标。

因此，rs2_deproject_pixel_to_point函数的输出结果是一个三维坐标，包含x、y和z三个分量，表示该点在3D空间中的位置。该点的坐标可以用一个浮点数数组来表示。

这里我简单总结一下，rs2_deproject_pixel_to_point函数中，第一个参数是我们需要的，我们需要自己创建一个float 类型的数组去接收坐标系转换的结果，我这里当时很郁闷，不知道怎么拿到转换后的坐标值，直接创建一个数组区接收就好了。

后面三个参数我们都可以通过相机的api拿到，这里说吗如何拿到最后一个参数，深度值。

我们可以通过depth_frame.get_distance()函数获取像素坐标对应的深度值，get_distance函数的输入值是某点的x,y的像素坐标，可以自己区看realsense的官方文档，

这里具体说下get_sditance函数:首先得知道深度值是什么意思?

深度值全程是深度距离(Depth Distance),通常是指物体表面到深度传感器(比如深度相机,激光雷达)之间的距离,我们这里用到的是深度传感器是深度相机,因此深度相机的深度距离指的是相机到物体表面的距离,也被称为相机拍摄的图像中每个像素点与深度相机之间的距离.注意:此深度距离值非真实世界下,物体和相机的距离.在深度相机的深度图像中，每个像素的深度值是以16位无符号整数（uint16_t）的形式存储的，以表示该像素与深度相机之间的距离。get_sditance函数获取某一个像素点（x，y）的深度值，并将其存储在uint16_t型变量et_sditance函数的输出值depth_value中,然而，这个深度值（depth_value）本身的单位只是相机内部使用的一种规范化的无量纲单位。为了获得真实的距离，可以使用get_depth_scale函数，获取了深度传感器的深度比例尺，以将深度值转换为实际距离。depth_scale表示深度值与实际距离的比例关系，是一个浮点数（float）值，具体根据深度传感器的实际规格而定。例如，对于Realsense D435深度相机，其默认的深度比例尺为0.001米（即1毫米）/深度值。另外，代码中还使用get_option函数获取深度传感器的深度偏移值（即单位深度值对应的真实距离），将其存储在float型变量depth_offset中。这个距离值通常以米为单位，因此深度距离distance可根据以下公式计算：

distance = depth_value * depth_scale + depth_offset

其中，depth_value是从深度相机的深度图像中获取的像素深度值，depth_scale是深度比例尺，depth_offset是深度偏移值，将他们分别相乘和相加，即可得到实际的距离（distance）。

下面是使用get_distance函数的简单示例：

  1. #include <librealsense2/rs.hpp>
  2. #include <opencv2/opencv.hpp>
  3.  
  4. // 获取深度图像某个像素的深度值
  5. float get_distance(const rs2::depth_frame& depth_frame, int x, int y)
  6. {
  7.     float depth = depth_frame.get_distance(x, y);
  8.     if (depth == 0) {
  9.         return -1;  // 无效深度值
  10.     }
  11.     return depth;
  12. }
  13.  
  14. int main()
  15. {
  16.     // 配置RealSense相机
  17.     rs2::pipeline pipe;
  18.     rs2::config cfg;
  19.     cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
  20.     pipe.start(cfg);
  21.  
  22.     // 循环读取深度图像并获取某个像素的深度值
  23.     while (true) {
  24.         rs2::frameset frames = pipe.wait_for_frames();
  25.         rs2::depth_frame depth_frame = frames.get_depth_frame();
  26.  
  27.         // 读取深度值
  28.         int x = 320;  // 像素坐标x
  29.         int y = 240;  // 像素坐标y
  30.         float distance = get_distance(depth_frame, x, y);
  31.  
  32.         if (distance > 0) {
  33.             std::cout << "Depth at (" << x << ", " << y << ") is " << distance << " meters." << std::endl;
  34.         }
  35.  
  36.         // 显示深度图像
  37.         cv::Mat depth_image(cv::Size(640, 480), CV_16UC1, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);
  38.         cv::Mat depth_image_show;
  39.         depth_image.convertTo(depth_image_show, CV_8U, 255.0 / 1000);
  40.         cv::imshow("Depth Image", depth_image_show);
  41.  
  42.         char c = cv::waitKey(1);
  43.         if (c == 'q' || c == 27) {
  44.             break;
  45.         }
  46.     }
  47.  
  48.     pipe.stop();
  49.  
  50.     return 0;
  51. }

下面是使用get_depth_scale函数和get_distance函数的示例代码

  1. #include <librealsense2/rs.hpp>
  2. #include <iostream>
  3.  
  4. int main()
  5. {
  6.     rs2::pipeline pipe;
  7.     rs2::config cfg;
  8.     rs2::frameset frames;
  9.     rs2::frame depth_frame;
  10.  
  11.     // 配置相机
  12.     cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
  13.     pipe.start(cfg);
  14.  
  15.     // 获取深度帧
  16.     frames = pipe.wait_for_frames();
  17.     depth_frame = frames.get_depth_frame();
  18.  
  19.     // 获取像素点深度值和距离值
  20.     int x = 320;
  21.     int y = 240;
  22.     uint16_t depth_value = depth_frame.get_distance(x, y);
  23.     float depth_scale = pipe.get_active_profile().get_device().first<rs2::depth_sensor>().get_depth_scale();
  24.     float depth_offset = pipe.get_active_profile().get_device().first<rs2::depth_sensor>().get_option(RS2_OPTION_DEPTH_UNITS);
  25.     float distance = (depth_value * depth_scale) + depth_offset;
  26.  
  27.     // 输出距离值
  28.     std::cout << "Distance to object: " << distance << " meters" << std::endl;
  29.  
  30.     return 0;
  31. }

第七步，实际应用

下面我会用C++给出一段示例代码，也是我自己写的和使用的代码，没有问题。

  1. #include <librealsense2/rs.hpp>
  2. #include <librealsense2/rsutil.h>
  3.  
  4.  
  5. int main()
  6. {
  7.     // 相机参数配置
  8.     rs2::pipeline pipe;
  9.     rs2::config cfg;
  10.  
  11.     cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30);
  12.     cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);
  13.  
  14.     rs2::pipeline_profile profile = pipe.start(cfg); 
  15.     rs2::align align_to_color(RS2_STREAM_COLOR);   // 创建深度流和彩色流对齐的实例化对象
  16.     
  17.     while(true)
  18.     {
  19.         rs2::frameset frameset = pipe.wait_for_frames();
  20.         auto aligned_frameset = align_to_color.process(frameset); // 实际进行流对齐
  21.  
  22.         // 基于对齐的混合流获取深度流和彩色流,进而获取流对齐后的深度内参
  23.         rs2::video_frame color_stream = aligned_frameset.get_color_frame();
  24.         rs2::depth_frame aligned_depth_stream = aligned_frameset.get_depth_frame();
  25.  
  26.         rs2::video_stream_profile depth_stream_profile =                 
  27.         aligned_depth_stream.get_profile().as<rs2::video_stream_profile>();
  28.         const auto depth_intrinsics = depth_stream_profile.get_intrinsics(); //获取对齐后的深度内参
  29.  
  30.         // 获取彩色图像宽
  31.         const int w = color_stream.as<rs2::video_frame>().get_width(); 
  32.         const int h = color_stream.as<rs2::video_frame>().get_height(); 
  33.         
  34.         // 获取图像中心像素点
  35.         float u = 0.5;
  36.         float v = 0.5;
  37.  
  38.         int c = w * u;
  39.         int r = h * v;
  40.  
  41. 		float pixe_center[2];
  42. 		pixe_center[0] = c;
  43. 		pixe_center[1] = r;
  44.  
  45.         // 像素坐标系转换到相机坐标系
  46.         float pixed_center_depth_value =     aligned_depth_stream.get_distance(pixe_center[0],pixe_center[1]);
  47.  
  48.         rs2_deproject_pixel_to_point(point_in_color_coordinates, &depth_intrinsics, pixe_center, pixed_center_depth_value);
  49.  
  50.         std::cout << "像素中心在在彩色相机坐标系下的X坐标" << point_in_color_coordinates[0] << "Y坐标系" << point_in_color_coordinates[1] << "Z坐标" << point_in_color_coordinates[2] << endl;
  51.  
  52.     }
  53.     
  54. }

上述代码有三个注意点，1.流对齐；2.深度内参的获取；3.get_distance函数的使用；4.rs2_deproject_pixel_to_point函数的使用。

1.流对齐

流对齐尽量选择将深度流对齐彩色流，精度更高

2.深度内参

如果要转坐标系，深度内参在流对齐之后获取，才是正确的做法，不然精度不高

3.get_distance函数

获取深度时，也要选择对齐后的深度流

4.rs2_deproject_pixel_to_point函数

该函数最终将坐标系转换到了相机的彩色相机坐标系下

彩色相机坐标系如下，这张图是我从realsense的官方github仓库里摘下来的，真的花了很多时间

 

这篇文章，我来来回回修改好几遍了呀，花了很多时间，看完有帮助的友友们，给我点赞啊，点赞让我快乐，哈哈哈，希望对大家有所帮助。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/IT小白/article/detail/70992
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号