i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-12 19:20:20 +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/include/orbbec_camera/utils.h
+7 -2
View File
@@ -206,8 +206,13 @@ bool isGemini2R(int pid);
OBStreamType obStreamTypeFromString(const std::string& stream_type);
cv::Mat undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
const OBCameraDistortion& distortion);
struct UndistortedImageResult {
cv::Mat image;
OBCameraIntrinsic new_intrinsic;
};
UndistortedImageResult undistortImage(const cv::Mat& image, const OBCameraIntrinsic& intrinsic,
const OBCameraDistortion& distortion);
std::string getDistortionModels(OBCameraDistortion distortion);
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]);
}
orbbec_camera/src/utils.cpp
+20 -9
View File
@@ -928,9 +928,9 @@ OBStreamType obStreamTypeFromString(const std::string &stream_type) {
}
}
cv::Mat undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
const OBCameraDistortion &distortion) {
cv::Mat undistorted_image;
UndistortedImageResult undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
const OBCameraDistortion &distortion) {
UndistortedImageResult result;
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;
@@ -940,12 +940,23 @@ cv::Mat undistortImage(const cv::Mat &image, const OBCameraIntrinsic &intrinsic,
// 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;
std::cout << "1" << camera_matrix << std::endl;
cv::Size image_size(image.cols, image.rows);
cv::Mat new_camera_matrix =
cv::getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, image_size, 0.0, image_size);
std::cout << new_camera_matrix << std::endl;
// Undistort the image using the new camera matrix
cv::undistort(image, result.image, camera_matrix, dist_coeffs, new_camera_matrix);
std::cout << new_camera_matrix << std::endl;
// Update the intrinsic parameters with the new camera matrix
result.new_intrinsic = intrinsic; // Copy original values first
result.new_intrinsic.fx = new_camera_matrix.at<double>(0, 0);
result.new_intrinsic.fy = new_camera_matrix.at<double>(1, 1);
result.new_intrinsic.cx = new_camera_matrix.at<double>(0, 2);
result.new_intrinsic.cy = new_camera_matrix.at<double>(1, 2);
result.new_intrinsic.width = image.cols;
result.new_intrinsic.height = image.rows;
return result;
}
std::string getDistortionModels(OBCameraDistortion distortion) {
switch (distortion.model) {