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

update camera intrinsics

Browse Source
This commit is contained in:
linbo 2025-08-25 14:03:11 +08:00
parent 5b2de6be0e
commit 3582d84176
5 changed files with 64 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/service/grpc_camera_service.h
View File

@ -26,11 +26,6 @@ namespace cmvr::service {
grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
public:
void read_message_(grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback
, cmvr::api::GetRGBImageStreamCommand_Request>* stream);
void write_message_(grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback
, cmvr::api::GetRGBImageStreamCommand_Request>* stream,std::shared_ptr<cmvr::device::AbstractCamera> dev);
private:
device::DeviceManager& dmgr_;

17
protos/cmvr/api/camera_command.proto
View File

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

21
src/devices/camera/realsense_camera/realsense_camera.cpp
View File

@ -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::align>(RS2_STREAM_COLOR);
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(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<rs2::video_stream_profile>().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];
}
// 保存图像数据
{

5
src/devices/camera/realsense_camera/realsense_camera.h
View File

@ -7,7 +7,7 @@
#include "camera/uvc_camera/uvc_camera.h"
#include <librealsense2/rs.hpp>
#include <librealsense2/hpp/rs_internal.hpp>
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_;

73
src/service/grpc_camera_service.cpp
View File

@ -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<cmvr::api::GetRGBImageStreamCommand_Feedback
, cmvr::api::GetRGBImageStreamCommand_Request>* 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<cmvr::api::GetRGBImageStreamCommand_Feedback
, cmvr::api::GetRGBImageStreamCommand_Request>* stream,std::shared_ptr<cmvr::device::AbstractCamera> 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));
}
}