add RealSenseCapture

cmvr-es/common/utils/ffmpeg/include/CameraCapture.h

@@ -4,26 +4,29 @@
 #include <string>
 #include <memory>
 #include <functional>
+#include <opencv2/opencv.hpp>
 
 extern "C"{
 #include <libavformat/avformat.h>
 #include <libavcodec/avcodec.h>
 #include <libavdevice/avdevice.h>
+#include <libswscale/swscale.h>
 }
 
 namespace ffmpeg {
 
     struct CameraConfig {
-        std::string device_name = "0";  // Windows: "0" (dshow), Linux: "0" (对应 /dev/video0)
+        std::string device_name = "0";  // Windows: "0" (dshow), Linux: "/dev/video0"
         int width = 1280;
         int height = 720;
         int fps = 30;
-        AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P;
+        AVPixelFormat pixel_format = AV_PIX_FMT_YUV420P;  // 保持原有的像素格式
     };
 
     class CameraCapture {
     public:
-        using FrameCallback = std::function<void(AVFrame* frame)>;
+        // 回调函数：同时返回AVFrame和cv::Mat
+        using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
 
         CameraCapture();
         ~CameraCapture();
@@ -54,6 +57,9 @@ namespace ffmpeg {
         bool is_capturing_ = false;
         AVPacket* packet_ = nullptr;
         AVFrame* frame_ = nullptr;
+        AVFrame* rgb_frame_ = nullptr;  // 用于转换的RGB帧
+        SwsContext* sws_ctx_ = nullptr; // 格式转换上下文
+        int64_t frame_count_ = 0;
 
         // 初始化设备（平台相关）
         int init_device();
@@ -61,6 +67,12 @@ namespace ffmpeg {
         // 初始化解码器
         int init_decoder();
 
+        // 初始化SWS上下文
+        int init_sws_context();
+
+        // 将AVFrame转换为cv::Mat
+        cv::Mat avframe_to_cvmat(AVFrame* frame);
+
         // 释放资源
         void cleanup();
     };

cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h

@@ -0,0 +1,94 @@
+// RealSenseCapture.h
+#pragma once
+
+#include <string>
+#include <memory>
+#include <functional>
+#include <mutex>
+#include <thread>
+#include <atomic>
+#include <opencv2/opencv.hpp>
+
+#include <librealsense2/rs.hpp>
+
+extern "C" {
+#include <libavformat/avformat.h>
+#include <libavcodec/avcodec.h>
+#include <libswscale/swscale.h>
+}
+
+namespace ffmpeg {
+
+struct RealSenseConfig {
+    std::string serial_number = "";  // 设备序列号，为空则使用第一个设备
+    int width = 1280;
+    int height = 720;
+    int fps = 30;
+    AVPixelFormat pixel_format = AV_PIX_FMT_BGR24;  // RealSense输出BGR格式
+};
+
+class RealSenseCapture {
+public:
+    // 回调函数：同时返回AVFrame和cv::Mat
+    using FrameCallback = std::function<void(AVFrame* frame, cv::Mat rgb_image, int64_t timestamp)>;
+
+    RealSenseCapture();
+    ~RealSenseCapture();
+
+    // 初始化摄像头
+    int initialize(const RealSenseConfig& config);
+
+    // 开始捕获
+    int start_capture(FrameCallback callback);
+
+    // 停止捕获
+    void stop_capture();
+
+    // 获取当前配置
+    RealSenseConfig get_config() const { return config_; }
+
+    // 获取设备信息
+    std::string get_device_info() const;
+
+    // 获取内参
+    rs2_intrinsics get_intrinsics() const { return intrinsics_; }
+
+private:
+    RealSenseConfig config_;
+
+    // RealSense相关
+    rs2::context ctx_;
+    rs2::pipeline pipe_;
+    rs2::config rs_cfg_;
+    rs2::pipeline_profile profile_;
+    rs2_intrinsics intrinsics_;
+
+    // FFmpeg相关
+    AVFrame* frame_ = nullptr;        // 用于编码的AVFrame
+    AVFrame* rgb_frame_ = nullptr;    // 用于OpenCV的AVFrame
+
+    // 状态控制
+    std::atomic<bool> is_capturing_{false};
+    std::unique_ptr<std::thread> capture_thread_;
+    mutable std::mutex mutex_;
+
+    // 帧计数
+    int64_t frame_count_ = 0;
+
+    // 初始化设备
+    int init_device();
+
+    // 创建AVFrame
+    AVFrame* create_avframe(int width, int height, AVPixelFormat pix_fmt);
+
+    // 捕获线程函数
+    void capture_thread_func(FrameCallback callback);
+
+    // 将RealSense帧转换为AVFrame
+    AVFrame* realsense_to_avframe(rs2::frame frame);
+
+    // 将RealSense帧转换为cv::Mat
+    cv::Mat realsense_to_cvmat(rs2::frame frame);
+};
+
+} // namespace ffmpeg

cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp

@@ -9,6 +9,7 @@ namespace ffmpeg {
 CameraCapture::CameraCapture() {
     packet_ = av_packet_alloc();
     frame_ = av_frame_alloc();
+    rgb_frame_ = av_frame_alloc();
 }
 
 CameraCapture::~CameraCapture() {
@@ -17,6 +18,8 @@ CameraCapture::~CameraCapture() {
 
     if (packet_) av_packet_free(&packet_);
     if (frame_) av_frame_free(&frame_);
+    if (rgb_frame_) av_frame_free(&rgb_frame_);
+    if (sws_ctx_) sws_freeContext(sws_ctx_);
 }
 
 int CameraCapture::initialize(const CameraConfig& config) {
@@ -36,6 +39,25 @@ int CameraCapture::initialize(const CameraConfig& config) {
         return ret;
     }
 
+    // 初始化SWS上下文
+    ret = init_sws_context();
+    if (ret < 0) {
+        std::cerr << "初始化SWS上下文失败" << std::endl;
+        return ret;
+    }
+
+    // 分配RGB帧缓冲区（用于转换）
+    rgb_frame_->width = config_.width;
+    rgb_frame_->height = config_.height;
+    rgb_frame_->format = AV_PIX_FMT_BGR24;  // OpenCV使用BGR格式
+    ret = av_frame_get_buffer(rgb_frame_, 0);
+    if (ret < 0) {
+        std::cerr << "分配RGB帧缓冲区失败" << std::endl;
+        return ret;
+    }
+
+    std::cout << "摄像头初始化成功: " << config_.width << "x" << config_.height
+              << "@" << config_.fps << "fps" << std::endl;
     return 0;
 }
 
@@ -56,8 +78,15 @@ int CameraCapture::init_device() {
         return -1;
     }
 
+    // 设置摄像头参数
+    AVDictionary* options = nullptr;
+    av_dict_set(&options, "framerate", std::to_string(config_.fps).c_str(), 0);
+    av_dict_set(&options, "video_size", (std::to_string(config_.width) + "x" + std::to_string(config_.height)).c_str(), 0);
+
     // 打开摄像头
-    int ret = avformat_open_input(&fmt_ctx_, device_path.c_str(), input_fmt, nullptr);
+    int ret = avformat_open_input(&fmt_ctx_, device_path.c_str(), input_fmt, &options);
+    av_dict_free(&options);
+
     if (ret < 0) {
         char err_buf[1024];
         av_strerror(ret, err_buf, sizeof(err_buf));
@@ -114,6 +143,46 @@ int CameraCapture::init_decoder() {
     return 0;
 }
 
+int CameraCapture::init_sws_context() {
+    // 获取解码器输出的像素格式
+    AVPixelFormat src_format = decoder_ctx_->pix_fmt;
+
+    sws_ctx_ = sws_getContext(
+        decoder_ctx_->width, decoder_ctx_->height, src_format,
+        config_.width, config_.height, AV_PIX_FMT_BGR24,  // 转换为BGR24给OpenCV
+        SWS_BILINEAR, nullptr, nullptr, nullptr
+    );
+
+    if (!sws_ctx_) {
+        std::cerr << "创建SWS上下文失败" << std::endl;
+        return -1;
+    }
+
+    return 0;
+}
+
+cv::Mat CameraCapture::avframe_to_cvmat(AVFrame* frame) {
+    // 确保RGB帧可写
+    av_frame_make_writable(rgb_frame_);
+
+    // 转换到BGR24
+    sws_scale(sws_ctx_,
+              frame->data, frame->linesize, 0, frame->height,
+              rgb_frame_->data, rgb_frame_->linesize);
+
+    // 创建cv::Mat（BGR格式）
+    cv::Mat image(config_.height, config_.width, CV_8UC3);
+
+    // 复制数据（由于linesize可能不等于width*3，需要逐行复制）
+    for (int i = 0; i < config_.height; i++) {
+        memcpy(image.data + i * image.step,
+               rgb_frame_->data[0] + i * rgb_frame_->linesize[0],
+               config_.width * 3);
+    }
+
+    return image;
+}
+
 int CameraCapture::start_capture(FrameCallback callback) {
     if (!callback || !fmt_ctx_ || !decoder_ctx_) {
         std::cerr << "参数无效或未初始化" << std::endl;
@@ -121,6 +190,9 @@ int CameraCapture::start_capture(FrameCallback callback) {
     }
 
     is_capturing_ = true;
+    frame_count_ = 0;
+
+    std::cout << "开始采集..." << std::endl;
 
     while (is_capturing_) {
         // 读取数据包
@@ -154,15 +226,18 @@ int CameraCapture::start_capture(FrameCallback callback) {
                     break;
                 }
 
-                // 回调处理帧
+                // 设置PTS
-                callback(frame_);
+                frame_->pts = frame_count_;
+
+                // 转换为cv::Mat（用于显示或OpenCV处理）
+                cv::Mat rgb_image = avframe_to_cvmat(frame_);
+
+                // 回调：同时返回AVFrame和cv::Mat
+                callback(frame_, rgb_image, frame_count_++);
             }
         }
 
         av_packet_unref(packet_);
-
-        // 控制帧率
-        std::this_thread::sleep_for(std::chrono::milliseconds(1000 / config_.fps));
     }
 
     return 0;

cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp

@@ -0,0 +1,301 @@
+// RealSenseCapture.cpp
+#include "../include/RealSenseCapture.h"
+#include <iostream>
+#include <chrono>
+
+namespace ffmpeg {
+
+RealSenseCapture::RealSenseCapture() {
+    frame_ = av_frame_alloc();
+    rgb_frame_ = av_frame_alloc();
+}
+
+RealSenseCapture::~RealSenseCapture() {
+    stop_capture();
+
+    if (frame_) av_frame_free(&frame_);
+    if (rgb_frame_) av_frame_free(&rgb_frame_);
+}
+
+int RealSenseCapture::initialize(const RealSenseConfig& config) {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    config_ = config;
+
+    try {
+        // 初始化设备
+        int ret = init_device();
+        if (ret < 0) {
+            std::cerr << "初始化RealSense设备失败" << std::endl;
+            return ret;
+        }
+
+        // 创建AVFrame
+        if (frame_) av_frame_free(&frame_);
+        if (rgb_frame_) av_frame_free(&rgb_frame_);
+
+        frame_ = create_avframe(config_.width, config_.height, config_.pixel_format);
+        rgb_frame_ = create_avframe(config_.width, config_.height, AV_PIX_FMT_BGR24);
+
+        if (!frame_ || !rgb_frame_) {
+            std::cerr << "创建AVFrame失败" << std::endl;
+            return -1;
+        }
+
+        std::cout << "RealSense摄像头初始化成功: "
+                  << config_.width << "x" << config_.height << "@" << config_.fps << "fps" << std::endl;
+
+        return 0;
+
+    } catch (const rs2::error& e) {
+        std::cerr << "RealSense错误: " << e.what() << std::endl;
+        return -1;
+    } catch (const std::exception& e) {
+        std::cerr << "常规错误: " << e.what() << std::endl;
+        return -1;
+    }
+}
+
+int RealSenseCapture::init_device() {
+    try {
+        // 查询设备
+        rs2::device_list devices = ctx_.query_devices();
+        size_t device_count = devices.size();
+
+        if (device_count == 0) {
+            std::cerr << "未检测到RealSense设备" << std::endl;
+            return -1;
+        }
+
+        std::cout << "检测到 " << device_count << " 个RealSense设备" << std::endl;
+
+        // 如果指定了序列号，查找对应设备
+        if (!config_.serial_number.empty()) {
+            bool found = false;
+            for (size_t i = 0; i < device_count; i++) {
+                rs2::device dev = devices[i];
+                std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+
+                if (serial == config_.serial_number) {
+                    std::cout << "找到指定序列号的设备: " << serial << std::endl;
+                    found = true;
+                    rs_cfg_.enable_device(serial);
+                    break;
+                }
+            }
+
+            if (!found) {
+                std::cerr << "未找到序列号为 " << config_.serial_number << " 的设备" << std::endl;
+                return -1;
+            }
+        } else {
+            // 使用第一个设备
+            rs2::device dev = devices[0];
+            std::string serial = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+            std::cout << "使用第一个设备: " << dev.get_info(RS2_CAMERA_INFO_NAME)
+                      << " (序列号: " << serial << ")" << std::endl;
+            rs_cfg_.enable_device(serial);
+        }
+
+        // 配置彩色视频流
+        rs_cfg_.enable_stream(RS2_STREAM_COLOR,
+                              config_.width,
+                              config_.height,
+                              RS2_FORMAT_BGR8,  // OpenCV使用BGR格式
+                              config_.fps);
+
+        return 0;
+
+    } catch (const rs2::error& e) {
+        std::cerr << "初始化设备失败: " << e.what() << std::endl;
+        return -1;
+    }
+}
+
+AVFrame* RealSenseCapture::create_avframe(int width, int height, AVPixelFormat pix_fmt) {
+    AVFrame* frame = av_frame_alloc();
+    if (!frame) {
+        return nullptr;
+    }
+
+    frame->width = width;
+    frame->height = height;
+    frame->format = pix_fmt;
+
+    // 分配缓冲区
+    int ret = av_frame_get_buffer(frame, 0);
+    if (ret < 0) {
+        av_frame_free(&frame);
+        return nullptr;
+    }
+
+    return frame;
+}
+
+AVFrame* RealSenseCapture::realsense_to_avframe(rs2::frame frame) {
+    auto vf = frame.as<rs2::video_frame>();
+    const int width = vf.get_width();
+    const int height = vf.get_height();
+    const uint8_t* data = reinterpret_cast<const uint8_t*>(vf.get_data());
+
+    // 确保帧可写
+    av_frame_make_writable(frame_);
+
+    // RealSense的BGR8格式，linesize = width * 3
+    int src_linesize = width * 3;
+
+    // 复制数据到AVFrame
+    for (int i = 0; i < height; i++) {
+        memcpy(frame_->data[0] + i * frame_->linesize[0],
+               data + i * src_linesize,
+               src_linesize);
+    }
+
+    return frame_;
+}
+
+cv::Mat RealSenseCapture::realsense_to_cvmat(rs2::frame frame) {
+    auto vf = frame.as<rs2::video_frame>();
+    const int width = vf.get_width();
+    const int height = vf.get_height();
+
+    // RealSense的BGR8格式直接对应OpenCV的CV_8UC3
+    cv::Mat image(cv::Size(width, height), CV_8UC3, (void*)vf.get_data(), cv::Mat::AUTO_STEP);
+
+    // 返回深拷贝，确保图像数据独立
+    return image.clone();
+}
+
+int RealSenseCapture::start_capture(FrameCallback callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (!callback) {
+        std::cerr << "回调函数为空" << std::endl;
+        return -1;
+    }
+
+    if (is_capturing_) {
+        std::cerr << "已经在捕获中" << std::endl;
+        return -1;
+    }
+
+    try {
+        // 启动pipeline
+        profile_ = pipe_.start(rs_cfg_);
+
+        // 获取内参
+        auto stream = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
+        intrinsics_ = stream.get_intrinsics();
+
+        // 等待第一帧，确保设备正常工作
+        rs2::frameset frames = pipe_.wait_for_frames(2000); // 2秒超时
+        if (!frames.get_color_frame()) {
+            std::cerr << "无法获取第一帧" << std::endl;
+            pipe_.stop();
+            return -1;
+        }
+
+        is_capturing_ = true;
+        frame_count_ = 0;
+
+        // 启动捕获线程
+        capture_thread_ = std::make_unique<std::thread>(&RealSenseCapture::capture_thread_func,
+                                                         this, callback);
+
+        std::cout << "RealSense开始采集" << std::endl;
+        return 0;
+
+    } catch (const rs2::error& e) {
+        std::cerr << "启动采集失败: " << e.what() << std::endl;
+        return -1;
+    }
+}
+
+void RealSenseCapture::capture_thread_func(FrameCallback callback) {
+    const int frame_interval = 1000 / config_.fps;  // 毫秒
+
+    while (is_capturing_) {
+        auto frame_start = std::chrono::steady_clock::now();
+
+        try {
+            // 等待帧数据
+            rs2::frameset frames = pipe_.wait_for_frames(1000); // 1秒超时
+            rs2::frame color_frame = frames.get_color_frame();
+
+            if (!color_frame) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+                continue;
+            }
+
+            // 转换为AVFrame（用于编码）
+            AVFrame* av_frame = realsense_to_avframe(color_frame);
+            av_frame->pts = frame_count_;
+
+            // 转换为cv::Mat（用于显示或OpenCV处理）
+            cv::Mat rgb_image = realsense_to_cvmat(color_frame);
+
+            // 回调：同时返回AVFrame和cv::Mat
+            callback(av_frame, rgb_image, frame_count_++);
+
+        } catch (const rs2::error& e) {
+            std::cerr << "采集错误: " << e.what() << std::endl;
+            if (!is_capturing_) break;
+        }
+
+        // 控制帧率
+        auto frame_end = std::chrono::steady_clock::now();
+        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+            frame_end - frame_start).count();
+
+        if (elapsed < frame_interval) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(frame_interval - elapsed));
+        }
+    }
+}
+
+void RealSenseCapture::stop_capture() {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    if (!is_capturing_) {
+        return;
+    }
+
+    is_capturing_ = false;
+
+    if (capture_thread_ && capture_thread_->joinable()) {
+        capture_thread_->join();
+        capture_thread_.reset();
+    }
+
+    try {
+        pipe_.stop();
+    } catch (const std::exception& e) {
+        std::cerr << "停止pipeline时出错: " << e.what() << std::endl;
+    }
+
+    std::cout << "RealSense停止采集" << std::endl;
+}
+
+std::string RealSenseCapture::get_device_info() const {
+    try {
+        rs2::device_list devices = ctx_.query_devices();
+        if (devices.size() == 0) {
+            return "No RealSense device found";
+        }
+
+        std::string info;
+        for (size_t i = 0; i < devices.size(); i++) {
+            rs2::device dev = devices[i];
+            info += "Device " + std::to_string(i + 1) + ": ";
+            info += dev.get_info(RS2_CAMERA_INFO_NAME);
+            info += " (SN: " + std::string(dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER)) + ")";
+            if (i < devices.size() - 1) info += "\n";
+        }
+        return info;
+
+    } catch (const std::exception& e) {
+        return "Failed to get device info: " + std::string(e.what());
+    }
+}
+
+} // namespace ffmpeg