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.])