526互联

【RealSense】相机拍摄图像

发布时间 2023-06-21 15:31:44作者: 乞力马扎罗山的雪

saveDepthAndColor.cpp

// https://github.com/IntelRealSense/librealsense/tree/master/wrappers/opencv/imshow
// 简单的显示深度图和RGB图

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/rsutil.h>
#include <opencv2/opencv.hpp> // Include OpenCV API
#include <string>

using namespace cv;
using namespace std;

// 获取深度像素对应长度单位转换
float get_depth_scale(rs2::device dev);

// 检查摄像头数据管道设置是否改变
bool profile_changed(const std::vector<rs2::stream_profile> &current, const std::vector<rs2::stream_profile> &prev);

int main(int argc, char *argv[])
try
{

    // 创建opencv窗口
    // 创建 opencv 窗口
    const char *depth_win = "depth_Image";
    namedWindow(depth_win, WINDOW_AUTOSIZE);
    const char *color_win = "color_Image";
    namedWindow(color_win, WINDOW_AUTOSIZE);

    // 声明深度着色器以实现深度数据的漂亮可视化
    rs2::colorizer color_map;
    rs2::align align_to(RS2_STREAM_COLOR);

    // 声明 RealSense 管道，封装实际设备和传感器
    rs2::pipeline pipe;
    rs2::config pipe_config;
    pipe_config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
    pipe_config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
    // start() 函数返回数据管道的profile
    rs2::pipeline_profile profile = pipe.start(pipe_config);

    // 使用数据管道的 profile 获取深度图像像素对应于长度单位（米）的转换比例
    float depth_scale = get_depth_scale(profile.get_device());
    cout << "depth_scale = " << depth_scale << endl;

    // // 将深度图对齐到RGB图
    rs2::align align(RS2_STREAM_COLOR);  // 对齐的是彩色图，所以彩色图是不变的

    static int i = 0;

    while (getWindowProperty(depth_win, WND_PROP_AUTOSIZE) && getWindowProperty(color_win, WND_PROP_AUTOSIZE))
    {
        // 堵塞程序直到新的一帧捕获
        rs2::frameset frameset = pipe.wait_for_frames();

        // 正在对齐深度图到其他图像流，我们要确保对齐的图像流不发生改变
        if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
        {
            // 如果profile发生改变，则更新align对象，重新获取深度图像像素到长度单位的转换比例
            profile = pipe.get_active_profile();
            align = rs2::align(align_to);
            depth_scale = get_depth_scale(profile.get_device());
        }

        // 获取对齐后的帧
        auto processed = align.process(frameset);

        // 尝试获取对齐后的深度图像帧和其他帧
        rs2::frame aligned_color_frame = processed.get_color_frame(); // RGB图
        rs2::frame aligned_depth_frame = processed.get_depth_frame(); // 深度图

        // 获取宽高
        const int depth_w = aligned_depth_frame.as<rs2::video_frame>().get_width();
        const int depth_h = aligned_depth_frame.as<rs2::video_frame>().get_height();
        const int color_w = aligned_color_frame.as<rs2::video_frame>().get_width();
        const int color_h = aligned_color_frame.as<rs2::video_frame>().get_height();

        // 如果其中一个未能获取，继续迭代
        if (!aligned_depth_frame || !aligned_color_frame)
        {
            continue;
        }

        // 创建opencv类型，并传入数据
        Mat aligned_depth_image(Size(depth_w, depth_h), CV_16UC1, (void *)aligned_depth_frame.get_data(), Mat::AUTO_STEP);
        Mat aligned_color_image(Size(color_w, color_h), CV_8UC3, (void *)aligned_color_frame.get_data(), Mat::AUTO_STEP);

        // 图像显示
        imshow(color_win, aligned_color_image);
        imshow(depth_win, aligned_depth_image);

        std::vector<int> compression_param;
        compression_param.emplace_back(IMWRITE_PNG_COMPRESSION); // png 16位保存
        compression_param.emplace_back(0);                       // 无压缩

        if (waitKey(2) == 32) // 空格
        {
            std::string rgbFile_name = "rgb_" + std::to_string(i + 1) + ".png";
            std::string depthFile_name = "depth_" + std::to_string(i + 1) + ".png";
            imwrite(depthFile_name, aligned_depth_image, compression_param);
            imwrite(rgbFile_name, aligned_color_image);
            i++;
        }
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error &e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception &e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

float get_depth_scale(rs2::device dev)
{
    // 遍历设备的传感器
    for (rs2::sensor &sensor : dev.query_sensors())
    {
        // 检查传感器是否是深度传感器
        if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
            return dpt.get_depth_scale();
    }

    throw std::runtime_error("Device does not have a depth sensor");
}

bool profile_changed(const std::vector<rs2::stream_profile> &current, const std::vector<rs2::stream_profile> &prev)
{
    for (auto &&sp : prev)
    {
        // if previous profile is in current ( maybe just added another)
        auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile &current_sp)
                                { return sp.unique_id() == current_sp.unique_id(); });

        if (itr == std::end(current))
        {
            return true;
        }
    }
    return false;
}

CMakeLists.txt

#  minimum required cmake version: 3.1.0
cmake_minimum_required(VERSION 3.1.0)

project(saveImage)

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

set(DEPENDENCIES realsense2 ${OpenCV_LIBS})

add_executable(saveDepthAndColor saveDepthAndColor.cpp)
set_property(TARGET saveDepthAndColor PROPERTY CXX_STANDARD 11)
target_link_libraries(saveDepthAndColor ${DEPENDENCIES})