Skip to content

Apply disparity map scaling to rectified camera info #134

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 6 commits into from
Aug 12, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .devcontainer/Dockerfile-ROS1
Original file line number Diff line number Diff line change
@@ -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"]
2 changes: 1 addition & 1 deletion .devcontainer/Dockerfile-ROS2
Original file line number Diff line number Diff line change
@@ -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"]
1 change: 1 addition & 0 deletions .github/scripts/install_external_dependencies.sh
Original file line number Diff line number Diff line change
@@ -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
13 changes: 11 additions & 2 deletions ensenso_camera/src/ensenso_camera/ros2.py
Original file line number Diff line number Diff line change
@@ -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):
12 changes: 8 additions & 4 deletions ensenso_camera/src/stereo_camera.cpp
Original file line number Diff line number Diff line change
@@ -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)
{
13 changes: 7 additions & 6 deletions ensenso_camera2/ensenso_camera/ros2_launch.py
Original file line number Diff line number Diff line change
@@ -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)
4 changes: 4 additions & 0 deletions ensenso_camera_test/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)
1 change: 1 addition & 0 deletions ensenso_camera_test/package.xml
Original file line number Diff line number Diff line change
@@ -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>
20 changes: 20 additions & 0 deletions ensenso_camera_test/src/ensenso_camera_test/ros2_testing.py
Original file line number Diff line number Diff line change
@@ -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)

174 changes: 174 additions & 0 deletions ensenso_camera_test/src/ensenso_camera_test/scale_disparity_map.py
Original file line number Diff line number Diff line change
@@ -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()
23 changes: 23 additions & 0 deletions ensenso_camera_test/test/scale_disparity_map.test
Original file line number Diff line number Diff line change
@@ -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>
13 changes: 12 additions & 1 deletion ensenso_camera_test2/ensenso_camera_test/ros2_testing_launch.py
Original file line number Diff line number Diff line change
@@ -11,6 +11,7 @@

from launch_testing.actions import ReadyToTest

from ensenso_camera.ros2_launch import EnsensoInclude
from ensenso_camera.ros2_launch import LaunchFileInclude as _EnsensoLaunchInclude
from ensenso_camera.ros2_launch import ScriptInclude as _EnsensoScriptInclude

@@ -27,6 +28,16 @@ def get_data_path(filename):


def generate_test_description(*includes):
descriptions = [include.get_description() for include in includes]
descriptions = []

for include in includes:
if isinstance(include, EnsensoInclude):
description = include.get_description()
else:
# e.g. normal Node objects
description = include
descriptions.append(description)

descriptions.append(TimerAction(period=LAUNCH_DELAY_PERIOD_IN_S, actions=[ReadyToTest()]))

return LaunchDescription(descriptions)
1 change: 1 addition & 0 deletions ensenso_camera_test2/ignore_ros2_package.xml
Original file line number Diff line number Diff line change
@@ -16,6 +16,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>

<depend>depth_image_proc</depend>
<depend>ensenso_camera</depend>
<depend>ensenso_camera_msgs</depend>
<depend>launch_testing_ament_cmake</depend>
31 changes: 31 additions & 0 deletions ensenso_camera_test2/test/scale_disparity_map.test.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,31 @@
import pytest

import ensenso_camera_test.ros2_testing_launch as launch

from ensenso_camera_test.scale_disparity_map import TestScaleDisparityMap

from launch_ros.actions import Node


@pytest.mark.launch_test
def generate_test_description():
return launch.generate_test_description(
launch.EnsensoLaunchInclude(
name="stereo_node.launch.py",
args={
"serial": "scale_dmap_cam",
"file_camera_path": launch.get_data_path("colorized_point_cloud/stereo.zip"),
"link_frame": "Workspace",
},
),
Node(
package="depth_image_proc",
executable="point_cloud_xyz_node",
name="dip_point_cloud",
remappings=[
("/camera_info", "/depth/camera_info"),
("/image_rect", "/depth/image"),
("/points", "/point_cloud_dip"),
],
),
)