i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 12:07:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix the problem of abnormal internal parameter acquisition for individual USB devices.

Browse Source
This commit is contained in:
lixiaobin
2023-10-25 10:23:24 +08:00
parent 1b225a0587
commit 16befe51ee
1 changed files with 29 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+29 -1
View File
@@ -934,7 +934,13 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
int height = static_cast<int>(video_frame->height());
auto timestamp = frameTimeStampToROSTime(video_frame->systemTimeStamp());
camera_param_ = pipeline_->getCameraParam();
if (!camera_param_ && depth_registration_) {
camera_param_ = pipeline_->getCameraParam();
} else if (!camera_param_ && stream_index == COLOR) {
camera_param_ = getColorCameraParam();
} else if (!camera_param_ && (stream_index == DEPTH || stream_index == INFRA0)) {
camera_param_ = getDepthCameraParam();
}
auto &intrinsic =
stream_index == COLOR ? camera_param_->rgbIntrinsic : camera_param_->depthIntrinsic;
auto &distortion =
@@ -1096,11 +1102,22 @@ std::optional<OBCameraParam> OBCameraNode::findDefaultCameraParam() {
std::optional<OBCameraParam> OBCameraNode::getDepthCameraParam() {
auto camera_params = device_->getCalibrationCameraParamList();
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int depth_w = param.depthIntrinsic.width;
int depth_h = param.depthIntrinsic.height;
if (depth_w == width_[DEPTH] && depth_h == height_[DEPTH]) {
RCLCPP_INFO_STREAM(logger_, "getCameraDepthParam w: " << depth_w << ",h:" << depth_h);
return param;
}
}
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int depth_w = param.depthIntrinsic.width;
int depth_h = param.depthIntrinsic.height;
if (depth_w * height_[DEPTH] == depth_h * width_[DEPTH]) {
RCLCPP_INFO_STREAM(logger_, "getCameraDepthParam w: " << depth_w << ",h:" << depth_h);
return param;
}
}
@@ -1109,11 +1126,22 @@ std::optional<OBCameraParam> OBCameraNode::getDepthCameraParam() {
std::optional<OBCameraParam> OBCameraNode::getColorCameraParam() {
auto camera_params = device_->getCalibrationCameraParamList();
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int color_w = param.rgbIntrinsic.width;
int color_h = param.rgbIntrinsic.height;
if (color_w == width_[COLOR] && color_h == height_[COLOR]) {
RCLCPP_INFO_STREAM(logger_, "getColorCameraParam w: " << color_w << ",h:" << color_h);
return param;
}
}
for (size_t i = 0; i < camera_params->count(); i++) {
auto param = camera_params->getCameraParam(i);
int color_w = param.rgbIntrinsic.width;
int color_h = param.rgbIntrinsic.height;
if (color_w * height_[COLOR] == color_h * width_[COLOR]) {
RCLCPP_INFO_STREAM(logger_, "getColorCameraParam w: " << color_w << ",h:" << color_h);
return param;
}
}