Better camera calibration handling (#3)

* Support different distortion models

* Support rectified images

(cherry picked from commit e45dee6)

aruco_opencv/src/single_marker_tracker.cpp

@@ -51,6 +51,7 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
 {
   // Parameters
   std::string cam_base_topic_;
+  bool image_is_rectified_;
   std::string output_frame_;
   std::string marker_dict_;
   bool transform_poses_;
@@ -87,8 +88,9 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
 
 public:
   SingleMarkerTracker(rclcpp::NodeOptions options)
-  : LifecycleNode("single_marker_tracker", options), camera_matrix_(3, 3, CV_64FC1),
-    distortion_coeffs_(4, 1, CV_64FC1),
+  : LifecycleNode("single_marker_tracker", options),
+    camera_matrix_(3, 3, CV_64FC1),
+    distortion_coeffs_(4, 1, CV_64FC1, cv::Scalar(0)),
     marker_obj_points_(4, 1, CV_32FC3)
   {
     declare_parameters();
@@ -216,6 +218,7 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
   void declare_parameters()
   {
     declare_param(*this, "cam_base_topic", "camera/image_raw");
+    declare_param(*this, "image_is_rectified", false, false);
     declare_param(*this, "output_frame", "");
     declare_param(*this, "marker_dict", "ARUCO_ORIGINAL");
     declare_param(
@@ -235,6 +238,10 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
   {
     get_param(*this, "cam_base_topic", cam_base_topic_, "Camera Base Topic: ");
 
+    get_parameter("image_is_rectified", image_is_rectified_);
+    RCLCPP_INFO_STREAM(
+      get_logger(), "Assume images are rectified: " << (image_is_rectified_ ? "YES" : "NO"));
+
     get_parameter("output_frame", output_frame_);
     if (output_frame_.empty()) {
       RCLCPP_INFO(get_logger(), "Marker detections will be published in the camera frame");
@@ -319,11 +326,15 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
   {
     std::lock_guard<std::mutex> guard(cam_info_mutex_);
 
-    for (int i = 0; i < 9; ++i) {
-      camera_matrix_.at<double>(i / 3, i % 3) = cam_info->k[i];
-    }
-    for (int i = 0; i < 4; ++i) {
-      distortion_coeffs_.at<double>(i, 0) = cam_info->d[i];
+    if (image_is_rectified_) {
+      for (int i = 0; i < 12; ++i) {
+        camera_matrix_.at<double>(i / 4, i % 4) = cam_info->p[i];
+      }
+    } else {
+      for (int i = 0; i < 9; ++i) {
+        camera_matrix_.at<double>(i / 3, i % 3) = cam_info->k[i];
+      }
+      distortion_coeffs_ = cv::Mat(cam_info->d, true);
     }
 
     if (!cam_info_retrieved_) {
@@ -361,21 +372,22 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
     detection.header.stamp = img_msg->header.stamp;
     detection.markers.resize(n_markers);
 
-    cam_info_mutex_.lock();
-    cv::parallel_for_(
-      cv::Range(0, n_markers), [&](const cv::Range & range) {
-        for (size_t i = range.start; i < range.end; i++) {
-          int id = marker_ids[i];
-
-          cv::solvePnP(
-            marker_obj_points_, marker_corners[i], camera_matrix_, distortion_coeffs_,
-            rvec_final[i], tvec_final[i], false, cv::SOLVEPNP_IPPE_SQUARE);
-
-          detection.markers[i].marker_id = id;
-          detection.markers[i].pose = convert_rvec_tvec(rvec_final[i], tvec_final[i]);
-        }
-      });
-    cam_info_mutex_.unlock();
+    {
+      std::lock_guard<std::mutex> guard(cam_info_mutex_);
+      cv::parallel_for_(
+        cv::Range(0, n_markers), [&](const cv::Range & range) {
+          for (size_t i = range.start; i < range.end; i++) {
+            int id = marker_ids[i];
+
+            cv::solvePnP(
+              marker_obj_points_, marker_corners[i], camera_matrix_, distortion_coeffs_,
+              rvec_final[i], tvec_final[i], false, cv::SOLVEPNP_IPPE_SQUARE);
+
+            detection.markers[i].marker_id = id;
+            detection.markers[i].pose = convert_rvec_tvec(rvec_final[i], tvec_final[i]);
+          }
+        });
+    }
 
     if (transform_poses_ && n_markers > 0) {
       detection.header.frame_id = output_frame_;
@@ -414,12 +426,14 @@ class SingleMarkerTracker : public rclcpp_lifecycle::LifecycleNode
     if (debug_pub_->get_subscription_count() > 0) {
       auto debug_cv_ptr = cv_bridge::toCvCopy(img_msg, "bgr8");
       cv::aruco::drawDetectedMarkers(debug_cv_ptr->image, marker_corners, marker_ids);
-      for (size_t i = 0; i < marker_ids.size(); i++) {
-        cv::drawFrameAxes(
-          debug_cv_ptr->image, camera_matrix_, distortion_coeffs_, rvec_final[i],
-          tvec_final[i], 0.2, 3);
+      {
+        std::lock_guard<std::mutex> guard(cam_info_mutex_);
+        for (size_t i = 0; i < marker_ids.size(); i++) {
+          cv::drawFrameAxes(
+            debug_cv_ptr->image, camera_matrix_, distortion_coeffs_, rvec_final[i],
+            tvec_final[i], 0.2, 3);
+        }
       }
-
       std::unique_ptr<sensor_msgs::msg::Image> debug_img =
         std::make_unique<sensor_msgs::msg::Image>();
       debug_cv_ptr->toImageMsg(*debug_img);