Apply disparity map scaling to rectified camera info

.devcontainer/Dockerfile-ROS1

@@ -29,5 +29,7 @@ RUN wget -O /tmp/ensenso.deb https://download.ensenso.com/s/ensensosdk/download?
 RUN sudo dpkg -i /tmp/ensenso.deb
 RUN sudo apt-get install -f -y
 
+RUN sudo pip3 install numpy==1.22.4 scipy==1.10.0
+
 USER $USERNAME
 CMD ["/bin/bash"]

.devcontainer/Dockerfile-ROS2

@@ -30,7 +30,7 @@ RUN sudo apt-get install -f -y
 # Install remaining dependencies
 RUN sudo apt-get -y install libopencv-dev python3-opencv
 RUN sudo apt-get -y install ros-$ROS_DISTRO-tf-transformations
-RUN sudo pip3 install transforms3d
+RUN sudo pip3 install numpy==1.22.4 scipy==1.10.0 transforms3d
 
 USER $USERNAME
 CMD ["/bin/bash"]

.github/scripts/install_external_dependencies.sh

@@ -9,6 +9,7 @@ sudo apt-get -y install dpkg wget
 wget -O /tmp/ensenso.deb https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-${ENSENSO_SDK_VERSION}-x64.deb
 sudo dpkg -i /tmp/ensenso.deb
 sudo apt-get install -f -y
+sudo pip3 install numpy==1.22.4 scipy==1.10.0
 
 if [[ $ROS_VERSION -eq "2" ]]; then
     sudo apt-get -y install libopencv-dev python3-opencv

ensenso_camera/src/ensenso_camera/ros2.py

@@ -35,7 +35,6 @@ def format_error(error, note=None):
 # ----------------------------------------------------------------------------------------------------------------------
 if is_ros2():
     import threading
-    import time
 
     import rclpy
 
@@ -139,6 +138,13 @@ def get_param(node, name, value=None):
     def create_publisher(node, msg_type, topic, queue_size=1):
         return node.create_publisher(msg_type, topic, queue_size)
 
+    def get_qos_profile(name):
+        if name == "point_cloud_dip":
+            return rclpy.qos.QoSProfile(
+                reliability=rclpy.qos.ReliabilityPolicy.SYSTEM_DEFAULT,
+                depth=5,
+            )
+
     def create_subscription(node, msg_type, topic, callback, qos_profile=10):
         return node.create_subscription(msg_type, topic, callback, qos_profile)
 
@@ -330,7 +336,10 @@ def get_param(_, name, default=rospy.client._unspecified):
     def create_publisher(_, msg_type, topic, queue_size=1):
         return rospy.Publisher(topic, msg_type, queue_size=queue_size)
 
-    def create_subscription(_, msg_type, topic, callback):
+    def get_qos_profile(_):
+        return None
+
+    def create_subscription(_, msg_type, topic, callback, __=None):
         return rospy.Subscriber(topic, msg_type, callback)
 
     def wait_for_server(node, client, timeout_sec=None, exit=True):

ensenso_camera/src/stereo_camera.cpp

