def runTest(self): self.vis.delete() v = self.vis["shapes"] v.set_transform(tf.translation_matrix([1., 0, 0])) v["cube"].set_object(g.Box([0.1, 0.2, 0.3])) v["cube"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15])) v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22)) v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0]))) v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd))) v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15])) v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1])) v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1])) v = self.vis["meshes/valkyrie/head"] v.set_object(g.Mesh( g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")), g.MeshLambertMaterial( map=g.ImageTexture( image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png")) ) ) )) v.set_transform(tf.translation_matrix([0, 0.5, 0.5])) v = self.vis["points"] v.set_transform(tf.translation_matrix([-1, 0, 0])) verts = np.random.rand(3, 100000) colors = verts v["random"].set_object(g.PointCloud(verts, colors)) v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
def DoPublish(self, context, event): # TODO(SeanCurtis-TRI) We want to be able to use this visualizer to # draw without having it part of a Simulator. That means we'd like # vis.Publish(context) to work. Currently, pydrake offers no mechanism # to declare a forced event. However, by overriding DoPublish and # putting the forced event callback code in the override, we can # simulate it. # We need to bind a mechanism for declaring forced events so we don't # have to resort to overriding the dispatcher. LeafSystem.DoPublish(self, context, event) point_cloud_P = self.EvalAbstractInput(context, 0).get_value() # `Q` is a point in the point cloud. p_PQs = point_cloud_P.xyzs() # Use only valid points. valid = np.logical_not(np.isnan(p_PQs)) valid = np.all(valid, axis=0) # Reduce along XYZ axis. p_PQs = p_PQs[:, valid] if point_cloud_P.has_rgbs(): rgbs = point_cloud_P.rgbs()[:, valid] else: # Need manual broadcasting. count = p_PQs.shape[1] rgbs = np.tile(np.array([self._default_rgb]).T, (1, count)) # pydrake `PointCloud.rgbs()` are on [0..255], while meshcat # `PointCloud` colors are on [0..1]. rgbs = rgbs / 255. # Do not use in-place so we can promote types. # Send to meshcat. self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs)) self._meshcat_viz[self._name].set_transform(self._X_WP.GetAsMatrix4())
def visualize_point_cloud_meshcat(meshcat_vis, clouds: np.ndarray): ''' Visualize the ground truth (red), observation (blue), and transformed (yellow) point clouds in meshcat. Args: meschat_vis: an instance of a meshcat visualizer scene: an Nx3 numpy array representing scene point cloud model: an Mx3 numpy array representing model point cloud X_GO: 4x4 numpy array of the homogeneous transformation from the scene point cloud to the model point cloud. ''' meshcat_vis['model'].delete() meshcat_vis['observations'].delete() meshcat_vis['transformed_observations'].delete() for i, cloud in enumerate(clouds): # Make meshcat color arrays. N = cloud.shape[0] if i < 3: color_arr = make_meshcat_color_array(N, *rgb_sequences[i]) else: raise NotImplementedError # Create red and blue meshcat point clouds for visualization. cloud_meshcat = g.PointCloud(cloud[:, :3].T, color_arr, size=0.01) meshcat_vis[str(i)].set_object(cloud_meshcat)
def add_pointcloud(self, cloud, color=None, cloud_key='cloud_0'): # type: (np.ndarray, np.ndarray, str) -> None """ Add a point cloud to the visualizer :param cloud: (N, 3) np.ndarray :param color: (N, 3) np.ndarray of (r, g, b) color or None :param cloud_key: The key used to index the point cloud :return: """ if cloud_key in self._pointcloud_set: print('Duplicate pointcloud key detected') print('You should use update_pointcloud instread') return # The color if color is None: color = MeshCatVisualizer._random_color_meshcat(cloud.shape[0]) # Check shape assert cloud.shape[0] == color.shape[0] assert cloud.shape[1] == 3 assert color.shape[1] == 3 # Add to meshcat self._meshcat_vis[self._prefix][cloud_key].set_object(meshcat_geometry.PointCloud(cloud.T, color.T))
def VisualizeTransform(meshcat_vis, points, transform): """Visualizes the points transformed by transform in yellow. Args: @param meschat_vis An instance of a meshcat visualizer. @param points An Nx3 numpy array representing a point cloud. @param transform a 4x4 numpy array representing a homogeneous transformation. """ # Make meshcat color arrays. N = points.shape[0] yellow = MakeMeshcatColorArray(N, 1, 1, 0) homogenous_points = np.ones((N, 4)) homogenous_points[:, :3] = np.copy(points) transformed_points = transform.dot(homogenous_points.T) transformed_points_meshcat = \ g.PointCloud(transformed_points[:3, :], yellow.T) meshcat_vis['transformed_observations'].set_object( transformed_points_meshcat)
def draw_points(meshcat, points, color, **kwargs): """Helper for sending a 3xN points of a single color to MeshCat""" points = np.asarray(points) assert points.shape[0] == 3 if points.size == 3: points.shape = (3, 1) colors = np.tile(np.asarray(color).reshape(3, 1), (1, points.shape[1])) meshcat.set_object(g.PointCloud(points, colors, **kwargs))
def VisualizeICP(meshcat_vis, scene, model, X_MS): """ Visualizes the ground truth (red), observation (blue), and transformed (yellow) point clouds in meshcat. @param meschat_vis An instance of a meshcat visualizer. @param scene An Nx3 numpy array representing the scene point cloud. @param model An Mx3 numpy array representing the model point cloud. @param X_GO A 4x4 numpy array of the homogeneous transformation from the scene point cloud to the model point cloud. """ meshcat_vis['model'].delete() meshcat_vis['observations'].delete() meshcat_vis['transformed_observations'].delete() # Make meshcat color arrays. N = scene.shape[0] M = model.shape[0] red = MakeMeshcatColorArray(M, 0.5, 0, 0) blue = MakeMeshcatColorArray(N, 0, 0, 0.5) yellow = MakeMeshcatColorArray(N, 1, 1, 0) # Create red and blue meshcat point clouds for visualization. model_meshcat = g.PointCloud(model.T, red, size=0.01) observations_meshcat = g.PointCloud(scene.T, blue, size=0.01) meshcat_vis['model'].set_object(model_meshcat) meshcat_vis['observations'].set_object(observations_meshcat) # Create a copy of the scene point cloud that is homogenous # so we can apply a 4x4 homogenous transform to it. homogenous_scene = np.ones((N, 4)) homogenous_scene[:, :3] = np.copy(scene) # Apply the returned transformation to the scene samples to align the # scene point cloud with the ground truth point cloud. transformed_scene = X_MS.dot(homogenous_scene.T) # Create a yellow meshcat point cloud for visualization. transformed_scene_meshcat = \ g.PointCloud(transformed_scene[:3, :], yellow, size=0.01) meshcat_vis['transformed_observations'].set_object( transformed_scene_meshcat)
def draw_open3d_point_cloud(meshcat, pcd, normals_scale=0.0, size=0.001): pts = np.asarray(pcd.points) meshcat.set_object(g.PointCloud(pts.T, np.asarray(pcd.colors).T, size=size)) if pcd.has_normals() and normals_scale > 0.0: 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)))
def visualize_train_input(self, in_img, p, q, theta, z, roll, pitch): """Visualize the training input.""" points = [] colors = [] height = in_img[:, :, 3] for i in range(in_img.shape[0]): for j in range(in_img.shape[1]): pixel = (i, j) position = utils.pixel_to_position(pixel, height, self.bounds, self.pixel_size) points.append(position) colors.append(in_img[i, j, :3]) points = np.array(points).T # shape (3, N) colors = np.array(colors).T / 255.0 # shape (3, N) self.vis["pointclouds/scene"].set_object( g.PointCloud(position=points, color=colors)) pick_position = utils.pixel_to_position(p, height, self.bounds, self.pixel_size) label = "pick" utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1) pick_transform = np.eye(4) pick_transform[0:3, 3] = pick_position self.vis[label].set_transform(pick_transform) place_position = utils.pixel_to_position(q, height, self.bounds, self.pixel_size) label = "place" utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1) place_transform = np.eye(4) place_transform[0:3, 3] = place_position place_transform[2, 3] = z rotation = utils.get_pybullet_quaternion_from_rot( (roll, pitch, -theta)) quaternion_wxyz = np.asarray( [rotation[3], rotation[0], rotation[1], rotation[2]]) place_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] self.vis[label].set_transform(place_transform) _, ax = plt.subplots(2, 1) ax[0].imshow(in_img.transpose(1, 0, 2)[:, :, :3] / 255.0) ax[0].scatter(p[0], p[1]) ax[0].scatter(q[0], q[1]) ax[1].imshow(in_img.transpose(1, 0, 2)[:, :, 3]) ax[1].scatter(p[0], p[1]) ax[1].scatter(q[0], q[1]) plt.show()
def PlotMeshcatPointCloud(meshcat_vis, point_cloud_name, points, colors): """A wrapper function to plot meshcat point clouds. Args: @param meshcat_vis An instance of a meshcat visualizer. @param point_cloud_name string. The name of the meshcat point clouds. @param points An Nx3 numpy array of (x, y, z) points. @param colors An Nx3 numpy array of (r, g, b) colors corresponding to points. """ meshcat_vis[point_cloud_name].set_object(g.PointCloud(points.T, colors.T))
def _DoPublish(self, context, event): u_data = self.EvalAbstractInput(context, 0).get_value() x = self.EvalVectorInput(context, 1).get_value() w, h, _ = u_data.data.shape depth_image = u_data.data[:, :, 0] # Convert depth image to point cloud, with +z being # camera "forward" Kinv = np.linalg.inv( self.camera.depth_camera_info().intrinsic_matrix()) U, V = np.meshgrid(np.arange(h), np.arange(w)) points_in_camera_frame = np.vstack([ U.flatten(), V.flatten(), np.ones(w*h)]) points_in_camera_frame = Kinv.dot(points_in_camera_frame) * \ depth_image.flatten() # The depth camera has some offset from the camera's root frame, # so take than into account. pose_mat = self.camera.depth_camera_optical_pose().matrix() points_in_camera_frame = pose_mat[0:3, 0:3].dot(points_in_camera_frame) points_in_camera_frame += np.tile(pose_mat[0:3, 3], [w*h, 1]).T kinsol = self.rbt.doKinematics(x[:self.rbt.get_num_positions()]) points_in_world_frame = self.rbt.transformPoints( kinsol, points_in_camera_frame, self.camera.frame().get_frame_index(), 0) # Color points according to their normalized height min_height = 0.6 max_height = 0.9 colors = cm.jet( (points_in_world_frame[2, :]-min_height)/(max_height-min_height) ).T[0:3, :] self.vis[self.prefix]["points"].set_object( g.PointCloud(position=points_in_world_frame, color=colors, size=0.005))
def draw_points(vis, vis_prefix, name, points, normals=None, colors=None, size=0.001, normals_length=0.01): vis[vis_prefix][name].set_object( g.PointCloud(position=points, color=colors, size=size)) n_pts = points.shape[1] if normals is not None: # Drawing normals for debug lines = np.zeros([3, n_pts * 2]) inds = np.array(range(0, n_pts * 2, 2)) lines[:, inds] = points[0:3, :] lines[:, inds+1] = points[0:3, :] + \ normals * normals_length vis[vis_prefix]["%s_normals" % name].set_object( meshcat.geometry.LineSegmentsGeometry(lines, None))
def meshcat_visualize(vis, obs, act, info): """Visualize data using meshcat.""" for key in sorted(info.keys()): pose = info[key] pick_transform = np.eye(4) pick_transform[0:3, 3] = pose[0] quaternion_wxyz = np.asarray( [pose[1][3], pose[1][0], pose[1][1], pose[1][2]]) pick_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] label = 'obj_' + str(key) make_frame(vis, label, h=0.05, radius=0.0012, o=1.0) vis[label].set_transform(pick_transform) for cam_index in range(len(act['camera_config'])): verts = unproject_depth_vectorized( obs['depth'][cam_index], [0, 1], np.array(act['camera_config'][cam_index]['intrinsics']).reshape( 3, 3), np.zeros(5)) # switch from [N,3] to [3,N] verts = verts.T cam_transform = np.eye(4) cam_transform[0:3, 3] = act['camera_config'][cam_index]['position'] quaternion_xyzw = act['camera_config'][cam_index]['rotation'] quaternion_wxyz = np.asarray([ quaternion_xyzw[3], quaternion_xyzw[0], quaternion_xyzw[1], quaternion_xyzw[2] ]) cam_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3, 0:3] verts = apply_transform(cam_transform, verts) colors = obs['color'][cam_index].reshape(-1, 3).T / 255.0 vis['pointclouds/' + str(cam_index)].set_object( g.PointCloud(position=verts, color=colors))
def DoPublish(self, context, event): LeafSystem.DoPublish(self, context, event) point_cloud_P = self.EvalAbstractInput(context, 0).get_value() # `Q` is a point in the point cloud. p_PQs = point_cloud_P.xyzs() # Use only valid points. valid = np.logical_not(np.isnan(p_PQs)) valid = np.all(valid, axis=0) # Reduce along XYZ axis. p_PQs = p_PQs[:, valid] if point_cloud_P.has_rgbs(): rgbs = point_cloud_P.rgbs()[:, valid] else: # Need manual broadcasting. count = p_PQs.shape[1] rgbs = np.tile(np.array([self._default_rgb]).T, (1, count)) # pydrake `PointCloud.rgbs()` are on [0..255], while meshcat # `PointCloud` colors are on [0..1]. rgbs = rgbs / 255. # Do not use in-place so we can promote types. # Send to meshcat. self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs)) self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())
def runTest(self): self.vis.delete() v = self.vis["shapes"] v.set_transform(tf.translation_matrix([1., 0, 0])) v["box"].set_object(g.Box([1.0, 0.2, 0.3])) v["box"].delete() v["box"].set_object(g.Box([0.1, 0.2, 0.3])) v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15])) v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22)) v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0]))) v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd))) v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15])) v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1])) v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1])) v["transparent_ellipsoid"].set_object(g.Mesh( g.Ellipsoid([0.3, 0.1, 0.1]), g.MeshLambertMaterial(color=0xffffff, opacity=0.5))) v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1])) v = self.vis["meshes/valkyrie/head"] v.set_object(g.Mesh( g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")), g.MeshLambertMaterial( map=g.ImageTexture( image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png")) ) ) )) v.set_transform(tf.translation_matrix([0, 0.5, 0.5])) v = self.vis["meshes/convex"] v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj")))) v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii")))) v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0])) v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary")))) v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0])) v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae")))) v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0])) v = self.vis["points"] v.set_transform(tf.translation_matrix([0, 2, 0])) verts = np.random.rand(3, 1000000) colors = verts v["random"].set_object(g.PointCloud(verts, colors)) v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0])) v = self.vis["lines"] v.set_transform(tf.translation_matrix(([-2, -3, 0]))) vertices = np.random.random((3, 10)).astype(np.float32) v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices))) v["line"].set_object(g.Line(g.PointsGeometry(vertices))) v["line"].set_transform(tf.translation_matrix([0, 1, 0])) v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices))) v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0])) v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000))) v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0])) colors = vertices # Color each line by treating its xyz coordinates as RGB colors v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True))) v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0])) v["triad"].set_object(g.LineSegments( g.PointsGeometry(position=np.array([ [0, 0, 0], [1, 0, 0], [0, 0, 0], [0, 1, 0], [0, 0, 0], [0, 0, 1]]).astype(np.float32).T, color=np.array([ [1, 0, 0], [1, 0.6, 0], [0, 1, 0], [0.6, 1, 0], [0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T ), g.LineBasicMaterial(vertexColors=True))) v["triad"].set_transform(tf.translation_matrix(([0, 5, 0]))) v["triad_function"].set_object(g.triad(0.5)) v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))
def _add_rbt_geometry(self, rbt, rbt_key, draw_collision=False): # type: (RigidBodyTree, str, bool) -> None """ Add the geometry of a RigidBodyTree to the meshcat visualizer. Note that later update to rbt WOULD NOT be reflected :param rbt: :param rbt_key: The key to index the rigid body tree :param draw_collision: :return: """ n_bodies = rbt.get_num_bodies() - 1 # all_meshcat_geometry = {} for body_i in range(n_bodies): # Get the body index body = rbt.get_body(body_i + 1) body_name = rbt_key + body.get_name() + ("(%d)" % body_i) if draw_collision: draw_elements = [rbt.FindCollisionElement(k) for k in body.get_collision_element_ids()] else: draw_elements = body.get_visual_elements() for element_i, element in enumerate(draw_elements): element_local_tf = element.getLocalTransform() if element.hasGeometry(): geom = element.getGeometry() geom_type = geom.getShape() if geom_type == Shape.SPHERE: meshcat_geom = meshcat.geometry.Sphere(geom.radius) elif geom_type == Shape.BOX: meshcat_geom = meshcat.geometry.Box(geom.size) elif geom_type == Shape.CYLINDER: meshcat_geom = meshcat.geometry.Cylinder( geom.length, geom.radius) # In Drake, cylinders are along +z # In meshcat, cylinders are along +y # Rotate to fix this misalignment extra_rotation = tf.rotation_matrix( math.pi / 2., [1, 0, 0]) element_local_tf[0:3, 0:3] = \ element_local_tf[0:3, 0:3].dot( extra_rotation[0:3, 0:3]) elif geom_type == Shape.MESH: meshcat_geom = \ meshcat.geometry.ObjMeshGeometry.from_file( geom.resolved_filename[0:-3] + "obj") # respect mesh scale element_local_tf[0:3, 0:3] *= geom.scale elif geom_type == Shape.MESH_POINTS: # The shape of points is (3, N) points = geom.getPoints() # type: np.ndarray n_points = points.shape[1] color = MeshCatVisualizer._make_meshcat_color(n_points, r=255, g=0, b=0) meshcat_geom = meshcat_geometry.PointCloud(points, color.T, size=0.01) self._meshcat_vis[self._prefix][body_name][str(element_i)] \ .set_object(meshcat_geom) self._meshcat_vis[self._prefix][body_name][str(element_i)]. \ set_transform(element_local_tf) self._rbt_geometry_element_set.add(body_name) continue else: print "UNSUPPORTED GEOMETRY TYPE ", \ geom.getShape(), " IGNORED" continue # The color of the body rgba = [1., 0.7, 0., 1.] if not draw_collision: rgba = element.getMaterial() self._meshcat_vis[self._prefix][body_name][str(element_i)] \ .set_object(meshcat_geom, meshcat.geometry.MeshLambertMaterial( color=MeshCatVisualizer.rgba2hex(rgba))) self._meshcat_vis[self._prefix][body_name][str(element_i)]. \ set_transform(element_local_tf) self._rbt_geometry_element_set.add(body_name)