Intel RealSense Depth Camera D435相机调用，深度图调用及测距

迷南。 2023-06-15 09:46 10阅读 0赞

**Ubuntu18.04 + QT 5.12.0 + Opencv4.1.0调用**

Qt的.pro文件

TEMPLATE = app
    
    CONFIG += console c++11
    
    CONFIG -= app_bundle
    
    CONFIG -= qt
    
    INCLUDEPATH += /usr/local/include \
                    /usr/local/include/opencv \
                    /usr/local/include/opencv2 \
                    /usr/local/include/librealsense2                     //文件目录可能不对        sudo cp /usr/include/librealsense2 /usr/local/include/librealsense2  -r
    
    LIBS += /usr/local/lib/libopencv_* \
            /usr/local/lib/librealsense2.so \                            //文件目录可能不对          sudo cp /usr/lib/x86_64-linux-gnu/librealsense2.so  /usr/local/lib/librealsense2.so
    
    SOURCES += \
        main.cpp

读取原图与深度图：

#include <librealsense2/rs.hpp>
    #include <opencv2/opencv.hpp>
    
    #define JIAJIA
    //#define COUT_FPS
    
    using namespace std;
    using namespace cv;
    using namespace rs2;
    
    int main()
    { 
        colorizer color_map;
        pipeline pipe;      //构建一个抽象设备的管道
        config cfg;         //使用非默认配置文件创建配置以配置管道
        cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);    //将所需的流添加到配置
        cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
       pipe.start(cfg);    //指示管道使用请求的配置开始流式传输
    
        while(1)
        { 
            frameset frames;                            // 放下几个第一帧以使自动曝光稳定
            frames = pipe.wait_for_frames();            //等待所有配置的流生成一个帧
            frame color_frame = frames.get_color_frame(); //获取每个帧
    //        frame depth_frame = frames.get_depth_frame();
            frame depth_frame = frames.get_depth_frame().apply_filter(color_map);
    
            const int depth_w=depth_frame.as<rs2::video_frame>().get_width();
            const int depth_h=depth_frame.as<rs2::video_frame>().get_height();
            const int color_w=color_frame.as<rs2::video_frame>().get_width();
            const int color_h=color_frame.as<rs2::video_frame>().get_height();//        Mat color(Size(640, 480), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);        //从彩色图像创建OpenCV矩阵
    
            Mat color(Size(color_w, color_h), CV_8UC3, (void*)color_frame.get_data(), Mat::AUTO_STEP);        //从彩色图像创建OpenCV矩阵
            Mat depth(Size(depth_w,depth_h),CV_8UC3,(void*)depth_frame.get_data(),Mat::AUTO_STEP);
    
            imshow("depth_frame", depth);
            imshow("color_frame", color);
    
    
            int key = waitKey(1);
            if(char(key) == 27)break;
    
        }
        return 0;
    }

实现测距：

