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

Merge branch 'fix/undistortion' into v2-main

Browse Source
This commit is contained in:
ob-yalian
2026-05-06 17:05:24 +08:00
parent debc8b01b0 a631e6a382
commit a0aa03972b
3 changed files with 113 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+39 -16
View File
@@ -3085,6 +3085,10 @@ void OBCameraNode::setupPublishers() {
camera_info_qos_profile));
}
if (stream_index == COLOR && enable_color_undistortion_) {
color_undistortion_camera_info_publisher_ = node_->create_publisher<CameraInfo>(
"color/camera_info_undistorted",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(camera_info_qos_profile),
camera_info_qos_profile));
if (use_intra_process_) {
color_undistortion_publisher_ = std::make_shared<image_rcl_publisher>(
*node_, "color/image_undistorted", image_qos_profile);
@@ -4299,11 +4303,28 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
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 (color_undistortion_camera_info_publisher_) {
auto undistorted_camera_info = camera_info;
const auto distortion_coeff_count =
undistorted_camera_info.d.empty() ? 5 : undistorted_camera_info.d.size();
undistorted_camera_info.d.assign(distortion_coeff_count, 0.0);
undistorted_camera_info.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
undistorted_camera_info.roi.do_rectify = false;
undistorted_camera_info.k.fill(0.0);
undistorted_camera_info.k.at(0) = undistort_result.new_intrinsic.fx;
undistorted_camera_info.k.at(4) = undistort_result.new_intrinsic.fy;
undistorted_camera_info.k.at(2) = undistort_result.new_intrinsic.cx;
undistorted_camera_info.k.at(5) = undistort_result.new_intrinsic.cy;
undistorted_camera_info.k.at(8) = 1.0;
undistorted_camera_info.p.fill(0.0);
undistorted_camera_info.p.at(0) = undistort_result.new_intrinsic.fx;
undistorted_camera_info.p.at(5) = undistort_result.new_intrinsic.fy;
undistorted_camera_info.p.at(2) = undistort_result.new_intrinsic.cx;
undistorted_camera_info.p.at(6) = undistort_result.new_intrinsic.cy;
undistorted_camera_info.p.at(10) = 1.0;
color_undistortion_camera_info_publisher_->publish(undistorted_camera_info);
}
}
CHECK(camera_info_publishers_.count(stream_index) > 0);
@@ -4314,23 +4335,25 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
image = image * depth_scale;
}
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
CHECK(image_publishers_.count(stream_index) > 0);
saveImageToFile(stream_index, image, *image_msg);
if (stream_index == COLOR) {
fps_delay_status_color_->tick(frame_timestamp);
} else if (stream_index == DEPTH) {
fps_delay_status_depth_->tick(frame_timestamp);
}
if (has_raw_image_subscriber) {
if (has_raw_image_subscriber || save_images_[stream_index]) {
sensor_msgs::msg::Image::UniquePtr image_msg(new sensor_msgs::msg::Image());
cv_bridge::CvImage(std_msgs::msg::Header(), encoding_[stream_index], image)
.toImageMsg(*image_msg);
CHECK_NOTNULL(image_msg.get());
image_msg->header.stamp = timestamp;
image_msg->is_bigendian = false;
image_msg->step = width * unit_step_size_[stream_index];
image_msg->header.frame_id = frame_id;
saveImageToFile(stream_index, image, *image_msg);
if (!has_raw_image_subscriber) {
return;
}
if (frame_timestamp_csv_logger_ && frame_timestamp_csv_logger_->enabled() &&
(stream_index == COLOR || stream_index == DEPTH)) {
frame_timestamp_csv_logger_->recordPreImagePublish(stream_index.first, frame,
orbbec_camera/src/utils.cpp
+72 -20
View File
@@ -14,7 +14,10 @@
* limitations under the License.
*******************************************************************************/
#include <algorithm>
#include <mutex>
#include <regex>
#include <vector>
#include "orbbec_camera/utils.h"
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include "orbbec_camera/constants.h"
@@ -995,30 +998,79 @@ OBStreamType obStreamTypeFromString(const std::string &stream_type) {
}
}
namespace {
bool isSameIntrinsic(const OBCameraIntrinsic &lhs, const OBCameraIntrinsic &rhs) {
return lhs.fx == rhs.fx && lhs.fy == rhs.fy && lhs.cx == rhs.cx && lhs.cy == rhs.cy &&
lhs.width == rhs.width && lhs.height == rhs.height;
}
bool isSameDistortion(const OBCameraDistortion &lhs, const OBCameraDistortion &rhs) {
return lhs.model == rhs.model && lhs.k1 == rhs.k1 && lhs.k2 == rhs.k2 && lhs.k3 == rhs.k3 &&
lhs.k4 == rhs.k4 && lhs.k5 == rhs.k5 && lhs.k6 == rhs.k6 && lhs.p1 == rhs.p1 &&
lhs.p2 == rhs.p2;
}
struct UndistortMapCacheEntry {
int width = 0;
int height = 0;
int image_type = 0;
OBCameraIntrinsic intrinsic{};
OBCameraDistortion distortion{};
cv::Mat map1;
cv::Mat map2;
};
} // namespace
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;
camera_matrix.at<double>(0, 2) = intrinsic.cx;
camera_matrix.at<double>(1, 2) = intrinsic.cy;
result.new_intrinsic = 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);
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);
// Undistort the image using the new camera matrix
// cv::undistort(image, result.image, camera_matrix, dist_coeffs, new_camera_matrix);
cv::undistort(image, result.image, camera_matrix, dist_coeffs);
// 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);
if (image.empty()) {
return result;
}
cv::Mat map1;
cv::Mat map2;
{
static std::mutex cache_mutex;
static std::vector<UndistortMapCacheEntry> map_cache;
std::lock_guard<std::mutex> lock(cache_mutex);
auto cache_it = std::find_if(
map_cache.begin(), map_cache.end(), [&](const UndistortMapCacheEntry &entry) {
return entry.width == image.cols && entry.height == image.rows &&
entry.image_type == image.type() && isSameIntrinsic(entry.intrinsic, intrinsic) &&
isSameDistortion(entry.distortion, distortion);
});
if (cache_it == map_cache.end()) {
UndistortMapCacheEntry entry;
entry.width = image.cols;
entry.height = image.rows;
entry.image_type = image.type();
entry.intrinsic = intrinsic;
entry.distortion = distortion;
cv::Mat camera_matrix =
(cv::Mat_<double>(3, 3) << intrinsic.fx, 0.0, intrinsic.cx, 0.0, intrinsic.fy,
intrinsic.cy, 0.0, 0.0, 1.0);
cv::Mat dist_coeffs =
(cv::Mat_<double>(8, 1) << distortion.k1, distortion.k2, distortion.p1, distortion.p2,
distortion.k3, distortion.k4, distortion.k5, distortion.k6);
cv::initUndistortRectifyMap(camera_matrix, dist_coeffs, cv::Mat(), camera_matrix,
cv::Size(image.cols, image.rows), CV_16SC2, entry.map1,
entry.map2);
map_cache.emplace_back(std::move(entry));
cache_it = std::prev(map_cache.end());
}
map1 = cache_it->map1;
map2 = cache_it->map2;
}
result.image.create(image.size(), image.type());
cv::remap(image, result.image, map1, map2, cv::INTER_LINEAR);
result.new_intrinsic.width = image.cols;
result.new_intrinsic.height = image.rows;
return result;