@@ -451,9 +451,9 @@ void StereoCamera::onRequestData(ensenso::action::RequestDataGoalConstPtr const&
 
   if (requestDepthImage)
   {
-    // In case camera and target frame are different, the point cloud is recomputed with the relative(toWorld)-flag,
-    // such that the resulting point cloud (and thus the depth image) is transformed by the NxLib with the transform
-    // stored in the stereo camera's link node.
+    // In case camera and target frame are different, the previously computed point cloud has been automatically
+    // transformed to world coordinates by the NxLib. Recompute it with the relative (to camera) flag so that the depth
+    // image is in camera coordinates.
     if (params.cameraFrame != params.targetFrame)
     {
       NxLibCommand computePointMap(cmdComputePointMap, params.serial);
@@ -1444,11 +1444,15 @@ void StereoCamera::fillCameraInfoFromNxLib(sensor_msgs::msg::CameraInfoPtr const
     // removed), except for the stereo camera matrix.
     GET_D_MATRIX(info).resize(5, 0);
 
+    // Scaling the disparity map also scales the rectified images. The range is 0.1 to 1. The factor has to be applied
+    // to the projection/camera matrix P for correct 3D data.
+    auto scalingFactor = cameraNode[itmParameters][itmDisparityMap][itmScaling].asDouble();
+
     for (int row = 0; row < 3; row++)
     {
       for (int column = 0; column < 3; column++)
       {
-        GET_P_MATRIX(info)[4 * row + column] = stereoCalibrationNode[itmCamera][column][row].asDouble();
+        GET_P_MATRIX(info)[4 * row + column] = stereoCalibrationNode[itmCamera][column][row].asDouble() * scalingFactor;
 
         if (row == column)
         {

ensenso_camera2/ensenso_camera/ros2_launch.py

@@ -68,22 +68,23 @@ def get_launch_include(package_name, launch_file_name, launch_arguments={}):
     )
 
 
-class LaunchFileInclude:
+class EnsensoInclude:
     def __init__(self, name, args={}):
         self.name = name
         self.args = args
 
+    def get_description(self):
+        raise NotImplementedError
+
+
+class LaunchFileInclude(EnsensoInclude):
     def get_description(self):
         return get_launch_include("ensenso_camera", self.name, self.args)
 
     def get_launch_description(self):
         return LaunchDescription([self.get_description()])
 
 
-class ScriptInclude:
-    def __init__(self, name, args={}):
-        self.name = name
-        self.args = args
-
+class ScriptInclude(EnsensoInclude):
     def get_description(self):
         return get_dut_process("ensenso_camera", self.name, self.args)

ensenso_camera_test/CMakeLists.txt

@@ -19,6 +19,9 @@ catkin_package(
   tf
 )
 
+# pep8 is wrong, E203 is not PEP 8 compliant.
+set(ROSLINT_PYTHON_OPTS "--ignore=E203")
+
 roslint_python()
 
 if(CATKIN_ENABLE_TESTING)
@@ -32,6 +35,7 @@ if(CATKIN_ENABLE_TESTING)
   add_rostest(test/project_pattern.test)
   add_rostest(test/request_data_mono.test)
   add_rostest(test/request_data_stereo.test)
+  add_rostest(test/scale_disparity_map.test)
   add_rostest(test/telecentric_projection.test)
   add_rostest(test/texture_point_cloud.test)
   add_rostest(test/workspace_calibration.test)

ensenso_camera_test/package.xml

@@ -16,6 +16,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roslint</build_depend>
+  <depend>depth_image_proc</depend>
   <depend>ensenso_camera</depend>
   <depend>ensenso_camera_msgs</depend>
   <depend>rospy</depend>

ensenso_camera_test/src/ensenso_camera_test/ros2_testing.py

@@ -32,6 +32,16 @@ def create_tf_broadcaster(node):
 
         return TransformBroadcaster(node)
 
+    def create_tf_buffer():
+        from tf2_ros.buffer import Buffer
+
+        return Buffer()
+
+    def create_tf_listener(buffer, node):
+        from tf2_ros.transform_listener import TransformListener
+
+        return TransformListener(buffer, node)
+
     def create_tf_transform(pose, timestamp, child_frame, parent_frame):
         from geometry_msgs.msg import TransformStamped
 
@@ -90,6 +100,16 @@ def create_tf_broadcaster(_):
 
         return TransformBroadcaster()
 
+    def create_tf_buffer():
+        from tf2_ros.buffer import Buffer
+
+        return Buffer()
+
+    def create_tf_listener(buffer, _):
+        from tf2_ros.transform_listener import TransformListener
+
+        return TransformListener(buffer)
+
     def send_tf_transform(broadcaster, pose, timestamp, child_frame, parent_frame):
         broadcaster.sendTransform(pose.position, pose.orientation, timestamp, child_frame, parent_frame)
 

ensenso_camera_test/src/ensenso_camera_test/scale_disparity_map.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python
+import numpy as np
+import unittest
+
+from scipy.spatial.transform import Rotation
+
+import ensenso_camera.ros2 as ros2py
+
+from ensenso_camera_msgs.msg import Parameter
+
+import ensenso_camera_test.ros2_testing as ros2py_testing
+
+from sensor_msgs.msg import PointCloud2, PointField
+
+
+pc2 = ros2py_testing.import_point_cloud2()
+
+RequestData = ros2py.import_action("ensenso_camera_msgs", "RequestData")
+SetParameter = ros2py.import_action("ensenso_camera_msgs", "SetParameter")
+
+
+# In ROS2 the depth_image_proc/point_cloud_xyz node has a fixed QOS profile and the message is not received
+# after a single request, hence we request the data until we received a point cloud from the node.
+MAX_TRIES = 10
+
+# The corresponding test launch file opens a camera that is linked to "Workspace".
+TARGET_FRAME = "Workspace"
+
+
+class TestScaleDisparityMap(unittest.TestCase):
+    def setUp(self):
+        self.node = ros2py.create_node("test_scale_disparity_map")
+
+        self.tf_buffer = ros2py_testing.create_tf_buffer()
+        self.tf_listener = ros2py_testing.create_tf_listener(self.tf_buffer, self.node)
+
+        self.request_data_client = ros2py.create_action_client(self.node, "request_data", RequestData)
+        self.set_parameter_client = ros2py.create_action_client(self.node, "set_parameter", SetParameter)
+
+        clients = [self.request_data_client, self.set_parameter_client]
+        success = ros2py.wait_for_servers(self.node, clients, timeout_sec=ros2py_testing.TEST_TIMEOUT, exit=False)
+        self.assertTrue(success, msg="Timeout reached for servers.")
+
+        self.point_cloud_ens = None
+        self.point_cloud_dip = None
+
+        # The Ensenso point cloud and the reference point cloud calculated by depth_image_proc/point_cloud_xyz
+        self.pc_subscriber1 = ros2py.create_subscription(self.node, PointCloud2, "point_cloud", self.callback_ens)
+        self.pc_subscriber2 = ros2py.create_subscription(
+            self.node, PointCloud2, "point_cloud_dip", self.callback_dip, ros2py.get_qos_profile("point_cloud_dip")
+        )
+
+    def request_data(self):
+        request_data_goal = RequestData.Goal()
+        request_data_goal.request_rectified_images = True
+        request_data_goal.request_disparity_map = True
+        request_data_goal.request_depth_image = True
+        request_data_goal.request_point_cloud = True
+        request_data_goal.publish_results = True
+
+        response = ros2py.send_action_goal(self.node, self.request_data_client, request_data_goal)
+        result = response.get_result()
+
+        self.assertFalse(response.timeout(), msg="Requesting data action timed out.")
+        self.assertTrue(response.successful(), msg="Requesting data action not successful.")
+        self.assertEqual(result.error.code, 0, msg="Requesting data not successful.")
+
+    def callback_ens(self, msg):
+        self.point_cloud_ens = msg
+
+    def callback_dip(self, msg):
+        self.point_cloud_dip = msg
+
+    def transform_point_cloud_to_target_frame(self, point_cloud, target_frame):
+        try:
+            tf = self.tf_buffer.lookup_transform(
+                target_frame,
+                point_cloud.header.frame_id,
+                point_cloud.header.stamp,
+            )
+        except Exception as e:
+            self.node.get_logger().error(e)
+
+        # See https://robotics.stackexchange.com/questions/109924/
+        t = np.eye(4)
+        q = tf.transform.rotation
+        x = tf.transform.translation
+        t[:3, :3] = Rotation.from_quat([q.x, q.y, q.z, q.w]).as_matrix()
+        t[:3, 3] = [x.x, x.y, x.z]
+
+        points_msg = list(pc2.read_points(point_cloud))
+        points_map = np.ones((len(points_msg), 4))
+        points_map[:, :3] = points_msg
+        points_map = np.dot(t, points_map.T).T
+
+        fields = [
+            PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1),
+        ]
+
+        pcl_map_header = point_cloud.header
+        pcl_map_header.frame_id = "map"
+        return pc2.create_cloud(pcl_map_header, fields, points_map[:, :3])
+
+    def set_parameters(self, parameters=[]):
+        goal = SetParameter.Goal(parameters=parameters)
+        response = ros2py.send_action_goal(self.node, self.set_parameter_client, goal)
+
+        result = response.get_result()
+        self.assertTrue(response.successful())
+        self.assertEqual(result.error.code, 0)
+
+    def test_scale_disparity_map(self):
+        # Enable disparity map scaling.
+        self.set_parameters([Parameter(key=Parameter.SCALING, float_value=0.5)])
+
+        # Wait until we received a reference point cloud.
+        tries = 0
+        while self.point_cloud_dip is None and tries < MAX_TRIES:
+            self.request_data()
+            ros2py.sleep(self.node, 2)
+            tries += 1
+
+        self.assertIsNotNone(self.point_cloud_ens, msg="Received no Ensenso point cloud")
+        self.assertIsNotNone(self.point_cloud_dip, msg="Received no reference point cloud")
+
+        frame = self.point_cloud_ens.header.frame_id
+        self.assertEqual(
+            frame,
+            TARGET_FRAME,
+            msg=f"Expected Ensenso point cloud in frame '{TARGET_FRAME}', got '{frame}'!",
+        )
+
+        # Since cmdComputeDisparityMap returns the point cloud in the camera's linked coordinate system, the reference
+        # point cloud has to be transformed to that coordinate system as well.
+        self.point_cloud_dip = self.transform_point_cloud_to_target_frame(self.point_cloud_dip, TARGET_FRAME)
+
+        points_ens = list(pc2.read_points(self.point_cloud_ens, skip_nans=True))
+        points_dip = list(pc2.read_points(self.point_cloud_dip, skip_nans=True))
+        self.assertGreater(len(points_ens), 0, msg="The Ensenso point cloud is empty")
+        self.assertGreater(len(points_dip), 0, msg="The reference point cloud is empty")
+
+        def average_values(points, axis=0):
+            return np.average(np.array(points)[:, axis : axis + 1]) * 1000.0
+
+        self.assertAlmostEqual(
+            average_values(points_ens, axis=0),
+            average_values(points_dip, axis=0),
+            places=3,
+            msg="x values differ",
+        )
+
+        self.assertAlmostEqual(
+            average_values(points_ens, axis=1),
+            average_values(points_dip, axis=1),
+            places=3,
+            msg="y values differ",
+        )
+
+        self.assertAlmostEqual(
+            average_values(points_ens, axis=2),
+            average_values(points_dip, axis=2),
+            places=3,
+            msg="z values differ",
+        )
+
+
+def main():
+    ros2py_testing.run_ros1_test("test_scale_disparity_map", TestScaleDisparityMap)
+
+
+if __name__ == "__main__":
+    main()

ensenso_camera_test/test/scale_disparity_map.test

@@ -0,0 +1,23 @@
+<launch>
+  <arg name="serial" value="scale_dmap_cam" />
+  <arg name="path" value="$(find ensenso_camera_test)/data/colorized_point_cloud/stereo.zip" />
+
+  <node pkg="nodelet" type="nodelet" name="manager_" args="manager" output="screen" />
+
+  <node pkg="nodelet" type="nodelet" name="Ensenso_$(arg serial)"
+    args="load ensenso_camera/stereo_camera_node /manager_" output="screen">
+    <param name="serial" type="string" value="$(arg serial)" />
+    <param name="file_camera_path" type="string" value="$(arg path)" />
+    <param name="link_frame" type="string" value="Workspace" />
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="dip_pc2"
+    args="load depth_image_proc/point_cloud_xyz /manager_" output="screen">
+    <remap from="/camera_info" to="/depth/camera_info" />
+    <remap from="/image_rect" to="/depth/image" />
+    <remap from="/points" to="/point_cloud_dip" />
+  </node>
+
+  <test pkg="ensenso_camera_test" type="scale_disparity_map.py" test-name="scale_disparity_map_test">
+  </test>
+</launch>