#include <iostream>
    #include <sstream>
    #include <iostream>
    #include <fstream>
    #include <algorithm>
    #include <string>
    
    #include<opencv2/imgproc/imgproc.hpp>
    #include<opencv2/core/core.hpp>
    #include<opencv2/highgui/highgui.hpp>
    #include<librealsense2/rs.hpp>
    #include<librealsense2/rsutil.h>
    
    using namespace cv;
    using namespace std;
    using namespace rs2;
    
    
    //获取深度像素对应长度单位（米）的换算比例
    float get_depth_scale(device dev)
    { 
        for (sensor& sensor : dev.query_sensors()) //检查设备的传感器
        { 
            if (depth_sensor dpt = sensor.as<depth_sensor>()) //检查传感器是否为深度传感器
            { 
                return dpt.get_depth_scale();
            }
        }
        throw runtime_error("Device does not have a depth sensor");
    }
    //深度图对齐到彩色图函数
    Mat align_Depth2Color(Mat depth,Mat color,pipeline_profile profile)
    { 
        //声明数据流
        auto depth_stream=profile.get_stream(RS2_STREAM_DEPTH).as<video_stream_profile>();
        auto color_stream=profile.get_stream(RS2_STREAM_COLOR).as<video_stream_profile>();
    
        //获取内参
        const auto intrinDepth=depth_stream.get_intrinsics();
        const auto intrinColor=color_stream.get_intrinsics();
    
        //直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
        rs2_extrinsics  extrinDepth2Color;
        rs2_error *error;
        rs2_get_extrinsics(depth_stream,color_stream,&extrinDepth2Color,&error);
    
        float pd_uv[2],pc_uv[2];   //平面点定义
        float Pdc3[3], Pcc3[3];     //空间点定义
    
        float depth_scale = get_depth_scale(profile.get_device());//获取深度像素与现实单位比例（D435默认1毫米）
        int y=0,x=0;
    
        Mat result=Mat(color.rows,color.cols,CV_16U,Scalar(0));//初始化结果
    
        //对深度图像遍历
        for(int row=0;row<depth.rows;row++)
        { 
            for(int col=0;col<depth.cols;col++)
            { 
                //将当前的(x,y)放入数组pd_uv，表示当前深度图的点
                pd_uv[0]=col;
                pd_uv[1]=row;
                //取当前点对应的深度值
                uint16_t depth_value=depth.at<uint16_t>(row,col);
                float depth_m=depth_value*depth_scale;
    
                rs2_deproject_pixel_to_point(Pdc3,&intrinDepth,pd_uv,depth_m);  //将深度图的像素点根据内参转换到深度摄像头坐标系下的三维点
                rs2_transform_point_to_point(Pcc3,&extrinDepth2Color,Pdc3);     //将深度摄像头坐标系的三维点转化到彩色摄像头坐标系下
                rs2_project_point_to_pixel(pc_uv,&intrinColor,Pcc3);            //将彩色摄像头坐标系下的深度三维点映射到二维平面上
    
                //取得映射后的（u,v)
                x=(int)pc_uv[0];
                y=(int)pc_uv[1];
                //最值限定
                x=x<0? 0:x;
                x=x>depth.cols-1 ? depth.cols-1:x;
                y=y<0? 0:y;
                y=y>depth.rows-1 ? depth.rows-1:y;
    
                result.at<uint16_t>(y,x)=depth_value;
            }
        }
        return result;//返回一个与彩色图对齐了的深度信息图像
    }
    
    void measure_distance(Mat &color,Mat depth,Size range,pipeline_profile profile)
    {  
        float depth_scale = get_depth_scale(profile.get_device()); //获取深度像素与现实单位比例（D435默认1毫米）
        Point center(color.cols/2,color.rows/2);                   //自定义图像中心点
        Rect RectRange(center.x-range.width/2,center.y-range.height/2,
                       range.width,range.height);                  //自定义计算距离的范围
        //遍历该范围
        float distance_sum=0;
        int effective_pixel=0;
        for(int y=RectRange.y;y<RectRange.y+RectRange.height;y++){ 
            for(int x=RectRange.x;x<RectRange.x+RectRange.width;x++){ 
                //如果深度图下该点像素不为0，表示有距离信息
                if(depth.at<uint16_t>(y,x)){ 
                    distance_sum+=depth_scale*depth.at<uint16_t>(y,x);
                    effective_pixel++;
                }
            }
        }
        cout<<"遍历完成，有效像素点:"<<effective_pixel<<endl;
        float effective_distance=(distance_sum/effective_pixel)*1000;
        cout<<"目标距离："<<effective_distance<<" mm"<<endl;
        char distance_str[30];
        sprintf(distance_str,"the distance is:%f mm",effective_distance);
        rectangle(color,RectRange,Scalar(0,0,255),2,8);
        putText(color,(string)distance_str,Point(color.cols*0.02,color.rows*0.05),
                    FONT_HERSHEY_PLAIN,2,Scalar(0,255,0),2,8);
    }
    
    int main()
    { 
        colorizer color_map;   // 帮助着色深度图像
        pipeline pipe;         //创建数据管道
        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);
        pipeline_profile profile = pipe.start(pipe_config); //start()函数返回数据管道的profile
    
        while (1)
        { 
            frameset frameset = pipe.wait_for_frames();  //堵塞程序直到新的一帧捕获
            //取深度图和彩色图
            frame color_frame = frameset.get_color_frame();
            frame depth_frame = frameset.get_depth_frame();
            frame depth_frame_1 = frameset.get_depth_frame().apply_filter(color_map);
            //获取宽高
            const int depth_w=depth_frame.as<video_frame>().get_width();
            const int depth_h=depth_frame.as<video_frame>().get_height();
            const int color_w=color_frame.as<video_frame>().get_width();
            const int color_h=color_frame.as<video_frame>().get_height();
    
            //创建OPENCV类型 并传入数据
            Mat depth_image(Size(depth_w,depth_h),
                            CV_16U,(void*)depth_frame.get_data(),Mat::AUTO_STEP);
            Mat depth_image_1(Size(depth_w,depth_h),
                              CV_8UC3,(void*)depth_frame_1.get_data(),Mat::AUTO_STEP);
            Mat color_image(Size(color_w,color_h),
                            CV_8UC3,(void*)color_frame.get_data(),Mat::AUTO_STEP);
            //实现深度图对齐到彩色图
            Mat result=align_Depth2Color(depth_image,color_image,profile);
            measure_distance(color_image,result,Size(40,40),profile);            //自定义窗口大小
            //显示
            imshow("depth_image",depth_image_1);
            imshow("color_image",color_image);
            //imshow("result",result);
            int key = waitKey(1);
            if(char(key) == 27)break;
        }
        return 0;
    }

基本调用代码来源：  
[https://github.com/jiajia0516/jiajia001/tree/master/deepcamera][https_github.com_jiajia0516_jiajia001_tree_master_deepcamera]  
[https://github.com/jiajia0516/jiajia001/tree/master/measure\_distance][https_github.com_jiajia0516_jiajia001_tree_master_measure_distance]

更多资料请点击：[我的目录][Link 1]

[https_github.com_jiajia0516_jiajia001_tree_master_deepcamera]: https://github.com/jiajia0516/jiajia001/tree/master/deepcamera
[https_github.com_jiajia0516_jiajia001_tree_master_measure_distance]: https://github.com/jiajia0516/jiajia001/tree/master/measure_distance
[Link 1]: http://note.youdao.com/noteshare?id=1370b0e8f4a16b80e7dede7836df3c08