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)