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

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

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

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

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

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

//创建数据管道和参数文件
rs2::pipeline pipe;
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
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帧

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

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

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

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

获取深度图内参，

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

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

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

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

rs2::config cfg;
cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
rs2::pipeline pipe;
auto profile = pipe.start(cfg).get_active_profile();

auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto depth_intrinsics = depth_stream.get_intrinsics();

auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
auto color_intrinsics = color_stream.get_intrinsics();

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

    rs2::align align_to_color(RS2_STREAM_COLOR);
    rs2::frameset frameset;

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

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

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

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

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

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

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

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

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

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

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

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

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

            cv::Mat frame(cv::Size(w, h), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
            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函数的示例：

// 创建一个深度帧和一个内参结构体
rs2::depth_frame depth_frame = frames.get_depth_frame();
rs2::video_stream_profile depth_profile = depth_frame.get_profile().as<rs2::video_stream_profile>();
rs2_intrinsics intrin = depth_profile.get_intrinsics();

// 获取深度值和像素坐标
float depth_value = depth_frame.get_distance(x, y);
float pixel[2] = { (float)x, (float)y };

// 将像素坐标投影到3D坐标
float point[3];
rs2_deproject_pixel_to_point(point, &intrin, pixel, depth_value);

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

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

float point[3];
rs2_deproject_pixel_to_point(point, &intrinsics, pixel, depth);

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函数的简单示例：

#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>

// 获取深度图像某个像素的深度值
float get_distance(const rs2::depth_frame& depth_frame, int x, int y)
{
    float depth = depth_frame.get_distance(x, y);
    if (depth == 0) {
        return -1;  // 无效深度值
    }
    return depth;
}

int main()
{
    // 配置RealSense相机
    rs2::pipeline pipe;
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
    pipe.start(cfg);

    // 循环读取深度图像并获取某个像素的深度值
    while (true) {
        rs2::frameset frames = pipe.wait_for_frames();
        rs2::depth_frame depth_frame = frames.get_depth_frame();

        // 读取深度值
        int x = 320;  // 像素坐标x
        int y = 240;  // 像素坐标y
        float distance = get_distance(depth_frame, x, y);

        if (distance > 0) {
            std::cout << "Depth at (" << x << ", " << y << ") is " << distance << " meters." << std::endl;
        }

        // 显示深度图像
        cv::Mat depth_image(cv::Size(640, 480), CV_16UC1, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);
        cv::Mat depth_image_show;
        depth_image.convertTo(depth_image_show, CV_8U, 255.0 / 1000);
        cv::imshow("Depth Image", depth_image_show);

        char c = cv::waitKey(1);
        if (c == 'q' || c == 27) {
            break;
        }
    }

    pipe.stop();

    return 0;
}

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

#include <librealsense2/rs.hpp>
#include <iostream>

int main()
{
    rs2::pipeline pipe;
    rs2::config cfg;
    rs2::frameset frames;
    rs2::frame depth_frame;

    // 配置相机
    cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
    pipe.start(cfg);

    // 获取深度帧
    frames = pipe.wait_for_frames();
    depth_frame = frames.get_depth_frame();

    // 获取像素点深度值和距离值
    int x = 320;
    int y = 240;
    uint16_t depth_value = depth_frame.get_distance(x, y);
    float depth_scale = pipe.get_active_profile().get_device().first<rs2::depth_sensor>().get_depth_scale();
    float depth_offset = pipe.get_active_profile().get_device().first<rs2::depth_sensor>().get_option(RS2_OPTION_DEPTH_UNITS);
    float distance = (depth_value * depth_scale) + depth_offset;

    // 输出距离值
    std::cout << "Distance to object: " << distance << " meters" << std::endl;

    return 0;
}

第七步，实际应用

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

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


int main()
{
    // 相机参数配置
    rs2::pipeline pipe;
    rs2::config cfg;

    cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 30);
    cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_BGR8, 30);

    rs2::pipeline_profile profile = pipe.start(cfg); 
    rs2::align align_to_color(RS2_STREAM_COLOR);   // 创建深度流和彩色流对齐的实例化对象
    
    while(true)
    {
        rs2::frameset frameset = pipe.wait_for_frames();
        auto aligned_frameset = align_to_color.process(frameset); // 实际进行流对齐

        // 基于对齐的混合流获取深度流和彩色流,进而获取流对齐后的深度内参
        rs2::video_frame color_stream = aligned_frameset.get_color_frame();
        rs2::depth_frame aligned_depth_stream = aligned_frameset.get_depth_frame();

        rs2::video_stream_profile depth_stream_profile =                 
        aligned_depth_stream.get_profile().as<rs2::video_stream_profile>();
        const auto depth_intrinsics = depth_stream_profile.get_intrinsics(); //获取对齐后的深度内参

        // 获取彩色图像宽
        const int w = color_stream.as<rs2::video_frame>().get_width(); 
        const int h = color_stream.as<rs2::video_frame>().get_height(); 
        
        // 获取图像中心像素点
        float u = 0.5;
        float v = 0.5;

        int c = w * u;
        int r = h * v;

		float pixe_center[2];
		pixe_center[0] = c;
		pixe_center[1] = r;

        // 像素坐标系转换到相机坐标系
        float pixed_center_depth_value =     aligned_depth_stream.get_distance(pixe_center[0],pixe_center[1]);

        rs2_deproject_pixel_to_point(point_in_color_coordinates, &depth_intrinsics, pixe_center, pixed_center_depth_value);

        std::cout << "像素中心在在彩色相机坐标系下的X坐标" << point_in_color_coordinates[0] << "Y坐标系" << point_in_color_coordinates[1] << "Z坐标" << point_in_color_coordinates[2] << endl;

    }
    
}

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

1.流对齐

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

2.深度内参

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

3.get_distance函数

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

4.rs2_deproject_pixel_to_point函数

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

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

 

这篇文章，我来来回回修改好几遍了呀，花了很多时间，看完有帮助的友友们，给我点赞啊，点赞让我快乐，哈哈哈，希望对大家有所帮助。文章来源地址https://www.toymoban.com/news/detail-588778.html

到了这里，关于C++ api调用realsense d435相机，将坐标转换到相机坐标系的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！