From 3582d84176202771d92fbdf578c6b46d71196edc Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Mon, 25 Aug 2025 14:03:11 +0800 Subject: [PATCH] update camera intrinsics --- include/service/grpc_camera_service.h | 5 -- protos/cmvr/api/camera_command.proto | 17 ++++- .../realsense_camera/realsense_camera.cpp | 21 +++++- .../realsense_camera/realsense_camera.h | 5 +- src/service/grpc_camera_service.cpp | 73 +++++++------------ 5 files changed, 64 insertions(+), 57 deletions(-) diff --git a/include/service/grpc_camera_service.h b/include/service/grpc_camera_service.h index 6df9139e..9fd29510 100644 --- a/include/service/grpc_camera_service.h +++ b/include/service/grpc_camera_service.h @@ -26,11 +26,6 @@ namespace cmvr::service { grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter* stream) override; - public: - void read_message_(grpc::ServerReaderWriter* stream); - void write_message_(grpc::ServerReaderWriter* stream,std::shared_ptr dev); private: device::DeviceManager& dmgr_; diff --git a/protos/cmvr/api/camera_command.proto b/protos/cmvr/api/camera_command.proto index 36dfe9ac..6984c83f 100644 --- a/protos/cmvr/api/camera_command.proto +++ b/protos/cmvr/api/camera_command.proto @@ -21,6 +21,14 @@ message FrameData { bool is_key_frame = 6; } +message Rs2Intrinsics { + float cx = 1; // 主点水平坐标（从左边缘的像素偏移） + float cy = 2; // 主点垂直坐标（从上边缘的像素偏移） + float fx = 3; // x方向焦距（像素宽度的倍数） + float fy = 4; // y方向焦距（像素高度的倍数） + repeated float coeffs = 5 [packed = true]; // 畸变系数（固定5个元素） +} + message CameraState { bool is_initialized = 1; bool is_opened = 2; @@ -126,7 +134,8 @@ message GetRGBImageStreamCommand { message Feedback { CommandHeader.Feedback header = 1; FrameData color_frame = 2; - int32 seq_no = 3; + Rs2Intrinsics intrinsics = 3; + int32 seq_no = 4; } } @@ -139,7 +148,8 @@ message GetDepthImageStreamCommand { message Feedback { CommandHeader.Feedback header = 1; FrameData depth_frame = 2; - int32 seq_no = 3; + Rs2Intrinsics intrinsics = 3; + int32 seq_no = 4; } } @@ -153,7 +163,8 @@ message GetRGBDImagesStreamCommand { CommandHeader.Feedback header = 1; FrameData color_frame = 2; FrameData depth_frame = 3; - int32 seq_no = 4; + Rs2Intrinsics intrinsics = 4; + int32 seq_no = 5; } } diff --git a/src/devices/camera/realsense_camera/realsense_camera.cpp b/src/devices/camera/realsense_camera/realsense_camera.cpp index ea19cd0b..3a597a30 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.cpp +++ b/src/devices/camera/realsense_camera/realsense_camera.cpp @@ -84,6 +84,7 @@ RealsenseCamera::RealsenseCamera(const XmlNode& config) : AbstractCamera(config) max_retry_ = cfg_.getAttrDefault("max_retry", 5); is_sync_ = cfg_.getAttrDefault("sync", true); + align_mode_ = cfg_.getAttrDefault("align_mode", "color"); state_.fps = fps_; state_.width = width_; state_.height = height_; @@ -136,7 +137,12 @@ void RealsenseCamera::init() { rs_cfg_.enable_device(serial_); if (stream_mode_ == RGBD_MODE){ - align_ = std::make_shared(RS2_STREAM_COLOR); + if (align_mode_ == "color") { + align_ = std::make_shared(RS2_STREAM_COLOR); + } + else if (align_mode_ == "depth") { + align_ = std::make_shared(RS2_STREAM_DEPTH); + } rs_cfg_.enable_stream(RS2_STREAM_COLOR, state_.width, state_.height, RS2_FORMAT_BGR8, state_.fps); rs_cfg_.enable_stream(RS2_STREAM_DEPTH, state_.width, state_.height, RS2_FORMAT_Z16, state_.fps); } @@ -185,6 +191,12 @@ void RealsenseCamera::start() { } // 使用相同的同步策略启动 profile_ = pipe_.start(rs_cfg_); + auto frames = get_frameset(true); + rs2::frame color_frame = frames.get_color_frame(); + auto profile = color_frame.get_profile(); + auto color_profile = frames.get_color_frame().get_profile(); + intrinsics_ = color_profile.as().get_intrinsics(); + state_.is_opened = true; LOG(INFO) << "realsense start streaming successfully"; } @@ -540,6 +552,13 @@ void RealsenseCamera::streaming_worker_() { // 以下为编码部分，用于流模式 StreamFrameData frame_data; + frame_data.intrinsics.cx = intrinsics_.ppx; + frame_data.intrinsics.cy = intrinsics_.ppy; + frame_data.intrinsics.fx = intrinsics_.fx; + frame_data.intrinsics.fy = intrinsics_.fy; + for (int i = 0; i < 5 ; i++) { + frame_data.intrinsics.coeffs[i] = intrinsics_.coeffs[i]; + } // 保存图像数据 { diff --git a/src/devices/camera/realsense_camera/realsense_camera.h b/src/devices/camera/realsense_camera/realsense_camera.h index 27e74b28..f72f9712 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.h +++ b/src/devices/camera/realsense_camera/realsense_camera.h @@ -7,7 +7,7 @@ #include "camera/uvc_camera/uvc_camera.h" #include - +#include namespace cmvr::device{ enum StreamMode {RGBD_MODE, COLOR_MODE,DEPTH_MODE}; @@ -48,6 +48,7 @@ namespace cmvr::device{ int width_; int height_; std::string serial_; + std::string align_mode_; cv::VideoCapture cap_; size_t buffer_size_; std::string codec_; @@ -62,6 +63,8 @@ namespace cmvr::device{ rs2::pipeline pipe_; rs2::pipeline_profile profile_; rs2::config rs_cfg_; + rs2_intrinsics intrinsics_; + std::mutex frame_mtx_; cv::Mat dist_coeffs_; diff --git a/src/service/grpc_camera_service.cpp b/src/service/grpc_camera_service.cpp index 6c34d4dd..ddf167a6 100644 --- a/src/service/grpc_camera_service.cpp +++ b/src/service/grpc_camera_service.cpp @@ -295,6 +295,15 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con response.mutable_depth_frame()->set_codec(frame_data.codec); response.mutable_depth_frame()->set_width(frame_data.width); response.mutable_depth_frame()->set_height(frame_data.height); + + response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx); + for (int i = 0; i < 5 ; i++) { + response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); + } + response.set_seq_no(nFrameCount++); grpc::WriteOptions options; @@ -354,6 +363,14 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con response.mutable_depth_frame()->set_width(frame_data.width); response.mutable_depth_frame()->set_height(frame_data.height); + response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx); + for (int i = 0; i < 5 ; i++) { + response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); + } + response.set_seq_no(nFrameCount++); grpc::WriteOptions options; @@ -405,6 +422,15 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte response.mutable_color_frame()->set_codec(frame_data.codec); response.mutable_color_frame()->set_width(frame_data.width); response.mutable_color_frame()->set_height(frame_data.height); + + response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cx(frame_data.intrinsics.fx); + response.mutable_intrinsics()->set_cy(frame_data.intrinsics.fx); + for (int i = 0; i < 5 ; i++) { + response.mutable_intrinsics()->add_coeffs(frame_data.intrinsics.coeffs[i]); + } + response.set_seq_no(nFrameCount++); grpc::WriteOptions options; @@ -429,50 +455,3 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte return grpc::Status::OK; } } - -void gRPCCameraServiceImpl::read_message_(grpc::ServerReaderWriter* stream) -{ - try { - api::GetRGBImageStreamCommand_Request request; - - while (running_.load()) - { - stream->Read(&request); - if (request.eof()) - { - running_.store(false); - } - } - } - catch (exception &e) { - running_.store(false); - } -} -void gRPCCameraServiceImpl::write_message_(grpc::ServerReaderWriter* stream,std::shared_ptr dev) -{ - - int nFrameCount = 0; - size_t index = 0; - while (running_) - { - cmvr::device::StreamFrameData frame_data; - - dev->getEncodedFrame(frame_data,index); - - api::GetRGBImageStreamCommand_Feedback response; - response.mutable_header()->set_success(true); - - response.mutable_color_frame()->set_data(frame_data.rgbFrame.data(), frame_data.rgbFrame.size()); - response.mutable_color_frame()->set_codec(frame_data.codec); - response.mutable_color_frame()->set_width(frame_data.width); - response.mutable_color_frame()->set_height(frame_data.height); - response.set_seq_no(nFrameCount++); - - stream->Write(response); - - //先测试延时1s再去获取下一帧 - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } -}