def __init__(self,
config_file,
viz=False,
segment_scene_function=None,
get_pose_function=None):
"""
A system that takes in 3 Drake PointClouds and ImageRgba8U from
RGBDCameras and determines the pose of an object in them. The user must
supply a segmentation function and pose alignment to determine the pose.
If these functions aren't supplied, the returned pose will always be the
4x4 identity matrix.
@param config_file str. A path to a .yml configuration file for the
cameras.
@param viz bool. If True, save the aligned and segmented point clouds
as serialized numpy arrays.
@param segment_scene_function A Python function that returns a subset of
the scene point cloud. See self.SegmentScene for more details.
@param get_pose_function A Python function that calculates a pose from a
segmented point cloud. See self.GetPose for more details.
@system{
@input_port{camera_left_rgb},
@input_port{camera_middle_rgb},
@input_port{camera_right_rgb},
@input_port{left_point_cloud},
@input_port{middle_point_cloud},
@input_port{right_point_cloud},
@output_port{X_WObject}
}
"""
LeafSystem.__init__(self)
# TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB
# fields.
self.left_rgb = self._DeclareAbstractInputPort(
"camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.middle_rgb = self._DeclareAbstractInputPort(
"camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.right_rgb = self._DeclareAbstractInputPort(
"camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.left_depth = self._DeclareAbstractInputPort(
"left_point_cloud", AbstractValue.Make(mut.PointCloud()))
self.middle_depth = self._DeclareAbstractInputPort(
"middle_point_cloud", AbstractValue.Make(mut.PointCloud()))
self.right_depth = self._DeclareAbstractInputPort(
"right_point_cloud", AbstractValue.Make(mut.PointCloud()))
self._DeclareAbstractOutputPort(
"X_WObject", lambda: AbstractValue.Make(Isometry3.Identity()),
self._DoCalcOutput)
self.segment_scene_function = segment_scene_function
self.get_pose_function = get_pose_function
self._LoadConfigFile(config_file)
self.viz = viz
def test_unimplemented_rendering(self):
"""The RenderEngine API throws exceptions for derived implementations
that don't override DoRender*Image (or Render*Image for the deprecated
API). This test confirms that behavior propagates down to Python."""
class MinimalEngine(mut.render.RenderEngine):
"""Minimal implementation of the RenderEngine virtual API"""
def UpdateViewpoint(self, X_WC):
pass
def DoRegisterVisual(self, id, shape, properties, X_WG):
pass
def DoUpdateVisualPose(self, id, X_WG):
pass
def DoRemoveGeometry(self, id):
pass
def DoClone(self):
pass
class ColorOnlyEngine(MinimalEngine):
"""Rendering Depth and Label images should throw"""
def DoRenderColorImage(self, camera, image_out):
pass
class DepthOnlyEngine(MinimalEngine):
"""Rendering Color and Label images should throw"""
def DoRenderDepthImage(self, camera, image_out):
pass
class LabelOnlyEngine(MinimalEngine):
"""Rendering Color and Depth images should throw"""
def DoRenderLabelImage(self, camera, image_out):
pass
identity = RigidTransform_[float]()
intrinsics = CameraInfo(10, 10, pi / 4)
core = mut.render.RenderCameraCore("n/a", intrinsics,
mut.render.ClippingRange(0.1, 10),
identity)
color_cam = mut.render.ColorRenderCamera(core, False)
depth_cam = mut.render.DepthRenderCamera(
core, mut.render.DepthRange(0.1, 9))
color_image = ImageRgba8U(intrinsics.width(), intrinsics.height())
depth_image = ImageDepth32F(intrinsics.width(), intrinsics.height())
label_image = ImageLabel16I(intrinsics.width(), intrinsics.height())
color_only = ColorOnlyEngine()
color_only.RenderColorImage(color_cam, color_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
color_only.RenderDepthImage(depth_cam, depth_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
color_only.RenderLabelImage(color_cam, label_image)
depth_only = DepthOnlyEngine()
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
depth_only.RenderColorImage(color_cam, color_image)
depth_only.RenderDepthImage(depth_cam, depth_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
depth_only.RenderLabelImage(color_cam, label_image)
label_only = LabelOnlyEngine()
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
label_only.RenderColorImage(color_cam, color_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
label_only.RenderDepthImage(depth_cam, depth_image)
label_only.RenderLabelImage(color_cam, label_image)
def test_render_engine_api(self):
class DummyRenderEngine(mut.render.RenderEngine):
"""Mirror of C++ DummyRenderEngine."""
# See comment below about `rgbd_sensor_test.cc`.
latest_instance = None
def __init__(self, render_label=None):
mut.render.RenderEngine.__init__(self)
# N.B. We do not hide these because this is a test class.
# Normally, you would want to hide this.
self.force_accept = False
self.registered_geometries = set()
self.updated_ids = {}
self.include_group_name = "in_test"
self.X_WC = RigidTransform_[float]()
self.color_count = 0
self.depth_count = 0
self.label_count = 0
self.color_camera = None
self.depth_camera = None
self.label_camera = None
def UpdateViewpoint(self, X_WC):
DummyRenderEngine.latest_instance = self
self.X_WC = X_WC
def ImplementGeometry(self, shape, user_data):
DummyRenderEngine.latest_instance = self
def DoRegisterVisual(self, id, shape, properties, X_WG):
DummyRenderEngine.latest_instance = self
mut.GetRenderLabelOrThrow(properties)
if self.force_accept or properties.HasGroup(
self.include_group_name):
self.registered_geometries.add(id)
return True
return False
def DoUpdateVisualPose(self, id, X_WG):
DummyRenderEngine.latest_instance = self
self.updated_ids[id] = X_WG
def DoRemoveGeometry(self, id):
DummyRenderEngine.latest_instance = self
self.registered_geometries.remove(id)
def DoClone(self):
DummyRenderEngine.latest_instance = self
new = DummyRenderEngine()
new.force_accept = copy.copy(self.force_accept)
new.registered_geometries = copy.copy(
self.registered_geometries)
new.updated_ids = copy.copy(self.updated_ids)
new.include_group_name = copy.copy(self.include_group_name)
new.X_WC = copy.copy(self.X_WC)
new.color_count = copy.copy(self.color_count)
new.depth_count = copy.copy(self.depth_count)
new.label_count = copy.copy(self.label_count)
new.color_camera = copy.copy(self.color_camera)
new.depth_camera = copy.copy(self.depth_camera)
new.label_camera = copy.copy(self.label_camera)
return new
def DoRenderColorImage(self, camera, color_image_out):
DummyRenderEngine.latest_instance = self
self.color_count += 1
self.color_camera = camera
def DoRenderDepthImage(self, camera, depth_image_out):
DummyRenderEngine.latest_instance = self
self.depth_count += 1
self.depth_camera = camera
def DoRenderLabelImage(self, camera, label_image_out):
DummyRenderEngine.latest_instance = self
self.label_count += 1
self.label_camera = camera
engine = DummyRenderEngine()
self.assertIsInstance(engine, mut.render.RenderEngine)
self.assertIsInstance(engine.Clone(), DummyRenderEngine)
# Test implementation of C++ interface by using RgbdSensor.
renderer_name = "renderer"
builder = DiagramBuilder()
scene_graph = builder.AddSystem(mut.SceneGraph())
# N.B. This passes ownership.
scene_graph.AddRenderer(renderer_name, engine)
sensor = builder.AddSystem(
RgbdSensor(parent_id=scene_graph.world_frame_id(),
X_PB=RigidTransform(),
depth_camera=mut.render.DepthRenderCamera(
mut.render.RenderCameraCore(
renderer_name, CameraInfo(640, 480, np.pi / 4),
mut.render.ClippingRange(0.1, 5.0),
RigidTransform()),
mut.render.DepthRange(0.1, 5.0))))
builder.Connect(
scene_graph.get_query_output_port(),
sensor.query_object_input_port(),
)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
sensor_context = sensor.GetMyContextFromRoot(diagram_context)
image = sensor.color_image_output_port().Eval(sensor_context)
# N.B. Because there's context cloning going on under the hood, we
# won't be interacting with our originally registered instance.
# See `rgbd_sensor_test.cc` as well.
current_engine = DummyRenderEngine.latest_instance
self.assertIsNot(current_engine, engine)
self.assertIsInstance(image, ImageRgba8U)
self.assertEqual(current_engine.color_count, 1)
image = sensor.depth_image_32F_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth32F)
self.assertEqual(current_engine.depth_count, 1)
image = sensor.depth_image_16U_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageDepth16U)
self.assertEqual(current_engine.depth_count, 2)
image = sensor.label_image_output_port().Eval(sensor_context)
self.assertIsInstance(image, ImageLabel16I)
self.assertEqual(current_engine.label_count, 1)
# Confirm that the CameraProperties APIs are *not* available.
with catch_drake_warnings(expected_count=2):
cam = mut.render.CameraProperties(10, 10, np.pi / 4, "")
depth_cam = mut.render.DepthCameraProperties(
10, 10, np.pi / 4, "", 0.1, 5)
with self.assertRaises(TypeError):
engine.RenderColorImage(cam, True,
ImageRgba8U(cam.width, cam.height))
with self.assertRaises(TypeError):
engine.RenderLabelImage(cam, True,
ImageLabel16I(cam.width, cam.height))
with self.assertRaises(TypeError):
engine.RenderDepthImage(
depth_cam, True,
ImageDepth32F(depth_cam.width, depth_cam.height))
def test_unimplemented_rendering(self):
# Test that a derived RenderEngine has implementations of
# DoRender*Image that throw something suggestive of "not implemented"
# and that they are overridable.
class MinimalEngine(mut.render.RenderEngine):
def UpdateViewpoint(self, X_WC):
pass
def DoRegisterVisual(self, id, shae, properties, X_WG):
pass
def DoUpdateVisualPose(self, id, X_WG):
pass
def DoRemoveGeometry(self, id):
pass
def DoClone(self):
pass
class ColorOnlyEngine(MinimalEngine):
def DoRenderColorImage(self, camera, image_out):
pass
class DepthOnlyEngine(MinimalEngine):
def DoRenderDepthImage(self, camera, image_out):
pass
class LabelOnlyEngine(MinimalEngine):
def DoRenderLabelImage(self, camera, image_out):
pass
identity = RigidTransform_[float]()
intrinsics = CameraInfo(10, 10, np.pi / 4)
core = mut.render.RenderCameraCore("n/a", intrinsics,
mut.render.ClippingRange(0.1, 10),
identity)
color_cam = mut.render.ColorRenderCamera(core, False)
depth_cam = mut.render.DepthRenderCamera(core,
mut.render.DepthRange(0.1, 9))
color_image = ImageRgba8U(intrinsics.width(), intrinsics.height())
depth_image = ImageDepth32F(intrinsics.width(), intrinsics.height())
label_image = ImageLabel16I(intrinsics.width(), intrinsics.height())
color_only = ColorOnlyEngine()
color_only.RenderColorImage(color_cam, color_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
color_only.RenderDepthImage(depth_cam, depth_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
color_only.RenderLabelImage(color_cam, label_image)
depth_only = DepthOnlyEngine()
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
depth_only.RenderColorImage(color_cam, color_image)
depth_only.RenderDepthImage(depth_cam, depth_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
depth_only.RenderLabelImage(color_cam, label_image)
label_only = LabelOnlyEngine()
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
label_only.RenderColorImage(color_cam, color_image)
with self.assertRaisesRegex(RuntimeError, ".+pure virtual function.+"):
label_only.RenderDepthImage(depth_cam, depth_image)
label_only.RenderLabelImage(color_cam, label_image)
