From 62a90c3e64083b96dba0cd9e645eb7d244194771 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 6 Mar 2026 16:16:58 +0800 Subject: [PATCH] add RealSenseCapture --- .../utils/ffmpeg/include/CameraCapture.h | 18 +- .../utils/ffmpeg/include/RealSenseCapture.h | 94 ++++++ .../common/utils/ffmpeg/src/CameraCapture.cpp | 87 ++++- .../utils/ffmpeg/src/RealSenseCapture.cpp | 301 ++++++++++++++++++ 4 files changed, 491 insertions(+), 9 deletions(-) create mode 100644 cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h create mode 100644 cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp diff --git a/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h b/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h index b3561c27..6f902185 100644 --- a/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h +++ b/cmvr-es/common/utils/ffmpeg/include/CameraCapture.h @@ -4,26 +4,29 @@ #include #include #include +#include extern "C"{ #include #include #include +#include } 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; + // 回调函数：同时返回AVFrame和cv::Mat + using FrameCallback = std::function; 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(); }; diff --git a/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h b/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h new file mode 100644 index 00000000..66e5578d --- /dev/null +++ b/cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h @@ -0,0 +1,94 @@ +// RealSenseCapture.h +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +extern "C" { +#include +#include +#include +} + +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; + + 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 is_capturing_{false}; + std::unique_ptr 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 \ No newline at end of file diff --git a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp b/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp index 75ac0e70..001e0c18 100644 --- a/cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp +++ b/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; } - // 回调处理帧 - callback(frame_); + // 设置PTS + 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; diff --git a/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp b/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp new file mode 100644 index 00000000..d1165a49 --- /dev/null +++ b/cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp @@ -0,0 +1,301 @@ +// RealSenseCapture.cpp +#include "../include/RealSenseCapture.h" +#include +#include + +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 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(); + const int width = vf.get_width(); + const int height = vf.get_height(); + const uint8_t* data = reinterpret_cast(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(); + 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 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(); + 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(&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( + 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 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 \ No newline at end of file