fix: enhance image processing by adding caching for undistortion maps

orbbec_camera/src/ob_camera_node.cpp

@@ -4314,23 +4314,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

@@ -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"
@@ -997,30 +1000,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);
+  result.new_intrinsic = intrinsic;
-  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
+  if (image.empty()) {
-  cv::Mat dist_coeffs = (cv::Mat_<float>(8, 1) << distortion.k1, distortion.k2, distortion.p1,
+    return result;
-                         distortion.p2, distortion.k3, distortion.k4, distortion.k5, distortion.k6);
+  }
-  cv::Size image_size(image.cols, image.rows);
+
-  // cv::Mat new_camera_matrix =
+  cv::Mat map1;
-  // cv::getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, image_size, 0.0, image_size);
+  cv::Mat map2;
-  // Undistort the image using the new camera matrix
+  {
-  // cv::undistort(image, result.image, camera_matrix, dist_coeffs, new_camera_matrix);
+    static std::mutex cache_mutex;
-  cv::undistort(image, result.image, camera_matrix, dist_coeffs);
+    static std::vector<UndistortMapCacheEntry> map_cache;
-  // Update the intrinsic parameters with the new camera matrix
+
-  result.new_intrinsic = intrinsic;  // Copy original values first
+    std::lock_guard<std::mutex> lock(cache_mutex);
-  // result.new_intrinsic.fx = new_camera_matrix.at<double>(0, 0);
+    auto cache_it = std::find_if(
-  // result.new_intrinsic.fy = new_camera_matrix.at<double>(1, 1);
+        map_cache.begin(), map_cache.end(), [&](const UndistortMapCacheEntry &entry) {
-  // result.new_intrinsic.cx = new_camera_matrix.at<double>(0, 2);
+          return entry.width == image.cols && entry.height == image.rows &&
-  // result.new_intrinsic.cy = new_camera_matrix.at<double>(1, 2);
+                 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;