def draw_open3d_point_cloud(meshcat,
path,
pcd,
normals_scale=0.0,
point_size=0.001):
pts = np.asarray(pcd.points)
assert (pcd.has_colors()) # TODO(russt): handle this case better
cloud = PointCloud(pts.shape[0], Fields(BaseField.kXYZs | BaseField.kRGBs))
cloud.mutable_xyzs()[:] = pts.T
cloud.mutable_rgbs()[:] = 255 * np.asarray(pcd.colors).T
meshcat.SetObject(path, cloud, point_size=point_size)
if pcd.has_normals() and normals_scale > 0.0:
assert ('need to implement LineSegments in meshcat c++')
normals = np.asarray(pcd.normals)
vertices = np.hstack(
(pts, pts + normals_scale * normals)).reshape(-1, 3).T
meshcat["normals"].set_object(
g.LineSegments(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=0x000000)))
class TestPointCloudConcatenation(unittest.TestCase):
def setUp(self):
builder = DiagramBuilder()
X_WP_0 = RigidTransform.Identity()
X_WP_1 = RigidTransform.Identity()
X_WP_1.set_translation([1.0, 0, 0])
id_list = ["0", "1"]
self.pc_concat = builder.AddSystem(PointCloudConcatenation(id_list))
self.num_points = 10000
xyzs = np.random.uniform(-0.1, 0.1, (3, self.num_points))
# Only go to 254 to distinguish between point clouds with and without
# color.
rgbs = np.random.uniform(0., 254.0, (3, self.num_points))
self.pc = PointCloud(self.num_points,
Fields(BaseField.kXYZs | BaseField.kRGBs))
self.pc.mutable_xyzs()[:] = xyzs
self.pc.mutable_rgbs()[:] = rgbs
self.pc_no_rgbs = PointCloud(self.num_points, Fields(BaseField.kXYZs))
self.pc_no_rgbs.mutable_xyzs()[:] = xyzs
diagram = builder.Build()
simulator = Simulator(diagram)
self.context = diagram.GetMutableSubsystemContext(
self.pc_concat, simulator.get_mutable_context())
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_0").get_index(),
AbstractValue.Make(X_WP_0))
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_1").get_index(),
AbstractValue.Make(X_WP_1))
def test_no_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# Tthe second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
# Even if both input point clouds don't have rgbs, the fused point
# cloud should contain rgbs of the default color.
self.assertTrue(fused_pc.has_rgbs())
self.assertTrue(
np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255])))
self.assertTrue(
np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])))
def test_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# The second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
self.assertTrue(fused_pc.has_rgbs())
self.assertTrue(
np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255])))
self.assertTrue(
np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255])))
def test_mix_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# The second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
self.assertTrue(fused_pc.has_rgbs())
# We don't know in what order the two point clouds will be combined.
rgb_first = np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255]))
rgb_last = np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255]))
no_rgb_first = np.all(fused_pc.rgbs()[:,
0] == np.array([255, 255, 255]))
no_rgb_last = np.all(fused_pc.rgbs()[:,
-1] == np.array([255, 255, 255]))
self.assertTrue((rgb_first and no_rgb_last)
or (no_rgb_first and rgb_last))