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

Update intrinsic with the new camera matrix from undistortion

Browse Source
This commit is contained in:
ob-yalian
2025-12-29 09:54:10 +08:00
parent 6c44310645
commit eafd7d21e4
3 changed files with 45 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+18 -15
View File
@@ -3152,20 +3152,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (depth_registration_ && stream_index == DEPTH) {
frame_id = depth_aligned_frame_id_[stream_index];
}
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_->get_subscription_count() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);
sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg(*undistorted_image_msg);
CHECK_NOTNULL(undistorted_image_msg.get());
undistorted_image_msg->header.stamp = timestamp;
undistorted_image_msg->is_bigendian = false;
undistorted_image_msg->step = width * unit_step_size_[stream_index];
undistorted_image_msg->header.frame_id = frame_id;
color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
memset(&distortion, 0, sizeof(distortion));
}
sensor_msgs::msg::CameraInfo camera_info{};
if (color_info_manager_ && color_info_manager_->isCalibrated() && stream_index == COLOR) {
camera_info = color_info_manager_->getCameraInfo();
@@ -3187,6 +3173,24 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
camera_info.width = width;
camera_info.height = height;
}
auto &image = images_[stream_index];
if (stream_index == COLOR && enable_color_undistortion_) {
auto undistort_result = undistortImage(image, intrinsic, distortion);
sensor_msgs::msg::Image::UniquePtr undistorted_image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistort_result.image)
.toImageMsg(*undistorted_image_msg);
CHECK_NOTNULL(undistorted_image_msg.get());
undistorted_image_msg->header.stamp = timestamp;
undistorted_image_msg->is_bigendian = false;
undistorted_image_msg->step = width * unit_step_size_[stream_index];
undistorted_image_msg->header.frame_id = frame_id;
color_undistortion_publisher_->publish(std::move(undistorted_image_msg));
// Update intrinsic with the new camera matrix from undistortion
camera_info.p.at(0) = undistort_result.new_intrinsic.fx;
camera_info.p.at(5) = undistort_result.new_intrinsic.fy;
camera_info.p.at(2) = undistort_result.new_intrinsic.cx;
camera_info.p.at(6) = undistort_result.new_intrinsic.cy;
}
if (frame->getType() == OB_FRAME_IR_RIGHT && enable_stream_[INFRA1]) {
auto stream_profile = frame->getStreamProfile();
CHECK_NOTNULL(stream_profile);
@@ -3209,7 +3213,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
if (image_publishers_[stream_index]->get_subscription_count() == 0) {
return;
}
auto &image = images_[stream_index];
if (image.empty() || image.cols != width || image.rows != height) {
image.create(height, width, image_format_[stream_index]);
}