Fix out of bounds indexes when retrieving camera matrix for rectified…

… images (#47) * Fix out of bounds indexes when retrieving camera matrix for rectified images Co-authored-by: Sandip Das <[email protected]> * Add image_is_rectified parameter to example yaml config --------- Co-authored-by: Sandip Das <[email protected]>
fictionlab · Jun 4, 2024 · 988501b · 988501b
1 parent 146e912
commit 988501b
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git a/aruco_opencv/config/aruco_tracker.yaml b/aruco_opencv/config/aruco_tracker.yaml
@@ -5,6 +5,7 @@
 
     marker_dict: 4X4_50
 
+    image_is_rectified: false
     image_sub_compressed: false
     image_sub_qos:
       reliability: 2 # 0 - system default, 1 - reliable, 2 - best effort

diff --git a/aruco_opencv/src/aruco_tracker.cpp b/aruco_opencv/src/aruco_tracker.cpp
@@ -445,8 +445,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
     std::lock_guard<std::mutex> guard(cam_info_mutex_);
 
     if (image_is_rectified_) {
-      for (int i = 0; i < 12; ++i) {
-        camera_matrix_.at<double>(i / 4, i % 4) = cam_info->p[i];
+      for (int i = 0; i < 9; ++i) {
+        camera_matrix_.at<double>(i / 3, i % 3) = cam_info->p[i + i / 3];
       }
     } else {
       for (int i = 0; i < 9; ++i) {