diff --git a/happypose_ros/happypose_ros/camera_wrapper.py b/happypose_ros/happypose_ros/camera_wrapper.py
index 89934b1..3fa8c40 100644
--- a/happypose_ros/happypose_ros/camera_wrapper.py
+++ b/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,20 +137,16 @@ 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.
@@ -174,9 +154,9 @@ def ready(self) -> bool:
: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,7 +197,7 @@ 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.
@@ -225,4 +205,5 @@ def get_last_k_matrix(self) -> npt.NDArray[np.float64]:
: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())
diff --git a/happypose_ros/package.xml b/happypose_ros/package.xml
index 0d69a3a..fac23f6 100644
--- a/happypose_ros/package.xml
+++ b/happypose_ros/package.xml
@@ -22,6 +22,7 @@
tf2_ros
geometry_msgs
happypose_msgs
+ image_geometry
sensor_msgs
std_msgs
vision_msgs
diff --git a/happypose_ros/test/happypose_testing_utils.py b/happypose_ros/test/happypose_testing_utils.py
index 0938eb5..d5e1a68 100644
--- a/happypose_ros/test/happypose_testing_utils.py
+++ b/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)
diff --git a/happypose_ros/test/single_view_base.py b/happypose_ros/test/single_view_base.py
index 5049b95..23d3a4f 100644
--- a/happypose_ros/test/single_view_base.py
+++ b/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)