使用realsense进行目标检测并标识目标深度

发布于:2025-08-01 ⋅ 阅读:(13) ⋅ 点赞:(0)

涉及知识点都在代码中注释了,直接看代码

// This example is derived from the ssd_mobilenet_object_detection opencv demo
// and adapted to be used with Intel RealSense Cameras
// Please see https://github.com/opencv/opencv/blob/master/LICENSE

#include <opencv2/dnn.hpp>
#include <librealsense2/rs.hpp>
#include "../cv-helpers.hpp"

const size_t inWidth      = 300;
const size_t inHeight     = 300;
const float WHRatio       = inWidth / (float)inHeight;
const float inScaleFactor = 0.007843f;
const float meanVal       = 127.5;
const char* classNames[]  = {"background",
                             "aeroplane", "bicycle", "bird", "boat",
                             "bottle", "bus", "car", "cat", "chair",
                             "cow", "diningtable", "dog", "horse",
                             "motorbike", "person", "pottedplant",
                             "sheep", "sofa", "train", "tvmonitor"};

int main(int argc, char** argv) try
{
    using namespace cv;
    using namespace cv::dnn;
    using namespace rs2;

    Net net = readNetFromCaffe("MobileNetSSD_deploy.prototxt", 
                               "MobileNetSSD_deploy.caffemodel");

    // Start streaming from Intel RealSense Camera
    // 流水线(Pipeline)简化了用户与设备及计算机视觉处理模块的交互。
    // 该抽象类封装了相机配置、流传输、视觉模块触发和线程管理,使应用程序能够专注于模块的计算机视觉输出或设备的原始数据输出。
    // 流水线可管理以处理块(Processing Blocks)形式实现的计算机视觉模块:
    // 流水线作为处理块接口的消费者(调用方)
    // 应用程序则作为计算机视觉接口的消费者
    pipeline pipe;
    // 以默认配置启动流水线流传输
    // 流水线流传输循环会从设备捕获样本数据,并根据各模块的需求和线程模型,将其传递至已连接的计算机视觉模块和处理块。
    // 流传输期间,应用程序可通过以下方式访问相机数据流:
    // 调用 wait_for_frames()(阻塞式等待帧)
    // 调用 poll_for_frames()(非阻塞轮询帧)
    // 运行条件:
    // 循环持续执行,直至流水线被主动停止。
    // 仅当流水线处于未启动状态时方可启动,否则将抛出异常。
    // 返回值
    // 成功配置后,返回流水线设备及数据流的实际配置信息(profile)。
    // pipeline_profile rs2::pipeline::start()
    auto config = pipe.start();
    // 流水线配置(pipeline profile)包含设备及其启用的数据流组合(含具体参数配置)。该配置是流水线根据定义的过滤器和条件,从上述选项中筛选出的结果。同一设备的不同传感器可能共享多个数据流。
    // ◆ get_stream()
    // stream_profile rs2::pipeline_profile::get_stream (rs2_stream	stream_type, int stream_index = -1)	const i

    // Return the stream profile that is enabled for the specified stream in this profile.

    // Parameters
    // [in]	stream_type	Stream type of the desired profile
    // [in]	stream_index	Stream index of the desired profile. -1 for any matching.

    // Returns
    // The first matching stream profile 
    // template<class T >
    // rs2::stream_profile::as() const
    // Template function, casting the instance as another class type
    // Returns
    // class instance - pointer or null. 
    auto profile = config.get_stream(RS2_STREAM_COLOR)
                         .as<video_stream_profile>();
    // 创建对齐过滤器
    // 对齐操作将在深度图像与另一图像之间进行。具体配置方式如下:
    // 若需将深度图对齐至其他图像流
    // 将 align_to 参数设置为目标流的类型(如 RS2_STREAM_COLOR)。
    // 若需将非深度图对齐至深度图
    // 将 align_to 参数设为 RS2_STREAM_DEPTH。
    // 运行时机制
    // 相机校准参数与帧的流类型,将根据首次传入 process() 的有效帧集合动态确定。
    rs2::align align_to(RS2_STREAM_COLOR);

    Size cropSize;
    if (profile.width() / (float)profile.height() > WHRatio)
    {
        cropSize = Size(static_cast<int>(profile.height() * WHRatio),
                        profile.height());
    }
    else
    {
        cropSize = Size(profile.width(),
                        static_cast<int>(profile.width() / WHRatio));
    }

    Rect crop(Point((profile.width() - cropSize.width) / 2,
                    (profile.height() - cropSize.height) / 2),
              cropSize);

    const auto window_name = "Display Image";
    namedWindow(window_name, WINDOW_AUTOSIZE);

    while (getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
    {
        // Wait for the next set of frames
        auto data = pipe.wait_for_frames();
        // Make sure the frames are spatially aligned
        // 对输入帧执行对齐处理,生成已对齐的帧集合
        data = align_to.process(data);

        auto color_frame = data.get_color_frame();
        auto depth_frame = data.get_depth_frame();

        // If we only received new depth frame, 
        // but the color did not update, continue
        static int last_frame_number = 0;
        if (color_frame.get_frame_number() == last_frame_number) continue;
        last_frame_number = static_cast<int>(color_frame.get_frame_number());

        // Convert RealSense frame to OpenCV matrix:
        auto color_mat = frame_to_mat(color_frame);
        auto depth_mat = depth_frame_to_meters(depth_frame);
        /**
         * @brief 从图像创建4维blob(数据块)。可选进行中心裁剪/缩放、均值减法、归一化缩放和B/R通道交换。
         * @param image 输入图像(支持1/3/4通道格式)
         * @param size 输出图像的空间尺寸
         * @param mean 各通道要减去的均值(标量)。当图像为BGR格式且swapRB=true时,应按(mean-R, mean-G, mean-B)顺序提供
         * @param scalefactor 图像像素值的缩放系数
         * @param swapRB 是否交换3通道图像的首末通道(BGR<->RGB)
         * @param crop 缩放后是否中心裁剪
         * @param ddepth 输出blob的深度。可选CV_32F或CV_8U
         * @details 若crop=true,输入图像将按比例缩放至至少一个维度匹配目标尺寸,然后中心裁剪。
         *          若crop=false,则直接缩放图像(保持长宽比)。
         * @returns 返回NCHW维度顺序的4维Mat
         * 将输入图像转换为神经网络所需的4D blob格式(NCHW顺序),并支持以下预处理:
         * 尺寸变换:调整图像大小
         * 色彩处理:通道交换(BGR↔RGB)
         * 归一化:均值减法 + 像素值缩放
         * 裁剪策略:中心裁剪或保持比例缩放
        */
        Mat inputBlob = blobFromImage(color_mat, inScaleFactor,
                                      Size(inWidth, inHeight), meanVal, false); //Convert Mat to batch of images
        net.setInput(inputBlob, "data"); //set the network input
        /**
         * @brief 执行前向传播计算指定名称层的输出
         * @param outputName 需要获取输出的层名称(默认为空,表示整个网络,输出最后一层)
         * @return 返回指定层的第一个输出blob(多维数据块)
         * @details 默认情况下会对整个网络执行前向传播计算
         * 核心功能
         * 神经网络前向传播:执行已加载神经网络的计算过程
         * 灵活输出控制:可获取中间层或输出层的结果
         * 高效计算:自动处理层间依赖关系和数据传递
        */
        Mat detection = net.forward("detection_out"); //compute output
        
        /**
         * 在 OpenCV 的 Mat 类中,size[] 是一个数组,用于表示 多维矩阵的维度大小。对于神经网络输出的 4D blob(通常来自 SSD、Faster R-CNN 等目标检测模型),size[2] 有特定含义:
         * 1. 4D blob 的维度结构
         * 典型的目标检测网络(如 SSD)输出 blob 的维度为:
         * [1, 1, N, 7]
         * 对应的 size[] 索引和含义:
         * 索引 (size[n])	维度名称	含义
         * size[0]	第0维	批量大小(batch size),通常为1(单张图像推理)
         * size[1]	第1维	固定为1(历史遗留设计,无实际意义)
         * size[2]	第2维	检测到的物体数量(N)
         * size[3]	第3维	每个物体的参数数量(固定为7)
         * 2. 在代码中的具体应用
            // 网络输出是一个4D blob,形状为 [1, 1, N, 7]
            Mat detection = net.forward("detection_out"); 
            // 转换为2D矩阵:[N行 x 7列]
            Mat detectionMat(
                detection.size[2],  // 行数 = 检测到的物体数量(N)
                detection.size[3],  // 列数 = 每个物体的参数数量(7)
                CV_32F,            // 数据类型为32位浮点
                detection.ptr<float>()  // 直接引用数据指针
            );
         * 3. 为什么是 size[2] 和 size[3]?
            OpenCV 的 Mat 使用 行优先存储(Row-Major),维度索引从外到内:
            size[0]:最外层维度(批量)
            size[3]:最内层维度(单个参数值)
            对于目标检测任务,我们只关心实际的检测结果(物体数量和参数),所以跳过无意义的 size[0] 和 size[1]。
         * 4. 参数的具体内容(每行的7个值)
            转换后的 detectionMat 是一个 N×7 的矩阵,每行代表一个检测到的物体,包含7个值:
            列索引	含义	数据类型	说明
            0	   图像ID	float	  总是0(单图像推理时无用)
            1	   类别ID	float	对应 classNames 数组的索引
            2	   置信度	float	0~1之间的检测置信度
            3	   左上角x	float	归一化坐标(0~1)
            4	   左上角y	float	归一化坐标(0~1)
            5	   右下角x	float	归一化坐标(0~1)
            6	   右下角y	float	归一化坐标(0~1)
         * 5. 实际应用示例
            for (int i = 0; i < detectionMat.rows; i++) {  // 遍历所有检测到的物体
                float confidence = detectionMat.at<float>(i, 2);  // 获取置信度
                if (confidence > 0.5) {  // 过滤低置信度检测
                    int classId = static_cast<int>(detectionMat.at<float>(i, 1));  // 类别ID
                    // 将归一化坐标转换为像素坐标
                    int x1 = detectionMat.at<float>(i, 3) * image.cols;
                    int y1 = detectionMat.at<float>(i, 4) * image.rows;
                    int x2 = detectionMat.at<float>(i, 5) * image.cols;
                    int y2 = detectionMat.at<float>(i, 6) * image.rows;
                    // 绘制检测框...
                }
            }

         * 6. 其他模型的差异
              YOLOv3/v5:输出维度可能不同(如 [1, 25200, 85]),需调整解析逻辑。
              分类模型:输出通常是 [1, N](N为类别数),无需处理 size[2]。
        **/
        Mat detectionMat(detection.size[2], detection.size[3], CV_32F, detection.ptr<float>());

        // Crop both color and depth frames
        /**  eg:将1080p图像中心裁剪为224x224(分类模型输入)
            原始图像尺寸:1920x1080
            cv::Size profile(1920, 1080); 
            目标裁剪尺寸:224x224
            cv::Size cropSize(224, 224);  
            计算居中裁剪区域
            cv::Rect crop(
                cv::Point((1920 - 224)/2, (1080 - 224)/2),  // Point(848, 428)
                cropSize
            );
            执行裁剪
            cv::Mat originalImage = cv::imread("input.jpg");
            cv::Mat croppedImage = originalImage(crop);  // 得到224x224图像
        **/
        color_mat = color_mat(crop);
        depth_mat = depth_mat(crop);

        float confidenceThreshold = 0.8f;
        for(int i = 0; i < detectionMat.rows; i++)
        {
            float confidence = detectionMat.at<float>(i, 2);

            if(confidence > confidenceThreshold)
            {
                size_t objectClass = (size_t)(detectionMat.at<float>(i, 1));

                int xLeftBottom = static_cast<int>(detectionMat.at<float>(i, 3) * color_mat.cols);
                int yLeftBottom = static_cast<int>(detectionMat.at<float>(i, 4) * color_mat.rows);
                int xRightTop = static_cast<int>(detectionMat.at<float>(i, 5) * color_mat.cols);
                int yRightTop = static_cast<int>(detectionMat.at<float>(i, 6) * color_mat.rows);

                Rect object((int)xLeftBottom, (int)yLeftBottom,
                            (int)(xRightTop - xLeftBottom),
                            (int)(yRightTop - yLeftBottom));

                object = object  & Rect(0, 0, depth_mat.cols, depth_mat.rows);

                // Calculate mean depth inside the detection region
                // This is a very naive way to estimate objects depth
                // but it is intended to demonstrate how one might 
                // use depth data in general
                // 计算输入数组(通常是图像)的 各通道均值,支持通过掩码限定计算区域。
                // 返回 cv::Scalar(4元素向量),每个元素对应一个通道的均值:
                // 单通道图像:Scalar(mean_val, 0, 0, 0)
                // 三通道BGR图像:Scalar(mean_B, mean_G, mean_R, 0)
                Scalar m = mean(depth_mat(object));

                std::ostringstream ss;
                ss << classNames[objectClass] << " ";
                ss << std::setprecision(2) << m[0] << " meters away";
                String conf(ss.str());

                rectangle(color_mat, object, Scalar(0, 255, 0));
                int baseLine = 0;
                Size labelSize = getTextSize(ss.str(), FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);

                auto center = (object.br() + object.tl())*0.5;
                center.x = center.x - labelSize.width / 2;

                rectangle(color_mat, Rect(Point(center.x, center.y - labelSize.height),
                    Size(labelSize.width, labelSize.height + baseLine)),
                    Scalar(255, 255, 255), FILLED);
                putText(color_mat, ss.str(), center,
                        FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,0,0));
            }
        }

        imshow(window_name, color_mat);
        if (waitKey(1) >= 0) break;
    }

    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;
}

预期效果:
在这里插入图片描述


网站公告

今日签到

点亮在社区的每一天
去签到