From b51d1770761e152d2a68371cb4a2f969cab8febd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 4 Jun 2024 18:33:59 +0200 Subject: [PATCH] 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 * Add image_is_rectified parameter to example yaml config --------- Co-authored-by: Sandip Das (cherry picked from commit 988501b9e27c73500a8bf56aff4cb1c3d247c39c) --- aruco_opencv/config/aruco_tracker.yaml | 1 + aruco_opencv/src/aruco_tracker.cpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/aruco_opencv/config/aruco_tracker.yaml b/aruco_opencv/config/aruco_tracker.yaml index 65dc4c7..06a0c18 100644 --- 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 index cab2d54..51d6933 100644 --- a/aruco_opencv/src/aruco_tracker.cpp +++ b/aruco_opencv/src/aruco_tracker.cpp @@ -438,8 +438,8 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode std::lock_guard guard(cam_info_mutex_); if (image_is_rectified_) { - for (int i = 0; i < 12; ++i) { - camera_matrix_.at(i / 4, i % 4) = cam_info->p[i]; + for (int i = 0; i < 9; ++i) { + camera_matrix_.at(i / 3, i % 3) = cam_info->p[i + i / 3]; } } else { for (int i = 0; i < 9; ++i) {