def test_point_cloud_api(self):
self.assertEqual(mut.PointCloud.T, np.float32)
self.assertEqual(mut.PointCloud.D, np.float32)
self.assertTrue(
mut.PointCloud.IsDefaultValue(value=mut.PointCloud.kDefaultValue))
self.assertTrue(
mut.PointCloud.IsInvalidValue(value=mut.PointCloud.kDefaultValue))
fields = mut.Fields(mut.BaseField.kXYZs)
pc = mut.PointCloud(new_size=0, fields=fields)
self.assertEqual(pc.fields(), fields)
self.assertEqual(pc.size(), 0)
pc.resize(new_size=2)
self.assertEqual(pc.size(), 2)
self.assertTrue(pc.has_xyzs())
self.assertEqual(pc.xyzs().shape, (3, 2))
# Test reference semantics with NumPy + Pybind11.
test_xyzs = [[1., 2., 3.], [4., 5., 6.]]
pc.mutable_xyzs().T[:] = test_xyzs
np.testing.assert_equal(pc.xyzs().T, test_xyzs)
test_xyz = [10., 20., 30.]
pc.mutable_xyz(i=0)[:] = test_xyz
np.testing.assert_equal(pc.xyz(i=0), test_xyz)
np.testing.assert_equal(pc.xyzs().T[0], test_xyz)
pc_new = mut.PointCloud()
pc_new.SetFrom(other=pc)
np.testing.assert_equal(pc.xyzs(), pc_new.xyzs())
# Test Systems' value registration.
self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
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 __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD,
name="point_cloud", X_WP=Isometry3.Identity(),
default_rgb=[255., 255., 255.]):
"""
Args:
meshcat_viz: Either a native meshcat.Visualizer or a pydrake
MeshcatVisualizer object.
draw_period: The rate at which this class publishes to the
visualizer.
name: The string name of the meshcat object.
X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``.
Default is identity.
default_rgb: RGB value for published points if the PointCloud does
not provide RGB values.
"""
LeafSystem.__init__(self)
self._meshcat_viz = _get_native_visualizer(meshcat_viz)
self._X_WP = X_WP
self._default_rgb = np.array(default_rgb)
self._name = name
self.set_name('meshcat_point_cloud_visualizer_' + name)
self.DeclarePeriodicPublish(draw_period, 0.0)
self.DeclareAbstractInputPort("point_cloud_P",
AbstractValue.Make(mut.PointCloud()))
def test_point_cloud_api(self):
self.assertEqual(mut.PointCloud.T, np.float32)
self.assertEqual(mut.PointCloud.C, np.uint8)
self.assertEqual(mut.PointCloud.D, np.float32)
self.assertTrue(
mut.PointCloud.IsDefaultValue(value=mut.PointCloud.kDefaultValue))
self.assertTrue(
mut.PointCloud.IsInvalidValue(value=mut.PointCloud.kDefaultValue))
fields = mut.Fields(mut.BaseField.kXYZs)
pc = mut.PointCloud(new_size=0, fields=fields)
self.assertEqual(pc.fields(), fields)
self.assertEqual(pc.size(), 0)
pc.resize(new_size=2)
self.assertEqual(pc.size(), 2)
self.assertTrue(pc.has_xyzs())
self.assertEqual(pc.xyzs().shape, (3, 2))
# Test reference semantics with NumPy + Pybind11.
test_xyzs = [[1., 2., 3.], [4., 5., 6.]]
pc.mutable_xyzs().T[:] = test_xyzs
np.testing.assert_equal(pc.xyzs().T, test_xyzs)
test_xyz = [10., 20., 30.]
pc.mutable_xyz(i=0)[:] = test_xyz
np.testing.assert_equal(pc.xyz(i=0), test_xyz)
np.testing.assert_equal(pc.xyzs().T[0], test_xyz)
pc_new = mut.PointCloud()
pc_new.SetFrom(other=pc)
np.testing.assert_equal(pc.xyzs(), pc_new.xyzs())
# - Additional types are tested via simple existence checks.
all_fields = mut.Fields(mut.BaseField.kXYZs | mut.BaseField.kNormals
| mut.BaseField.kRGBs)
count = 1
pc = mut.PointCloud(new_size=count, fields=all_fields)
self.check_array(pc.mutable_xyzs(), np.float32, (3, count))
self.check_array(pc.mutable_normals(), np.float32, (3, count))
pc.normals()
pc.mutable_normal(i=0)
pc.normal(i=0)
self.check_array(pc.mutable_rgbs(), np.uint8, (3, count))
pc.rgbs()
pc.mutable_rgb(i=0)
pc.rgb(i=0)
# - Check for none.
with self.assertRaises(RuntimeError) as ex:
mut.PointCloud(new_size=0, fields=mut.Fields(mut.BaseField.kNone))
# Test Systems' value registration.
self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
def test_point_cloud_visualization(self):
"""A small point cloud"""
draw_period = 1 / 30.
sim_time = draw_period * 3.
def se3_from_xyz(xyz):
return Isometry3(np.eye(3), xyz)
def show_cloud(pc, pc2=None, use_native=False, **kwargs):
# kwargs go to ctor.
builder = DiagramBuilder()
# Add point cloud visualization.
if use_native:
viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
else:
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
pc_viz = builder.AddSystem(
MeshcatPointCloudVisualizer(viz, **kwargs))
if pc2:
pc_viz2 = builder.AddSystem(
MeshcatPointCloudVisualizer(viz,
name='second_point_cloud',
X_WP=se3_from_xyz([0, 0.3, 0]),
default_rgb=[0., 255., 0.]))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
# TODO(eric.cousineau): Replace `AbstractValue.Make(pc)` with just
# `pc` (#12086).
pc_viz.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc))
if pc2:
context = diagram.GetMutableSubsystemContext(
pc_viz2, diagram_context)
pc_viz2.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc2))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(sim_time)
# Generate some random points that are visually noticeable.
# ~300000 makes the visualizer less responsive on my (Eric) machine.
count = 100000
xyzs = np.random.uniform(-0.1, 0.1, (3, count))
rgbs = np.random.uniform(0., 255.0, (3, count))
pc = mut.PointCloud(
count, mut.Fields(mut.BaseField.kXYZs | mut.BaseField.kRGBs))
pc.mutable_xyzs()[:] = xyzs
pc.mutable_rgbs()[:] = rgbs
pc_no_rgbs = mut.PointCloud(count, mut.Fields(mut.BaseField.kXYZs))
pc_no_rgbs.mutable_xyzs()[:] = xyzs
show_cloud(pc)
# Exercise arguments.
show_cloud(pc,
use_native=True,
name="point_cloud_2",
X_WP=se3_from_xyz([0.3, 0, 0]))
show_cloud(pc, name="point_cloud_3", X_WP=se3_from_xyz([0.6, 0, 0]))
show_cloud(pc_no_rgbs,
name="point_cloud_4",
X_WP=se3_from_xyz([0.9, 0, 0]))
show_cloud(pc_no_rgbs,
name="point_cloud_5",
X_WP=se3_from_xyz([1.2, 0, 0]),
default_rgb=[255., 0., 0.])
show_cloud(pc,
pc_no_rgbs,
name="point_cloud_6",
X_WP=se3_from_xyz([1.5, 0, 0]),
default_rgb=[0., 255., 0.])
