SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add RealSenseCapture

Browse Source
This commit is contained in:
linbo 2026-03-06 16:16:58 +08:00
parent 17043684fe
commit 62a90c3e64
4 changed files with 491 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cmvr-es/common/utils/ffmpeg/include/CameraCapture.h
View File

@ -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();
};

94
cmvr-es/common/utils/ffmpeg/include/RealSenseCapture.h Normal file
View File

@ -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

87
cmvr-es/common/utils/ffmpeg/src/CameraCapture.cpp
View File

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

301
cmvr-es/common/utils/ffmpeg/src/RealSenseCapture.cpp Normal file
View File

@ -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