diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index a3082602..7bf2d637 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -39,6 +39,9 @@ namespace cmvr::device { Rs2Intrinsics intrinsics{}; int width = 0; int height = 0; + // Depth frames may use a different resolution from the encoded color frame. + int depth_width = 0; + int depth_height = 0; int fps = 0; bool bKey = false; bool depthKey = false; diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index e9db35ff..ff7aa47f 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -5,6 +5,8 @@ #include "../include/realsense_camera.h" +#include + using namespace std; using namespace cmvr::device; @@ -180,8 +182,9 @@ bool RealsenseCamera::init() { //初始化编码器 // 初始化RGB编码器（示例参数：640x480，30fps，H.264） - if (!CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { - CMVR_LOG(ERROR) << "[RealsenseCamera] (start): Failed to init RGB encoder!"; + if (stream_mode_ != DEPTH_MODE && + !CameraStreamEncoder::init(rgbEncoder_, codec_, encode_width_, encode_height_, fps_)) { + CMVR_LOG(ERROR) << "[RealsenseCamera] (init): Failed to init RGB encoder!"; state_.is_initialized = false; state_.is_error = true; state_.error_message = "Failed to init RGB encoder!"; @@ -253,16 +256,20 @@ bool RealsenseCamera::start() { intrinsics_ = Kd_; } - if (align_mode_ == "color") { - align_ = std::make_shared(RS2_STREAM_COLOR); - } - else if (align_mode_ == "depth") { - align_ = std::make_shared(RS2_STREAM_DEPTH); + if (stream_mode_ == RGBD_MODE) { + if (align_mode_ == "color") { + align_ = std::make_shared(RS2_STREAM_COLOR); + } + else if (align_mode_ == "depth") { + align_ = std::make_shared(RS2_STREAM_DEPTH); + } } // 启动后做一次首帧探测，避免“start 成功但长期无帧”假阳性。 rs2::frameset probe = pipe_.wait_for_frames(kProbeTimeoutMs); - if (!probe.get_color_frame()) { + const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE; + const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE; + if (needs_color && !probe.get_color_frame()) { last_error = "no color frame after start"; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; pipe_.stop(); @@ -270,7 +277,7 @@ bool RealsenseCamera::start() { std::this_thread::sleep_for(std::chrono::milliseconds(kRetrySleepMs)); continue; } - if (stream_mode_ == RGBD_MODE && !probe.get_depth_frame()) { + if (needs_depth && !probe.get_depth_frame()) { last_error = "no depth frame after start"; CMVR_LOG(WARNING) << "[RealsenseCamera] (start): " << last_error; pipe_.stop(); @@ -747,17 +754,19 @@ void RealsenseCamera::streaming_worker_() { rs2::frameset frames; frames = get_frameset(true); - rs2::frame color_frame = frames.get_color_frame(); - rs2::frame depth_frame = frames.get_depth_frame(); - if (!color_frame) { + rs2::video_frame color_frame = frames.get_color_frame(); + rs2::depth_frame depth_frame = frames.get_depth_frame(); + const bool needs_color = stream_mode_ == COLOR_MODE || stream_mode_ == RGBD_MODE; + const bool needs_depth = stream_mode_ == DEPTH_MODE || stream_mode_ == RGBD_MODE; + if (needs_color && !color_frame) { state_.is_error = true; state_.error_message = "missing color frame"; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } - if (stream_mode_ == RGBD_MODE && !depth_frame) { + if (needs_depth && !depth_frame) { state_.is_error = true; - state_.error_message = "missing depth frame in RGBD mode"; + state_.error_message = "missing depth frame"; CMVR_LOG(ERROR) << "[RealsenseCamera]streaming_worker_: " << state_.error_message; break; } @@ -774,43 +783,62 @@ void RealsenseCamera::streaming_worker_() { // 保存图像数据 { - cv::Mat temp(cv::Size(width_, height_), CV_8UC3, const_cast(color_frame.get_data()), cv::Mat::AUTO_STEP); - temp.copyTo(frame_data.rgbImage); // 执行深拷贝 + if (color_frame) { + cv::Mat temp(cv::Size(color_frame.get_width(), color_frame.get_height()), + CV_8UC3, const_cast(color_frame.get_data()), cv::Mat::AUTO_STEP); + temp.copyTo(frame_data.rgbImage); // 执行深拷贝 + } } if (depth_frame) { - auto temp = cv::Mat(cv::Size(width_, height_), CV_16UC1, const_cast(depth_frame.get_data())); + cv::Mat temp(cv::Size(depth_frame.get_width(), depth_frame.get_height()), + CV_16UC1, const_cast(depth_frame.get_data()), cv::Mat::AUTO_STEP); temp.copyTo(frame_data.depthImage); // 执行深拷贝 } // 记录编码开始时间 + if (depth_frame) { + frame_data.depth_width = frame_data.depthImage.cols; + frame_data.depth_height = frame_data.depthImage.rows; + frame_data.depthKey = true; + const size_t depth_bytes = frame_data.depthImage.total() * frame_data.depthImage.elemSize(); + frame_data.depthFrame.resize(depth_bytes); + std::memcpy(frame_data.depthFrame.data(), frame_data.depthImage.data, depth_bytes); + } + } + auto encode_start_time = std::chrono::high_resolution_clock::now(); // rgb图像编码 - cv::Mat rgb_to_encode = frame_data.rgbImage; - if (encode_width_ > 0 && encode_height_ > 0 && - (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { - cv::resize(frame_data.rgbImage, - rgb_to_encode, - cv::Size(encode_width_, encode_height_), - 0.0, - 0.0, - cv::INTER_LINEAR); + success = true; + if (needs_color) { + cv::Mat rgb_to_encode = frame_data.rgbImage; + if (encode_width_ > 0 && encode_height_ > 0 && + (frame_data.rgbImage.cols != encode_width_ || frame_data.rgbImage.rows != encode_height_)) { + cv::resize(frame_data.rgbImage, + rgb_to_encode, + cv::Size(encode_width_, encode_height_), + 0.0, + 0.0, + cv::INTER_LINEAR); + } + CameraStreamEncodeOptions encode_options; + encode_options.draw_timestamp = enable_stream_timestamp_; + success = CameraStreamEncoder::encode(rgbEncoder_, + rgb_to_encode, + frame_data.rgbFrame, + frame_data.bKey, + encode_options); } - CameraStreamEncodeOptions encode_options; - encode_options.draw_timestamp = enable_stream_timestamp_; - success = CameraStreamEncoder::encode(rgbEncoder_, - rgb_to_encode, - frame_data.rgbFrame, - frame_data.bKey, - encode_options); - // 深度图编码 - // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey); + if (needs_depth && frame_data.depthFrame.empty()) + success = false; if (success) { const uint64_t sequence = frame_sequence++; frame_data.stream_epoch = stream_epoch; frame_data.sequence = sequence; - frame_data.source_timestamp = static_cast(color_frame.get_timestamp()); - frame_data.source_frame_number = color_frame.get_frame_number(); + const rs2::frame source_frame = color_frame ? color_frame : depth_frame; + frame_data.source_timestamp = source_frame + ? static_cast(source_frame.get_timestamp()) : 0; + frame_data.source_frame_number = source_frame ? source_frame.get_frame_number() : 0; frame_data.capture_monotonic_ns = std::chrono::duration_cast( frame_start_time.time_since_epoch()).count(); frame_data.capture_utc_ns = std::chrono::duration_cast( @@ -821,9 +849,9 @@ void RealsenseCamera::streaming_worker_() { frame_data.time_base_den = fps_; frame_data.duration = 1; frame_data.fps = fps_; - frame_data.width = encode_width_; - frame_data.height = encode_height_; - frame_data.codec = codec_; + frame_data.width = needs_color ? encode_width_ : frame_data.depth_width; + frame_data.height = needs_color ? encode_height_ : frame_data.depth_height; + frame_data.codec = needs_color ? codec_ : "none"; stream_frame_buffer_->push(frame_data); } // 计算从帧开始到现在的总耗时 diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 08bc739c..42804863 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -12,6 +12,7 @@ #include #include #include +#include using namespace std; using namespace cmvr::service; @@ -219,6 +220,13 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + // FrameData::U8C3 is defined as RGB byte order. OpenCV/RealSense + // color frames are BGR, so normalize the single-frame contract here. + if (image.type() == CV_8UC3) { + cv::Mat rgb_image; + cv::cvtColor(image, rgb_image, cv::COLOR_BGR2RGB); + image = std::move(rgb_image); + } auto imageType = image.type(); if (imageType == CV_8UC1) { response->mutable_color_frame()->set_type(api::FrameData::U8C1); @@ -337,8 +345,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - // color image + // FrameData::U8C3 is defined as RGB byte order. OpenCV/RealSense + // color frames are BGR, so normalize the single-frame contract here. if (color_image.type() == CV_8UC3) { + cv::Mat rgb_image; + cv::cvtColor(color_image, rgb_image, cv::COLOR_BGR2RGB); + color_image = std::move(rgb_image); response->mutable_color_frame()->set_type(api::FrameData::U8C3); } else if (color_image.type() == CV_16UC3) { @@ -520,11 +532,12 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); + response.mutable_depth_frame()->set_type(api::FrameData::U16C1); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - 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_depth_frame()->set_codec("none"); + response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); @@ -606,11 +619,12 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con response.mutable_color_frame()->set_width(frame_data.width); response.mutable_color_frame()->set_height(frame_data.height); + response.mutable_depth_frame()->set_type(api::FrameData::U16C1); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - 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_depth_frame()->set_codec("none"); + response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy);