From 1b09e9cd4827fea2d5c3447660a6ca6c6a2b648b Mon Sep 17 00:00:00 2001
From: Wiktor Bajor <wiktorbajor1@gmail.com>
Date: Mon, 15 Jul 2024 17:10:22 +0200
Subject: [PATCH] Assert that image and depth image have equal shapes

---
 .../human_detector/human_detector.py          | 22 +++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/human_detector/human_detector/human_detector.py b/human_detector/human_detector/human_detector.py
index 2677274..3a5ac1b 100644
--- a/human_detector/human_detector/human_detector.py
+++ b/human_detector/human_detector/human_detector.py
@@ -110,15 +110,28 @@ def store_camera_info(self, info: CameraInfo):
         if not self.camera_info_is_stored:
             self.camera_info_is_stored = True
 
+    def are_rgb_image_same_size_as_depth_image(self):
+        rgb_image_height, rgb_image_width, _ = self.image.shape
+        depth_image_height, depth_image_width, _ = self.depth_image.shape
+
+        return rgb_image_height == depth_image_height and rgb_image_width == depth_image_width
+
     def store_human_pose(self):
         if not self.camera_info_is_stored or self.image is None or self.depth_image is None:
-            self.get_logger().error("No camera info or image or depth image are not stored")
+            self.get_logger().error(
+                "No camera info or image or depth image are not stored. Human will not be detected."
+            )
+            return
+
+        if not self.are_rgb_image_same_size_as_depth_image():
+            self.get_logger().error(
+                "Dimensions of rgb image and depth image are not equal. Human will not be detected."
+            )
             return
 
-        hight, width, _ = self.image.shape
         self.detected_landmarks = self.person_pose_estimator.process(self.image).pose_landmarks
         x_pos_of_detected_person, y_pos_of_detected_person = \
-            self.get_position_of_human_in_the_image(self.detected_landmarks, width, hight)
+            self.get_position_of_human_in_the_image(self.detected_landmarks)
 
         if x_pos_of_detected_person <= 0 or y_pos_of_detected_person <= 0:
             return
@@ -133,7 +146,8 @@ def store_human_pose(self):
             'z': mm_to_m(point_xyz[1])
         }
 
-    def get_position_of_human_in_the_image(self, detected_landmarks: NamedTuple, width: int, height: int):
+    def get_position_of_human_in_the_image(self, detected_landmarks: NamedTuple):
+        height, width, _ = self.image.shape
         x, y = 0, 0
         if detected_landmarks:
             landmarks = mp.solutions.pose.PoseLandmark