def test_pc_flags_api(self):
# BaseField
enum = mut.BaseField
enum.kNone
enum.kXYZs
# Fields
fields = mut.Fields(base_fields=enum.kXYZs)
self.assertEqual(fields.base_fields(), enum.kXYZs)
self.assertTrue(fields.has_base_fields())
self.assertEqual(fields, fields)
fields_none = mut.Fields(enum.kNone)
self.assertNotEqual(fields, fields_none)
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 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_pc_flags_api(self):
# BaseField
enum = mut.BaseField
enum.kNone
enum.kXYZs
# Fields
fields = mut.Fields(base_fields=enum.kXYZs)
self.assertEqual(fields.base_fields(), enum.kXYZs)
self.assertTrue(fields.has_base_fields())
self.assertEqual(fields, fields)
fields_none = mut.Fields(enum.kNone)
self.assertNotEqual(fields, fields_none)
# TODO(jwnimmer-tri) Ideally, these would map back to the bitwise
# constants, but for now this works; the ctor accepts bare ints.
self.assertEqual(repr(fields_none), "Fields(base_fields=0)")
self.assertEqual(repr(fields), "Fields(base_fields=2)")
repr_and_back = eval(repr(fields), dict(Fields=mut.Fields))
self.assertEqual(repr_and_back.base_fields(), enum.kXYZs)
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.])
