Add ROI and binning handling (#16)

* Use `image_geometry` to crop ROI

* Fix type casting

* Fix QOS and expected matrix type

* Add base for image cropping

* Add advanced QOS settings

* Bring back standard single view example

* Remove accidentally created publisher

* Use reliable QoS in tests

* Fix unittests

* Fix `received` typos

* Fix unittest | Increase quality of compressed images

happypose_ros/happypose_ros/camera_wrapper.py

@@ -11,6 +11,7 @@
 from sensor_msgs.msg import CameraInfo, CompressedImage, Image
 
 from cv_bridge import CvBridge
+from image_geometry.cameramodels import PinholeCameraModel
 
 from message_filters import ApproximateTimeSynchronizer, Subscriber
 
@@ -60,7 +61,9 @@ def __init__(
             topic_postfix = "/image_raw"
 
         self._image = None
+        self._camera_info = None
         self._cvb = CvBridge()
+        self._cam_model = PinholeCameraModel()
 
         sync_topics = [
             Subscriber(
@@ -88,43 +91,24 @@ def __init__(
         # Register callback depending on the configuration
         self._image_approx_time_sync.registerCallback(self._on_image_data_cb)
 
-    def image_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]:
-        """Decorator, checks if image was already received.
+    def data_received_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]:
+        """Decorator, checks if data was already received.
 
         :param func: Function to wrap.
         :type func: Callable[..., RetType]
-        :raises RuntimeError: No images were received yet.
-        :return: Wrapped function.
-        :rtype: Callable[..., RetType]
-        """
-
-        def _image_guarded_inner(self, *args, **kwargs) -> RetType:
-            if self._image is None:
-                raise RuntimeError(
-                    f"No images received yet from camera '{self._camera_name}'!"
-                )
-            return func(self, *args, **kwargs)
-
-        return _image_guarded_inner
-
-    def k_matrix_guarded(func: Callable[..., RetType]) -> Callable[..., RetType]:
-        """Decorator, checks if an intrinsic matrix was already received.
-
-        :param func: Function to wrap.
-        :type func: Callable[..., RetType]
-        :raises RuntimeError: No images were received yet.
+        :raises RuntimeError: No data was received yet.
         :return: Wrapped function.
         :rtype: Callable[..., RetType]
         """
 
-        def _k_matrix_guarded_inner(self, *args, **kwargs) -> RetType:
-            if self._camera_k is None:
+        def _data_received_guarded_inner(self, *args, **kwargs) -> RetType:
+            if self._image is None and self._camera_info is None:
                 raise RuntimeError(
-                    f"Camera info was not received yet from camera '{self._camera_name}'!"
+                    f"No data received yet from the camera '{self._camera_name}'!"
                 )
             return func(self, *args, **kwargs)
 
-        return _k_matrix_guarded_inner
+        return _data_received_guarded_inner
 
     def _validate_k_matrix(self, k_arr: npt.NDArray[np.float64]) -> bool:
         """Performs basic check of the structure of intrinsic matrix.
@@ -153,30 +137,26 @@ def _on_image_data_cb(
         """
 
         if self._validate_k_matrix(info.k):
-            self._camera_k = info.k
+            self._camera_info = info
         else:
             self._node.get_logger().warn(
-                f"K matrix from topic '{self._info_sub.topic_name()}' is incorrect!"
-                f" Fix it or set 'cameras.{self._camera_name}.k_matrix'"
-                f" param for the camera '{self._camera_name}'.",
+                f"K matrix from topic '{self._info_sub.topic_name()}' is incorrect!",
                 throttle_duration_sec=5.0,
             )
             return
 
         self._image = image
-        # Fire the callback only if camera info is available
-        if self._camera_k is not None:
-            self._image_sync_hook()
+        self._image_sync_hook()
 
     def ready(self) -> bool:
         """Checks if the camera has all the data needed to use.
 
         :return: Camera image and intrinsic matrix are available
         :rtype: bool
         """
-        return self._image is not None and self._camera_k is not None
+        return self._image is not None
 
-    @image_guarded
+    @data_received_guarded
     def get_last_image_frame_id(self) -> str:
         """Returns frame id associated with the last received image.
 
@@ -186,7 +166,7 @@ def get_last_image_frame_id(self) -> str:
         """
         return self._image.header.frame_id
 
-    @image_guarded
+    @data_received_guarded
     def get_last_image_stamp(self) -> Time:
         """Returns time stamp associated with last received image.
 
@@ -196,7 +176,7 @@ def get_last_image_stamp(self) -> Time:
         """
         return Time.from_msg(self._image.header.stamp)
 
-    @image_guarded
+    @data_received_guarded
     def get_last_rgb_image(self) -> npt.NDArray[np.uint8]:
         """Returns last received color image.
 
@@ -217,12 +197,13 @@ def get_last_rgb_image(self) -> npt.NDArray[np.uint8]:
         )
         return encoder(self._image, desired_encoding)
 
-    @k_matrix_guarded
+    @data_received_guarded
     def get_last_k_matrix(self) -> npt.NDArray[np.float64]:
         """Returns intrinsic matrix associated with last received camera info message.
 
         :raises RuntimeError: No camera info messages were received yet.
         :return: 3x3 Numpy array with intrinsic matrix.
         :rtype: numpy.typing.NDArray[numpy.float64]
         """
-        return self._camera_k.reshape((3, 3))
+        self._cam_model.fromCameraInfo(self._camera_info)
+        return np.array(self._cam_model.intrinsicMatrix())

happypose_ros/package.xml

@@ -22,6 +22,7 @@
   <depend>tf2_ros</depend>
   <depend>geometry_msgs</depend>
   <depend>happypose_msgs</depend>
+  <depend>image_geometry</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>vision_msgs</depend>

happypose_ros/test/happypose_testing_utils.py

@@ -21,7 +21,7 @@
 from cv_bridge import CvBridge
 
 from geometry_msgs.msg import Pose, Transform
-from sensor_msgs.msg import CameraInfo, Image, CompressedImage
+from sensor_msgs.msg import CameraInfo, Image, CompressedImage, RegionOfInterest
 from sensor_msgs.msg._image import Metaclass_Image
 from std_msgs.msg import Header
 from vision_msgs.msg import Detection2D, Detection2DArray, BoundingBox2D, VisionInfo
@@ -302,6 +302,10 @@ def publish_image(
         bgr: npt.NDArray[np.uint8],
         K: npt.NDArray[np.float32],
         stamp: Optional[Time] = None,
+        width: int = 0,
+        height: int = 0,
+        x_offset: int = 0,
+        y_offset: int = 0,
     ) -> None:
         """Publish image for a given camera.
 
@@ -313,25 +317,51 @@ def publish_image(
         :type K: numpy.typing.NDArray[numpy.float32]
         :param stamp: Time stamp of the message. If None uses the current time, defaults to None.
         :type stamp: Optional[rclpy.Time], optional
+        :param width: Final cropped image width. 0 means no cropping.
+        :type width: int, optional
+        :param height: Final cropped image height. 0 means no cropping.
+        :type height: int, optional
+        :param x_offset: X offset used for cropping.
+        :type x_offset: int, optional
+        :param y_offset: Y offset used for cropping.
+        :type y_offset: int, optional
         """
         if stamp is None:
             stamp = self.get_clock().now()
 
+        height = bgr.shape[0] if height == 0 else height
+        width = bgr.shape[1] if width == 0 else width
+        cropped_bgr = bgr[
+            y_offset : y_offset + height,
+            x_offset : x_offset + width,
+        ]
+
         if isinstance(self._cam_pubs[cam][0].msg_type, Metaclass_Image):
-            img_msg = self._cvb.cv2_to_imgmsg(bgr, encoding="rgb8")
+            img_msg = self._cvb.cv2_to_imgmsg(cropped_bgr, encoding="rgb8")
         else:
             # Convert BGR to RGB
-            rgb = bgr[:, :, ::-1]
-            img_msg = self._cvb.cv2_to_compressed_imgmsg(rgb, dst_format="jpg")
+            rgb = cropped_bgr[:, :, ::-1]
+            img_msg = self._cvb.cv2_to_compressed_imgmsg(rgb, dst_format="png")
 
         header = Header(frame_id=cam, stamp=stamp.to_msg())
         img_msg.header = header
         info_msg = CameraInfo(
             header=header,
+            # Resized dimensions are specified in ROI message.
+            # Here shape before resizing is expected.
             height=bgr.shape[0],
             width=bgr.shape[1],
             k=K.reshape(-1),
         )
+        if bgr.shape != cropped_bgr.shape:
+            info_msg.roi = RegionOfInterest(
+                width=width,
+                height=height,
+                x_offset=x_offset,
+                y_offset=y_offset,
+                do_rectify=True,
+            )
+
         # Publish messages
         self._cam_pubs[cam][0].publish(img_msg)
         self._cam_pubs[cam][1].publish(info_msg)

happypose_ros/test/single_view_base.py

@@ -398,30 +398,46 @@ def test_12_dynamic_params_labels_to_keep_reset(self: ActiveIoHandler) -> None:
             msg="Number of objects with symmetries is not equal to full YCBV dataset!",
         )
 
-    def test_13_dynamic_params_labels_to_keep_empty_message(
-        self: ActiveIoHandler,
-    ) -> None:
-        # Undo filtering of the labels
-        self.node.set_params(
-            [
-                Parameter(
-                    "cosypose.inference.labels_to_keep",
-                    Parameter.Type.STRING_ARRAY,
-                    ["ycbv-obj_000020"],
-                )
-            ]
-        )
-
+    def test_13_region_of_interest(self: ActiveIoHandler) -> None:
         # Clear old messages
         self.node.clear_msg_buffer()
 
         # Publish new to trigger parameter change
-        self.node.publish_image("cam_1", self.rgb, self.K)
+        self.node.publish_image(
+            "cam_1",
+            self.rgb,
+            self.K,
+            width=504,
+            height=378,
+            x_offset=10,
+            y_offset=51,
+        )
+
         self.node.assert_message_received("happypose/detections", timeout=20.0)
         detections = self.node.get_received_message("happypose/detections")
 
         self.assertGreaterEqual(
-            len(detections.detections),
-            0,
-            "After filtering labels published message is not empty!",
+            len(detections.detections), 2, "Incorrect number of detected objects!"
+        )
+
+        # In compressed case pose of 05 object is not reliable
+        ycbv_02 = assert_and_find_detection(detections, "ycbv-obj_000002")
+        ycbv_15 = assert_and_find_detection(detections, "ycbv-obj_000015")
+
+        # Based on ground truth, object poses for image 629
+        ycbv_02_pose = Pose(
+            position=Point(**dict(zip("xyz", [0.0552, -0.0913, 1.0283]))),
+            orientation=Quaternion(
+                **dict(zip("xyzw", [0.2279, 0.1563, 0.0245, 0.9607]))
+            ),
+        )
+
+        ycbv_15_pose = Pose(
+            position=Point(**dict(zip("xyz", [-0.1013, 0.0329, 0.9138]))),
+            orientation=Quaternion(
+                **dict(zip("xyzw", [0.2526, 0.4850, 0.7653, -0.3392]))
+            ),
         )
+
+        assert_pose_equal(ycbv_02.results[0].pose.pose, ycbv_02_pose)
+        assert_pose_equal(ycbv_15.results[0].pose.pose, ycbv_15_pose)