Add symmetries publisher and publish empty messages on no detections

README.md

@@ -61,6 +61,10 @@ The node provides the following ROS topics:
 
     Information about the used pose estimator (currently only CosyPose is supported) and URL with object database location.
 
+- **happypose/object_symmetries** [happypose_msgs/msg/ObjectSymmetriesArray] (*QOS: TRANSIENT_LOCAL*)
+
+    Discrete and continuous symmetries of objects in the dataset.
+
 - **happypose/markers** [visualization_msgs/msg/MarkerArray]
 
     Array of markers used to visualize detections with their meshes in software like RViz 2.

happypose_examples/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5.0)
+cmake_minimum_required(VERSION 3.10)
 project(happypose_examples)
 
 find_package(ament_cmake REQUIRED)

happypose_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>happypose_examples</name>
-  <version>0.0.0</version>
+  <version>0.0.2</version>
   <description>Examples for happypose_ros package</description>
   <author email="[email protected]">Krzysztof Wojciechowski</author>
   <maintainer email="[email protected]">Guilhem Saurel</maintainer>

happypose_msgs/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.10)
+project(happypose_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+find_package(rclpy REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(
+  ${PROJECT_NAME}
+  msg/ContinuousSymmetry.msg
+  msg/ObjectSymmetries.msg
+  msg/ObjectSymmetriesArray.msg
+  DEPENDENCIES
+  builtin_interfaces
+  std_msgs
+  geometry_msgs)
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME}_py)
+install(FILES package.xml DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    test/test_discretize_symmetries.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      TIMEOUT 60
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

happypose_msgs/happypose_msgs_py/symmetries.py

@@ -0,0 +1,97 @@
+import numpy as np
+import numpy.typing as npt
+import transforms3d
+from typing import List, Union
+
+from geometry_msgs.msg import Transform, Vector3, Quaternion
+
+from happypose_msgs.msg import ContinuousSymmetry, ObjectSymmetries
+
+
+def discretize_symmetries(
+    object_symmetries: ObjectSymmetries,
+    n_symmetries_continuous: int = 8,
+    return_ros_msg: bool = False,
+) -> Union[npt.NDArray[np.float64], List[Transform]]:
+    """Converts discrete and continuous symmetries to a list of discrete symmetries.
+
+    :param object_symmetries: ROS message containing symmetries of a given object.
+    :type object_symmetries: happypose_msgs.msg.ObjectSymmetries
+    :param n_symmetries_continuous: Number of segments to discretize continuous symmetries.
+    :type n_symmetries_continuous: int
+    :param return_ros_msg: Whether to return ROS message or numpy array
+        with 4x4 matrices, defaults to False.
+    :type return_ros_msg: bool, optional
+    :return: If ``return_ros_msg`` is False returns array of a shape (n, 4, 4) with ``n``
+        SE3 transformation matrices representing symmetries.
+        Otherwise list of ROS Transform messages.
+    :rtype: Union[npt.NDArray[np.float64], List[geometry_msgs.msg.Transform]]
+    """
+
+    # If there are not continuous symmetries and ROS message is expected skip computations
+    if return_ros_msg and len(object_symmetries.symmetries_continuous) == 0:
+        return object_symmetries.symmetries_discrete
+
+    def _discretize_continuous(
+        sym: ContinuousSymmetry, idx: int
+    ) -> npt.NDArray[np.float64]:
+        axis = np.array([sym.axis.x, sym.axis.y, sym.axis.z])
+        if not np.isclose(axis.sum(), 1.0):
+            raise ValueError(
+                f"Continuous symmetry at index {idx} has non unitary rotation axis!"
+            )
+        symmetries = np.zeros((n_symmetries_continuous, 4, 4))
+
+        # Precompute steps of rotations
+        rot_base = 2.0 * axis * np.pi / n_symmetries_continuous
+        for i in range(n_symmetries_continuous):
+            symmetries[i, :3, :3] = transforms3d.euler.euler2mat(*(rot_base * i))
+
+        symmetries[:, -1, -1] = 1.0
+        symmetries[:, :3, -1] = np.array([sym.offset.x, sym.offset.y, sym.offset.z])
+
+        return symmetries
+
+    symmetries_continuous = np.array(
+        [
+            _discretize_continuous(sym_c, idx)
+            for idx, sym_c in enumerate(object_symmetries.symmetries_continuous)
+        ]
+    ).reshape((-1, 4, 4))
+
+    def _transform_msg_to_mat(sym: Transform) -> npt.NDArray[np.float64]:
+        M = np.eye(4)
+        M[:3, :3] = transforms3d.quaternions.quat2mat(
+            (sym.rotation.w, sym.rotation.x, sym.rotation.y, sym.rotation.z)
+        )
+        M[0, -1] = sym.translation.x
+        M[1, -1] = sym.translation.y
+        M[2, -1] = sym.translation.z
+        return M
+
+    symmetries_discrete = np.array(
+        [
+            _transform_msg_to_mat(sym_d)
+            for sym_d in object_symmetries.symmetries_discrete
+        ]
+    ).reshape((-1, 4, 4))
+
+    symmetries_mixed = [
+        (symmetries_discrete[i] @ symmetries_continuous).reshape((-1, 4, 4))
+        for i in range(len(object_symmetries.symmetries_discrete))
+    ]
+
+    symmetries = np.vstack(
+        [symmetries_continuous, symmetries_discrete, *symmetries_mixed]
+    )
+    if not return_ros_msg:
+        return symmetries
+
+    def _mat_to_msg(M: npt.NDArray[np.float64]):
+        q = transforms3d.quaternions.mat2quat(M[:3, :3])
+        return Transform(
+            translation=Vector3(**dict(zip("xyz", M[:, -1]))),
+            rotation=Quaternion(**dict(zip("wxyz", q))),
+        )
+
+    return [_mat_to_msg(M) for M in symmetries]

happypose_msgs/msg/ContinuousSymmetry.msg

@@ -0,0 +1,6 @@
+# Definition of continuous symmetry.
+# Consists of rotation axis and offset.
+# Symilarly to HappyPose object ContinuousSymmetry
+
+geometry_msgs/Vector3 offset
+geometry_msgs/Vector3 axis

happypose_msgs/msg/ObjectSymmetries.msg

@@ -0,0 +1,10 @@
+# Class id for which symmetries are considered
+string class_id
+# Lists discrete and continuous symmetries of considered object.
+# If no symmetries of a given type, list is left empty.
+
+# In HappyPose discrete symmetries are represented as
+# transformation matrices. Those matrices are directyl
+# converted to the Transform message.
+geometry_msgs/Transform[] symmetries_discrete
+ContinuousSymmetry[] symmetries_continuous

happypose_msgs/msg/ObjectSymmetriesArray.msg

@@ -0,0 +1,5 @@
+# Time stamp at which the message was sent
+std_msgs/Header header
+
+# List of objects detected be HappyPose node
+ObjectSymmetries[] objects

happypose_msgs/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>happypose_msgs</name>
+  <version>0.0.2</version>
+  <description>Custom messages used by happypose_ros package.</description>
+  <author email="[email protected]">Krzysztof Wojciechowski</author>
+  <maintainer email="[email protected]">Guilhem Saurel</maintainer>
+  <license>BSD</license>
+
+  <url type="website">https://gitlab.laas.fr/kwojciecho/happypose_ros</url>
+  <url type="bugtracker">https://gitlab.laas.fr/kwojciecho/happypose_ros/-/issues</url>
+  <url type="repository">https://gitlab.laas.fr/kwojciecho/happypose_ros</url>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <build_depend>builtin_interfaces</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-transforms3d</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>pinocchio</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

happypose_msgs/test/test_discretize_symmetries.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+import numpy as np
+import pinocchio as pin
+from typing import List
+
+from geometry_msgs.msg import Transform, Vector3, Quaternion
+
+from happypose_msgs_py.symmetries import discretize_symmetries
+
+from happypose_msgs.msg import ObjectSymmetries
+
+
+def are_transforms_close(t1: Transform, t2: Transform) -> bool:
+    """Checks if two ROS transform messages are close.
+
+    :param t1: First transform to compare.
+    :type t1: geometry_msgs.msg.Transform
+    :param t2: Second transform to compare.
+    :type t2: geometry_msgs.msg.Transform
+    :return: If those transformations are close.
+    :rtype: bool
+    """
+    T1, T2 = [
+        pin.XYZQUATToSE3(
+            [
+                t.translation.x,
+                t.translation.y,
+                t.translation.z,
+                t.rotation.x,
+                t.rotation.y,
+                t.rotation.z,
+                t.rotation.w,
+            ]
+        )
+        for t in (t1, t2)
+    ]
+    diff = T1.inverse() * T2
+    return np.linalg.norm(pin.log6(diff).vector) < 1e-6
+
+
+def is_transform_in_list(t1: Transform, t_list: List[Transform]) -> bool:
+    """Checks if a transform is in the list of transformations.
+
+    :param t1: Transform to check if in the list.
+    :type t1: Transform
+    :param t_list: List of transforms to check if ``t1`` exists in there.
+    :type t_list: List[Transform]
+    :return: If the transform in the list.
+    :rtype: bool
+    """
+    return any(are_transforms_close(t1, t2) for t2 in t_list)
+
+
+def test_empty_message_np() -> None:
+    msg = ObjectSymmetries(
+        symmetries_discrete=[],
+        symmetries_continuous=[],
+    )
+
+    res = discretize_symmetries(msg)
+    assert isinstance(res, np.ndarray), "Result is not an instance of Numpy array!"
+    assert res.shape == (0, 4, 4), "Result shape is incorrect!"
+
+
+def test_empty_message_ros() -> None:
+    msg = ObjectSymmetries(
+        symmetries_discrete=[],
+        symmetries_continuous=[],
+    )
+
+    res = discretize_symmetries(msg, return_ros_msg=True)
+    assert isinstance(res, list), "Result is not an instance of a list!"
+    assert len(res) == 0, "Results list is not empty!"
+
+
+def test_only_discrete_ros() -> None:
+    msg = ObjectSymmetries(
+        symmetries_discrete=[
+            Transform(
+                translation=Vector3(x=0.0, y=0.0, z=0.0),
+                rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            ),
+            Transform(
+                translation=Vector3(x=0.1, y=0.1, z=0.1),
+                rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
+            ),
+        ],
+        symmetries_continuous=[],
+    )
+
+    res = discretize_symmetries(msg, return_ros_msg=True)
+
+    assert len(res) == len(
+        msg.symmetries_discrete
+    ), "Results list does not have all discrete symmetries from message received!"
+
+    assert all(
+        isinstance(r, Transform) for r in res
+    ), "Returned type of elements in the list is not geometry_msgs.msg.Transform!"
+
+    assert all(
+        is_transform_in_list(t, msg.symmetries_discrete) for t in res
+    ), "Resulted discrete symmetries are not close to the initial ones int the message!"

happypose_ros/.gitignore

@@ -20,3 +20,5 @@ AMENT_IGNORE
 # The file has to be manually modified as the code generations has a bug in __map
 happypose_ros/happypose_ros_parameters.py
 happypose_ros/test/__pycache__
+happypose_msgs/test/__pycache__
+happypose_msgs_py/test/__pycache__