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

chore: Add enable_color_undistortion param

Browse Source
This commit is contained in:
Joe Dong
2024-07-31 14:30:54 +08:00
parent 9a3c415b70
commit 87c6ee7d97
7 changed files with 60 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+18
View File
@@ -1034,6 +1034,7 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(min_depth_limit_, "min_depth_limit", 0);
setAndGetNodeParameter<int>(max_depth_limit_, "max_depth_limit", 0);
setAndGetNodeParameter<bool>(enable_heartbeat_, "enable_heartbeat", false);
setAndGetNodeParameter<bool>(enable_color_undistortion_, "enable_color_undistortion", false);
if (enable_3d_reconstruction_mode_) {
laser_on_off_mode_ = 1; // 0 off, 1 on-off, 1 off-on
}
@@ -1170,6 +1171,10 @@ void OBCameraNode::setupPublishers() {
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
}
if (stream_index == COLOR && enable_color_undistortion_) {
color_undistortion_publisher_ =
image_transport::create_publisher(node_, "color/image_undistorted", image_qos_profile);
}
}
if (enable_sync_output_accel_gyro_) {
@@ -1853,6 +1858,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, image_msg);
image_publishers_[stream_index].publish(std::move(image_msg));
if (stream_index == COLOR && enable_color_undistortion_ &&
color_undistortion_publisher_.getNumSubscribers() > 0) {
auto undistorted_image = undistortImage(image, intrinsic, distortion);
auto undistorted_image_msg =
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], undistorted_image)
.toImageMsg();
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));
}
}
void OBCameraNode::publishMetadata(const std::shared_ptr<ob::Frame> &frame,
orbbec_camera/src/utils.cpp
+20
View File
@@ -852,4 +852,24 @@ OBStreamType obStreamTypeFromString(const std::string &stream_type) {
return OB_STREAM_UNKNOWN;
}
}
cv::Mat undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
const OBCameraDistortion &distortion) {
cv::Mat undistorted_image;
cv::Mat camera_matrix = cv::Mat::eye(3, 3, CV_64F);
camera_matrix.at<double>(0, 0) = intrinsic.fx;
camera_matrix.at<double>(1, 1) = intrinsic.fy;
camera_matrix.at<double>(0, 2) = intrinsic.cx;
camera_matrix.at<double>(1, 2) = intrinsic.cy;
// Create the distortion coefficients matrix using the extended distortion model
cv::Mat dist_coeffs = (cv::Mat_<float>(8, 1) << distortion.k1, distortion.k2, distortion.p1,
distortion.p2, distortion.k3, distortion.k4, distortion.k5, distortion.k6);
// Undistort the image using OpenCV's undistort function
// This function corrects for lens distortion
cv::undistort(image, undistorted_image, camera_matrix, dist_coeffs);
return undistorted_image;
}
} // namespace orbbec_camera