def vis(coords, node_indices, point_indices=None): import trimesh depth = len(coords) scale_factor = np.max(np.linalg.norm(coords[0], axis=-1)) for c in coords: c /= scale_factor if point_indices is None: point_indices = [0] * depth for i, point_index in enumerate(point_indices): ci = coords[i] center = ci[point_index] for j in range(i + 1): cj = coords[j] ni = node_indices[i][j] neighbors = ni[point_index] print(i, j, ci.shape, cj.shape, neighbors.shape) print(np.max(np.linalg.norm(cj[neighbors] - center, axis=-1))) scene = trimesh.Scene() scene.add_geometry(trimesh.PointCloud(cj, color=(0, 255, 0))) scene.add_geometry(trimesh.PointCloud(ci, color=(0, 0, 255))) scene.add_geometry( trimesh.PointCloud(cj[neighbors], color=(255, 0, 0))) scene.add_geometry( trimesh.primitives.Sphere(center=center, radius=0.01)) scene.show(background=(0, 0, 0))
def algorithm(): gpu = 0 if gpu >= 0: cuda.get_device_from_id(gpu).use() instance_id = 3 # == class_id models = morefusion.datasets.YCBVideoModels() pcd_cad = models.get_pcd(class_id=instance_id) dataset = morefusion.datasets.YCBVideoDataset("train") example = dataset.get_example(1000) depth = example["depth"] instance_label = example["label"] K = example["meta"]["intrinsic_matrix"] pcd_depth = morefusion.geometry.pointcloud_from_depth(depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2]) nonnan = ~np.isnan(pcd_depth).any(axis=2) mask = (instance_label == instance_id) & nonnan pcd_depth_target = pcd_depth[mask] # ------------------------------------------------------------------------- quaternion_init = np.array([1, 0, 0, 0], dtype=np.float32) translation_init = np.median(pcd_depth_target, axis=0) transform_init = morefusion.functions.transformation_matrix( quaternion_init, translation_init).array pcd_cad = morefusion.extra.open3d.voxel_down_sample(pcd_cad, voxel_size=0.01) pcd_depth_target = morefusion.extra.open3d.voxel_down_sample( pcd_depth_target, voxel_size=0.01) registration = ICPRegistration(pcd_depth_target, pcd_cad, transform_init) for T_cad2cam in registration.register_iterative(): scene = trimesh.Scene() geom = trimesh.PointCloud(pcd_depth_target, colors=[1.0, 0, 0]) scene.add_geometry(geom, geom_name="a", node_name="a") geom = trimesh.PointCloud(pcd_cad, colors=[0, 1.0, 0]) scene.add_geometry(geom, geom_name="b", node_name="b", transform=T_cad2cam) scene.camera_transform = morefusion.extra.trimesh.to_opengl_transform() yield scene
def vis_clouds(in_coords, out_coords, target_index, neighbors): out_colors = np.ones((out_coords.shape[0], 3), dtype=np.uint8) out_colors *= 128 out_colors[target_index] = [0, 255, 0] out_cloud = trimesh.PointCloud(out_coords, out_colors) in_colors = np.zeros((in_coords.shape[0], 3), dtype=np.uint8) in_colors[:, 2] = 255 neigh_indices = neighbors[neighbors[:, 0] == target_index, 1] in_colors[neigh_indices] = [255, 0, 0] in_cloud = trimesh.PointCloud(in_coords, in_colors) scene = in_cloud.scene() scene.add_geometry(out_cloud) scene.show(background=(0, 0, 0))
def visualize(occupied, empty, K, width, height, rgb, pcd, mask, resolution, aabb): window = pyglet.window.Window(width=int(640 * 0.9 * 3), height=int(480 * 0.9)) @window.event def on_key_press(symbol, modifiers): if modifiers == 0: if symbol == pyglet.window.key.Q: window.on_close() gui = glooey.Gui(window) hbox = glooey.HBox() hbox.set_padding(5) camera = trimesh.scene.Camera( resolution=(width, height), focal=(K[0, 0], K[1, 1]), transform=np.eye(4), ) camera_marker = trimesh.creation.camera_marker(camera, marker_height=0.1) # initial camera pose camera.transform = np.array( [[0.73256052, -0.28776419, 0.6168848, 0.66972396], [-0.26470017, -0.95534823, -0.13131483, -0.12390466], [0.62712751, -0.06709345, -0.77602162, -0.28781298], [ 0., 0., 0., 1., ]], ) aabb_min, aabb_max = aabb bbox = trimesh.path.creation.box_outline( aabb_max - aabb_min, tf.translation_matrix((aabb_min + aabb_max) / 2), ) geom = trimesh.PointCloud(vertices=pcd[mask], colors=rgb[mask]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='pointcloud')) geom = trimesh.voxel.multibox(occupied, pitch=resolution, colors=[1., 0, 0, 0.5]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='occupied')) geom = trimesh.voxel.multibox(empty, pitch=resolution, colors=[0.5, 0.5, 0.5, 0.5]) scene = trimesh.Scene(camera=camera, geometry=[bbox, geom, camera_marker]) hbox.add(labeled_scene_widget(scene, label='empty')) gui.add(hbox) pyglet.app.run()
def main(): models = morefusion.datasets.YCBVideoModels() points = models.get_pcd(class_id=2) quaternion_true = tf.random_quaternion() quaternion_pred = quaternion_true + [0.1, 0, 0, 0] transform_true = tf.quaternion_matrix(quaternion_true) transform_pred = tf.quaternion_matrix(quaternion_pred) scenes = {} for use_translation in [False, True]: if use_translation: translation_true = np.random.uniform(-0.02, 0.02, (3,)) translation_pred = np.random.uniform(-0.02, 0.02, (3,)) transform_true[:3, 3] = translation_true transform_pred[:3, 3] = translation_pred add = morefusion.metrics.average_distance( [points], [transform_true], [transform_pred] )[0][0] # --------------------------------------------------------------------- scene = trimesh.Scene() points_true = tf.transform_points(points, transform_true) colors = np.full((points_true.shape[0], 3), [1.0, 0, 0]) geom = trimesh.PointCloud(vertices=points_true, color=colors) scene.add_geometry(geom) points_pred = tf.transform_points(points, transform_pred) colors = np.full((points_true.shape[0], 3), [0, 0, 1.0]) geom = trimesh.PointCloud(vertices=points_pred, color=colors) scene.add_geometry(geom) scenes[f"use_translation: {use_translation}, add: {add}"] = scene if scenes: camera_transform = list(scenes.values())[0].camera_transform scene.camera_transform = camera_transform morefusion.extra.trimesh.display_scenes(scenes)
def visual_weights(bone_obj, bone_pts_weights, vis_label=True): ''' visulize_weight Parameters: bone_obj: trimesh with N vertices / np.array [N, 3] bone_pts_weights: np.array [N,k] vis_label: argmax weight as color if true Return: colored bone obj (trimesh.mesh or trimesh.pointcloud) ''' if vis_label: label_list = np.argmax(bone_pts_weights, axis=1) empty_weight_mask = np.max(bone_pts_weights, axis=1) == 0 bone_num = bone_pts_weights.shape[-1] if isinstance(bone_obj, np.ndarray): rgb = np.stack([ colorsys.hsv_to_rgb(i * 1.0 / bone_num, 0.8, 0.8) for i in label_list ]) rgb[empty_weight_mask] = [0, 0, 0] a = np.ones([rgb.shape[0], 1]) rgba = np.hstack([rgb, a]) bone_pts = trimesh.PointCloud(vertices=bone_obj.reshape(-1, 3), colors=rgba) return bone_pts else: if isinstance(bone_obj.visual, trimesh.visual.TextureVisuals): bone_obj.visual = bone_obj.visual.to_color() bone_obj.visual.vertex_colors = np.zeros( [len(bone_obj.vertices), 4]) if vis_label: for i in range(len(label_list)): (r, g, b) = colorsys.hsv_to_rgb(label_list[i] * 1.0 / bone_num, 0.8, 0.8) bone_obj.visual.vertex_colors[i] = (r * 255, g * 255, b * 255, 255) else: for i in range(bone_pts_weights.shape[0]): weights = bone_pts_weights[i].reshape(-1) color_map = np.arange(len(weights)) * 1.0 / bone_num h = np.dot(weights, color_map) (r, g, b) = colorsys.hsv_to_rgb(h, 0.8, 0.8) bone_obj.visual.vertex_colors[i] = (r * 255, g * 255, b * 255, 255) return bone_obj
def vis(coords, node_indices, row_splits, outer_row_splits): import trimesh depth = len(node_indices) for i in range(depth): scene = trimesh.scene.Scene() p0 = trimesh.PointCloud(coords[i], color=(255, 255, 255)) p1 = trimesh.PointCloud(coords[i + 1], color=(0, 0, 255)) geometries = [p0, p1] rs = row_splits[i] ni = node_indices[i] for j in outer_row_splits[i][:-1]: geometries.append( trimesh.primitives.Sphere(center=coords[i + 1][j], radius=0.02, color=(255, 255, 255))) geometries.append( trimesh.PointCloud(coords[i][ni[rs[j]:rs[j + 1]]], color=(255, 0, 0))) for g in geometries: scene.add_geometry(g) scene.show(background=(0, 0, 0))
def save_mesh(verts, savename, normals=None, faces=None, verts_color=None): if faces is None: if verts_color is None: verts_color = np.zeros([verts.shape[0], 4]) verts_pts = trimesh.PointCloud(vertices=verts, colors=verts_color) verts_pts.export(savename) return verts_pts else: verts_mesh = trimesh.Trimesh(vertices=verts, faces=faces, normals=normals, process=False) verts_mesh.export(savename) return verts_mesh
def vis_cloud(dataset: tf.data.Dataset, augment_func: Optional[Callable] = None): if augment_func is not None: dataset = dataset.map(augment_func) for coords, label in dataset: if isinstance(coords, dict): coords = coords["positions"] coords = coords.numpy() pc = trimesh.PointCloud(coords) print(f"label = {label.numpy()}") print( f"max_r = {tf.reduce_max(tf.linalg.norm(coords, axis=-1)).numpy()}" ) pc.show()
def get_scenes(): dataset = morefusion.datasets.YCBVideoDataset("train") index = 0 video_id_prev = None scene = trimesh.Scene() while index < len(dataset): example = dataset[index] video_id, frame_id = dataset.ids[index].split("/") clear = False if video_id_prev is not None and video_id_prev != video_id: clear = True for node_name in scene.graph.nodes_geometry: scene.graph.transforms.remove_node(node_name) video_id_prev = video_id rgb = example["color"] depth = example["depth"] K = example["meta"]["intrinsic_matrix"] T_world2cam = np.r_[example["meta"]["rotation_translation_matrix"], [[0, 0, 0, 1]]] T_cam2world = np.linalg.inv(T_world2cam) pcd = morefusion.geometry.pointcloud_from_depth(depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2]) nonnan = ~np.isnan(depth) geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan]) scene.add_geometry(geom, transform=T_cam2world) # A kind of current camera view, but a bit far away to see whole scene. scene.camera.resolution = (rgb.shape[1], rgb.shape[0]) scene.camera.focal = (K[0, 0], K[1, 1]) scene.camera_transform = morefusion.extra.trimesh.to_opengl_transform( T_cam2world @ tf.translation_matrix([0, 0, -0.5])) # scene.set_camera() geom = trimesh.creation.camera_marker(scene.camera, marker_height=0.05)[1] geom.colors = [(0, 1.0, 0)] * len(geom.entities) scene.add_geometry(geom, transform=T_cam2world) index += 15 print(f"[{index:08d}] video_id={video_id}") yield {"__clear__": clear, "rgb": rgb, "scene": scene}
def vis_example(image, cloud): import numpy as np import matplotlib.pyplot as plt import trimesh if hasattr(image, 'numpy'): image = image.numpy() if hasattr(cloud, 'numpy'): cloud = cloud.numpy() image -= np.min(image) image /= np.max(image) plt.imshow(image) plt.show() colors = np.empty((cloud.shape[0], 4), dtype=np.uint8) colors[:] = [0, 0, 255, 255] trimesh.PointCloud(cloud, colors=colors).show()
def show_texture(texture, wt): points = [] colors = [] for i in range(mesh.faces.shape[0]): if wt[i] == 0: continue temp = texture[i] / wt[i] A, B, C = vertices[i] for u in range(reso): for v in range(reso - u): X = A + u * (B - A) + v * (C - A) points.append(X) colors.append(temp[u * reso + v]) pcd = trimesh.PointCloud(vertices=points, colors=colors) print("Time taken:", time() - start) pcd.scene().show()
def create_scene(graph, fov=(640, 480)): """ Visualise the beliefs of the camera poses and landmarks over the GBP iterations. """ cam_params, landmarks = [], [] for cam in graph.cam_nodes: cam_params.append(list(cam.mu)) for lmk in graph.lmk_nodes: landmarks.append(list(lmk.mu)) K = graph.factors[0].args[0] focal = [K[0, 0], K[1, 1]] angle_fov = np.array([0., 0.]) angle_fov[0] = np.arctan(fov[0] / focal[0]) # in radians angle_fov[1] = np.arctan(fov[1] / focal[1]) angle_fov_deg = angle_fov * (180 / np.pi) # in degrees scene = trimesh.Scene() for i, params in enumerate(cam_params): Twc = np.linalg.inv(transformations.getT_axisangle(params)) cam_name = f'cam_{i}' cam = trimesh.scene.Camera(fov=angle_fov_deg) geom = trimesh.creation.camera_marker(cam, marker_height=0.1) # geom[1].colors = [(0., 0., 1.)] * 16 scene.add_geometry(geom[1], transform=Twc, node_name=cam_name) if i == 0: # Set initial viewpoint behind this keyframe cam_pose = Twc.copy() cam_pose[:3, 3] -= 2.0 * cam_pose[:3, 2] / np.linalg.norm(cam_pose[:3, 2]) scene.camera.transform = to_opengl_transform(transform=cam_pose) landmarks_pcd = trimesh.PointCloud(landmarks) landmarks_pcd.colors = [[0., 0., 0.9]] * len(landmarks) scene.add_geometry(landmarks_pcd, node_name='landmarks_pcd') scene.camera.resolution = [1200, 900] scene.camera.fov = 60 * (scene.camera.resolution / scene.camera.resolution.max()) return scene
def main(_): import tensorflow as tf tf.compat.v1.enable_eager_execution() import tensorflow_datasets as tfds FLAGS = flags.FLAGS if FLAGS.version == "2": from shape_tfds.shape.modelnet import Pointnet2, get_pointnet2_config builder = Pointnet2(config=get_pointnet2_config(num_classes=40)) elif FLAGS.version == "2h": from shape_tfds.shape.modelnet import Pointnet2H5 builder = Pointnet2H5() elif FLAGS.version == "1": from shape_tfds.shape.modelnet import Pointnet builder = Pointnet() else: raise ValueError( 'Invalid choice of version {} - must be "1", "2", or "2h"'.format( FLAGS.version)) download_config = tfds.core.download.DownloadConfig( register_checksums=True) # download_config = None builder.download_and_prepare(download_config=download_config) if FLAGS.vis: import numpy as np try: import trimesh except ImportError: raise ImportError("visualizing requires trimesh") for cloud, _ in builder.as_dataset(split="train", as_supervised=True): pos = cloud["positions"].numpy() print(np.min(pos, axis=0)) print(np.max(pos, axis=0)) print(np.max(np.linalg.norm(pos, axis=-1))) trimesh.PointCloud(pos).show()
def show_texture(texture, wt, fileName): points = [] colors = [] for i in range(mesh.elements[1].data.shape[0]): A, B, C = Vertex[tuple(mesh.elements[1].data[i])] for u in np.arange(0, 1, 1 / SAMPLE_SIZE): for v in np.arange(0, 1 - u, 1 / SAMPLE_SIZE): if wt[i][int(u * SAMPLE_SIZE * SAMPLE_SIZE + v * SAMPLE_SIZE)] == 0: continue w = 1 - u - v X = u * A + v * B + w * C points.append(X) color = texture[i][int(u * SAMPLE_SIZE * SAMPLE_SIZE + v * SAMPLE_SIZE)] / wt[i][ int(u * SAMPLE_SIZE * SAMPLE_SIZE + v * SAMPLE_SIZE)] colors.append([int(color[2]), int(color[1]), int(color[0])]) pcd = trimesh.PointCloud(vertices=points, colors=colors) pcd.export(fileName)
def show_cloud(): b_min = [-0.5, -1.0, 0.0] b_max = [1.0, 1.0, 1.5] pts_inside = esdf.debug_points(b_min, b_max) if skrobot_found: viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) tpc = trimesh.PointCloud(pts_inside) pclink = skrobot.model.primitives.PointCloudLink(tpc) robot_model = skrobot.models.PR2() #viewer.add(robot_model) viewer.add(pclink) viewer.show() else: fig = plt.figure() ax = fig.add_subplot(111, projection='3d') myscat = lambda X: ax.scatter(X[:, 0], X[:, 1], X[:, 2]) myscat(pts_inside) ax.set_xlabel('X Label') ax.set_ylabel('Y Label') ax.set_zlabel('Z Label') plt.show()
for pc_name in point_cloud_name_list: if pc_name not in mesh_name_list: continue mesh_name = pc_name print('Loading ' + pc_name) # Read point cloud v = o3d.io.read_point_cloud( os.path.join(os.path.abspath(point_cloud_directory), pc_name) + point_cloud_format) pc = trimesh.PointCloud(np.asarray(v.points)) # Load mesh mesh = trimesh.load_mesh( os.path.join(os.path.abspath(mesh_directory), mesh_name) + mesh_format) # Change mesh vertices and faces color and set transparency mesh.visual.face_colors[:] = np.array([255, 50, 50, 80]) mesh.visual.vertex_colors[:] = np.array([255, 50, 50, 80]) # Rotate mesh and pc of a fixed amount transform = trimesh.transformations.rotation_matrix(
def brute_force_closest_vertex_to_joints(self): scenes = self.get_valid_scenes() # calculate 50 nearest vertex ids for all meshes and joints closest_k = 50 candidates = {} keep_searching = True while keep_searching: for key, scene in scenes.items(): joint = np.squeeze(scene['joint'].vertices) distances, vertex_ids = scene['mesh'].kdtree.query( joint, closest_k) candidates[key] = { vertex_id: dist for vertex_id, dist in zip(vertex_ids, distances) } # only keep common ids from functools import reduce common_ids = reduce(np.intersect1d, [list(c.keys()) for c in candidates.values()]) if len(common_ids) <= self.settings_loader.min_vertices: closest_k += 10 continue else: keep_searching = False # calculate average distance per mesh/joint for valid ids mean_dist = [ np.mean([c[common_id] for c in candidates.values()]) for common_id in common_ids ] mean_dist = { common_id: dist for common_id, dist in zip(common_ids, mean_dist) } mean_dist = { k: v for k, v in sorted(mean_dist.items(), key=lambda item: item[1]) } # pick closest vertex with min average distance to all joints per mesh closest_id = list(mean_dist)[0] final_vertices = [closest_id] mean_dist.pop(closest_id) while len(final_vertices) < self.settings_loader.min_vertices: # calculate all distance combinations between valid vertices vertex_ids = list(mean_dist) id_dist = [ sp.distance.cdist(s['mesh'].vertices[final_vertices], s['mesh'].vertices[vertex_ids]) for s in scenes.values() ] id_dist = np.mean(id_dist, axis=0) # min the ratio between distances to joint and distance to all other vertices best_dist = list(mean_dist.values()) / id_dist best_id = np.argmin(best_dist) # max the difference between distance to all other vertices and distances to joint # best_dist = id_dist - list(mean_dist.values()) # best_id = np.argmax(best_dist) n, m = np.unravel_index(best_id, best_dist.shape) best_id = vertex_ids[m] final_vertices.append(best_id) mean_dist.pop(best_id) vertices, joints = [], [] for scene in scenes.values(): verts = np.asarray(scene['mesh'].vertices).reshape([-1, 3]) verts = verts[final_vertices] vertices.append(verts) joint = np.asarray(scene['joint'].vertices).reshape([-1, 3]) joints.append(joint) vertex_weight = np.zeros([ 6890, ]) vertices = np.stack(vertices).transpose([0, 2, 1]).reshape( [-1, len(final_vertices)]) joints = np.stack(joints).transpose([0, 2, 1]).reshape([-1]) # constraint weights to sum up to 1 vertices = np.concatenate( [vertices, np.ones([1, vertices.shape[1]])], 0) joints = np.concatenate([joints, np.ones(1)]) # solve with non negative least squares weights = so.nnls(vertices, joints)[0] vertex_weight[final_vertices] = weights file = join('regressors', 'regressor_{}.npy'.format(self.regressor_name.text())) with open(file, 'wb') as f: vertex_weight = vertex_weight.astype(np.float32) vertex_weight = np.expand_dims(vertex_weight, -1) np.save(f, vertex_weight) widget = QMessageBox( icon=QMessageBox.Information, text= 'Regressor file successfully saved to: {}\n\nClick Reset to start again' .format(file), buttons=[QMessageBox.Ok]) widget.exec_() vertex_weight = np.squeeze(vertex_weight) self.convert_button.setEnabled(False) self.regressor_name.setEnabled(False) for scene in self.scene_chache.values(): mesh = scene.geometry['geometry_0'] mesh.visual.vertex_colors = [200, 200, 200, 255] mesh.visual.vertex_colors[final_vertices] = [0, 255, 0, 255] x = np.matmul(vertex_weight, mesh.vertices[:, 0]) y = np.matmul(vertex_weight, mesh.vertices[:, 1]) z = np.matmul(vertex_weight, mesh.vertices[:, 2]) joints = np.vstack((x, y, z)).T joints = trimesh.PointCloud(joints, colors=[0, 255, 0, 255]) scene.add_geometry(joints, geom_name='new_joints')
def point_cloud(vertices, color=None): colors = None if color is None else broadcast_colors( color, vertices.shape[0]) return trimesh.PointCloud(vertices, colors=colors)
tree = KDTree(coords, leaf_size=leaf_size) sample_result, query_result = tree.rejection_ifp_sample_query( r2, r2, tree.get_node_indices(), out_size, k_query ) # sample_result, query_result = tree.ifp_sample_query(r2, tree.get_node_indices(), # out_size, k_query) print(query_result.dists[0, : query_result.counts[0]]) red = [[255, 0, 0]] green = [[0, 255, 0]] blue = [[0, 0, 255]] sampled_coords = coords[sample_result.indices] scene = trimesh.scene.Scene() pc = trimesh.PointCloud(coords, colors=np.full((N, 3), 255, dtype=np.uint8)) scene = pc.scene() scene.add_geometry( trimesh.PointCloud(sampled_coords, colors=np.tile(blue, (out_size, 1))) ) rl = query_result.counts[0] scene.add_geometry( trimesh.PointCloud( coords[query_result.indices[0, :rl]], colors=np.tile(green, (rl, 1)) ) ) scene.add_geometry(trimesh.PointCloud(sampled_coords[:1], colors=red)) # scene.add_geometry( # trimesh.primitives.Sphere(radius=radius, center=sampled_coords[0])) # scene.add_geometry(trimesh.primitives.Sphere(radius=1.1, center=[0, 0, 0])) scene.show(background=np.zeros((3,), dtype=np.uint8))
def _load_obj_impl(file_obj, raw_mesh=False, resolver=None, split_object=False, group_material=True): """ Load a Wavefront OBJ file into kwargs for a trimesh.Scene object. Parameters -------------- file : file like object Contains OBJ data raw_mesh : bool Only load the basic geometry data, e.g. vertices, faces. Ignore materials, uv, etc. Besides, the vertices and faces will keep consistent with the raw data. resolver : trimesh.visual.resolvers.Resolver Allow assets such as referenced textures and material files to be loaded split_object : bool Split meshes at each `o` declared in file group_material : bool Group faces that share the same material into the same mesh. Returns ------------- geometry : dict Loaded geometry parts of obj file. Each part has a name, which corresponds to a submesh """ # if we still have no resolver try using file_obj name if resolver is None and hasattr(file_obj, 'name') and len(file_obj.name) > 0: resolver = FilePathResolver(file_obj.name) # get text as bytes or string blob text = file_obj.read() # if text was bytes decode into string text = util.decode_text(text) # add leading and trailing newlines so we can use the # same logic even if they jump directly in to data lines text = '\n{}\n'.format(text.strip().replace('\r\n', '\n')) # Load Materials materials = {} if not raw_mesh: mtl_position = text.find('mtllib') if mtl_position >= 0: # take the line of the material file after `mtllib` # which should be the file location of the .mtl file mtl_path = text[mtl_position + 6:text.find('\n', mtl_position)].strip() try: # use the resolver to get the data material_kwargs = parse_mtl(resolver[mtl_path], resolver=resolver) # turn parsed kwargs into material objects materials = { k: SimpleMaterial(**v) for k, v in material_kwargs.items() } except IOError: # usually the resolver couldn't find the asset log.warning( 'unable to load materials from: {}'.format(mtl_path)) except BaseException: # something else happened so log a warning log.warning( 'unable to load materials from: {}'.format(mtl_path), exc_info=True) # extract vertices from raw text v, vn, vt, vc = _parse_vertices(text=text) # get relevant chunks that have face data # in the form of (material, object, chunk) face_tuples = _preprocess_faces(text=text, split_object=split_object) # combine chunks that have the same material # some meshes end up with a LOT of components # and will be much slower if you don't do this if group_material: face_tuples = _group_by_material(face_tuples) # Load Faces # now we have clean- ish faces grouped by material and object # so now we have to turn them into numpy arrays and kwargs # for trimesh mesh and scene objects geometry = {} while len(face_tuples) > 0: # consume the next chunk of text material, current_object, chunk = face_tuples.pop() # do wangling in string form # we need to only take the face line before a newline # using builtin functions in a list comprehension # is pretty fast relative to other options # this operation is the only one that is O(len(faces)) # slower due to the tight-loop conditional: # face_lines = [i[:i.find('\n')] # for i in chunk.split('\nf ')[1:] # if i.rfind('\n') >0] # maxsplit=1 means that it can stop working # after it finds the first newline # passed as arg as it's not a kwarg in python2 face_lines = [i.split('\n', 1)[0] for i in chunk.split('\nf ')[1:]] # then we are going to replace all slashes with spaces joined = ' '.join(face_lines).replace('/', ' ') # the fastest way to get to a numpy array # processes the whole string at once into a 1D array # also wavefront is 1-indexed (vs 0-indexed) so offset array = np.fromstring(joined, sep=' ', dtype=np.int64) - 1 # get the number of raw 2D columns in a sample line columns = len(face_lines[0].strip().replace('/', ' ').split()) # make sure we have the right number of values for vectorized if len(array) == (columns * len(face_lines)): # everything is a nice 2D array faces, faces_tex, faces_norm = _parse_faces_vectorized( array=array, columns=columns, sample_line=face_lines[0]) else: # if we had something annoying like mixed in quads # or faces that differ per-line we have to loop # i.e. something like: # '31407 31406 31408', # '32303/2469 32304/2469 32305/2469', log.warning('faces have mixed data, using slow fallback!') faces, faces_tex, faces_norm = _parse_faces_fallback(face_lines) # TODO: name usually falls back to something useless name = current_object if name is None or len(name) == 0 or name in geometry: name = '{}_{}'.format(name, util.unique_id()) # try to get usable texture mesh = {'faces': faces, 'vertices': v} if not raw_mesh: if faces_tex is not None: # convert faces referencing vertices and # faces referencing vertex texture to new faces # where each face if faces_norm is not None and len(faces_norm) == len(faces): new_faces, mask_v, mask_vt, mask_vn = unmerge_faces( faces, faces_tex, faces_norm) else: mask_vn = None new_faces, mask_v, mask_vt = unmerge_faces( faces, faces_tex) if tol.strict: # we should NOT have messed up the faces # note: this is EXTREMELY slow due to all the # float comparisons so only run this in unit tests assert np.allclose(v[faces], v[mask_v][new_faces]) # faces should all be in bounds of vertives assert new_faces.max() < len(v[mask_v]) try: # survive index errors as sometimes we # want materials without UV coordinates uv = vt[mask_vt] except BaseException as E: uv = None raise E # mask vertices and use new faces mesh.update({'vertices': v[mask_v].copy(), 'faces': new_faces}) else: # otherwise just use unmasked vertices uv = None # check to make sure indexes are in bounds if tol.strict: assert faces.max() < len(v) if vn is not None and np.shape(faces_norm) == faces.shape: # do the crazy unmerging logic for split indices new_faces, mask_v, mask_vn = unmerge_faces( faces, faces_norm) else: # generate the mask so we only include # referenced vertices in every new mesh mask_v = np.zeros(len(v), dtype=np.bool) mask_v[faces] = True # reconstruct the faces with the new vertex indices inverse = np.zeros(len(v), dtype=np.int64) inverse[mask_v] = np.arange(mask_v.sum()) new_faces = inverse[faces] # no normals mask_vn = None # start with vertices and faces mesh.update({'faces': new_faces, 'vertices': v[mask_v].copy()}) # if vertex colors are OK save them if vc is not None: mesh['vertex_colors'] = vc[mask_v] # if vertex normals are OK save them if mask_vn is not None: mesh['vertex_normals'] = vn[mask_vn] if not raw_mesh: visual = None if material in materials: visual = TextureVisuals(uv=uv, material=materials[material]) elif material is not None: # material will be None by default log.warning( 'specified material ({}) not loaded!'.format(material)) mesh['visual'] = visual # process will be passed to constructor of Trimesh # which specifies whether remove duplicates and unreferences process = not raw_mesh # store geometry by name if (isinstance(mesh['vertices'], dict) or isinstance(mesh['faces'], dict)): geometry[name] = trimesh.Trimesh(**misc.load_dict(mesh), process=process) elif mesh['faces'] is None: # vertices without faces returns a PointCloud geometry[name] = trimesh.PointCloud(**mesh) else: geometry[name] = trimesh.Trimesh(**mesh, process=process) return geometry
dataset = 'ShapeNet' category = 'airplane' ckpt_model = 'airplane_10b' root_data = '/disk1/yicheng/' + dataset + '/test_data_npy/' + category + '/' root_results = '/disk1/yicheng/' + dataset + '/results/' + ckpt_model + '/' list_el = glob.glob(os.path.join(root_results, '*.mat')) print(len(list_el)) for i, name in enumerate(list_el): # read input model input_id = Path(name).stem pc = np.load(os.path.join(root_data, '%s.npy' % input_id)) if pc.shape[1] > 3: pc = pc[:, :3] pc_tri = trimesh.PointCloud(vertices=pc) # get prediction params_pred = sio.loadmat(name) keypoints = get_kpts_from_non_rigid_modelling(params_pred) keypoints_tri = trimesh.PointCloud(vertices=keypoints) # visualize scene = pc_tri.scene() for kp in keypoints: sphere = trimesh.primitives.Sphere(center=kp, radius=0.02) sphere.visual.face_colors = [255., 0., 0., 255.] scene.add_geometry(sphere) scene.show()
def get_counts(tree, radius): counts = np.zeros((positions.shape[0], ), dtype=np.int64) for i, j in tree.query_pairs(radius): counts[i] += 1 counts[j] += 1 return counts for cloud, label in tqdm.tqdm(tfds.as_numpy(dataset), total=problem.examples_per_epoch(split)): positions = cloud['positions'] print(positions.shape) print(np.max(np.sum(positions**2, axis=-1), axis=0)) tree = cKDTree(positions) counts = get_counts(tree, 0.1) mean = np.mean(counts) means.append(mean) if mean > 15: print('{}: {}, {}'.format(label, class_names[label], mean)) # c2 = np.mean(get_counts(tree, 0.2)) # c4 = np.mean(get_counts(tree, 0.4)) # print(c2, c4) trimesh.PointCloud(positions).show() import matplotlib.pyplot as plt plt.hist(means) plt.show()
import numpy as np import trimesh from trimesh.transformations import transform_points colors = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]] vertices = colors scene = trimesh.scene.Scene() for v, c in zip(vertices, colors): sphere = trimesh.primitives.Sphere(center=v, radius=0.1) # add point clouds - primitives don't render well vertex_colors = np.tile( np.expand_dims(c, axis=0) * 255, (sphere.vertices.shape[0], 1)) pc = trimesh.PointCloud(sphere.vertices, colors=vertex_colors) scene.add_geometry(pc) scene.camera._resolution = (640, 480) print('# # camera resoltuion \n', scene.camera._resolution) camera = scene.camera transform = camera.transform print("\n# # Camera extrinsic") print(transform) K = camera.K print("\n# # Camera intrinsic") print(K) scene.show() # render scene from io import BytesIO img = scene.save_image(resolution=(640, 480)) from PIL import Image rendered = Image.open(BytesIO(img)).convert("RGB")
def main(): parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument( '--input-dir', '-i', type=str, help='input urdf', default='/media/kosuke55/SANDISK-2/meshdata/ycb_eval/019_pitcher_base/pocky-2020-10-17-06-01-16-481902-45682') # noqa parser.add_argument( '--idx', type=int, help='data idx', default=0) parser.add_argument( '--large-axis', '-la', action='store_true', help='use large axis as visualizing marker') args = parser.parse_args() base_dir = args.input_dir start_idx = args.idx viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) for idx in range(start_idx, 100000): print(idx) if idx != start_idx: viewer.delete(pc) # noqa for c in contact_point_marker_list: # noqa viewer.delete(c) annotation_path = osp.join( base_dir, 'annotation', '{:06}.json'.format(idx)) annotation_data = load_json(annotation_path) color_path = osp.join(base_dir, 'color', '{:06}.png'.format(idx)) color = o3d.io.read_image(color_path) depth_path = osp.join(base_dir, 'depth', '{:06}.npy'.format(idx)) depth = np.load(depth_path) depth = o3d.geometry.Image(depth) camera_info_path = osp.join( base_dir, 'camera_info', '{:06}.yaml'.format(idx)) cameramodel = cameramodels.PinholeCameraModel.from_yaml_file( camera_info_path) intrinsics = cameramodel.open3d_intrinsic rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False) pcd = o3d.geometry.PointCloud.create_from_rgbd_image( rgbd, intrinsics) contact_point_marker_list = [] # for manual annotaion which have labels if 'label' in annotation_data[0]: labels = [a['label'] for a in annotation_data] color_map = label_colormap(max(labels) + 1) for annotation in annotation_data: cx = annotation['xy'][0] cy = annotation['xy'][1] q = np.array(annotation['quaternion']) dep = annotation['depth'] color = [255, 0, 0] if 'label' in annotation: label = annotation['label'] color = color_map[label] print(cx, cy) pos = np.array( cameramodel.project_pixel_to_3d_ray([cx, cy])) length = dep * 0.001 / pos[2] pos = pos * length if args.large_axis: contact_point_marker = skrobot.model.Axis(0.003, 0.05) else: contact_point_marker = skrobot.model.Sphere(0.003, color=color) contact_point_marker.newcoords( skrobot.coordinates.Coordinates(pos=pos, rot=q)) viewer.add(contact_point_marker) contact_point_marker_list.append(contact_point_marker) trimesh_pc = trimesh.PointCloud( np.asarray( pcd.points), np.asarray( pcd.colors)) pc = skrobot.model.PointCloudLink(trimesh_pc) viewer.add(pc) if idx == start_idx: viewer.show() input('Next data?: [ENTER]')
pcd_path = pcd_paths[0] for pcd_path in pcd_paths: pcd = o3d.io.read_point_cloud(str(pcd_path)) paths = list(sorted(Path('./') .glob('*.json'))) pose_path = pcd_path.with_suffix('.json') contact_points_dict = load_json(str(pose_path)) contact_points = contact_points_dict['contact_points'] contact_point_sphere_list = [] for i, cp in enumerate(contact_points): contact_point_sphere = skrobot.model.Axis(0.003, 0.05) contact_point_sphere.newcoords( skrobot.coordinates.Coordinates(pos=cp[0], rot=cp[1:])) contact_point_sphere_list.append(contact_point_sphere) viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 640)) trimesh_pc = trimesh.PointCloud( np.asarray(pcd.points), np.asarray(pcd.colors)) pc = skrobot.model.PointCloudLink(trimesh_pc) viewer.add(pc) for contact_point_sphere in contact_point_sphere_list: viewer.add(contact_point_sphere) print('Press [q] on the window to move the next object.\n' + 'Press [ctrl + c] on the command line to finish.') viewer._init_and_start_app()
scenes = {} for is_solid in ["nonsolid", "solid"]: name = f"{is_solid}" if is_solid == "nonsolid": points = models.get_pcd(class_id=2) else: assert is_solid == "solid" points = models.get_solid_voxel_grid(class_id=2).points points = morefusion.extra.open3d.voxel_down_sample(points, voxel_size=0.01) points = points.astype(np.float32) geom = trimesh.PointCloud(vertices=points) scene.add_geometry(geom) dim = 16 pitch = max(geom.extents) / dim * 1.1 origin = (-pitch * dim / 2, ) * 3 sdf = models.get_cad(class_id=2).nearest.signed_distance(points) print(f"[{name}] pitch: {pitch}") print(f"[{name}] dim: {dim}") grid, _, _ = morefusion.functions.pseudo_occupancy_voxelization( points=cuda.to_gpu(points), sdf=cuda.to_gpu(sdf), pitch=pitch, origin=origin, dims=(dim, ) * 3, threshold=2,
def main(): current_dir = osp.dirname(osp.abspath(__file__)) trained_model_dir = osp.join(osp.dirname(osp.dirname(current_dir)), 'pretrained_model') parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--input-dir', '-i', type=str, help='input directory', default=None) parser.add_argument('--color', '-c', type=str, help='color image (.png)', default=None) parser.add_argument('--depth', '-d', type=str, help='depth image (.npy)', default=None) parser.add_argument('--camera-info', '-ci', type=str, help='camera info file (.yaml)', default=None) parser.add_argument('--pretrained_model', '-p', type=str, help='Pretrained models', default=osp.join(trained_model_dir, 'hanging_1020.pt')) # default=osp.join(trained_model_dir, 'pouring.pt')) # default='/media/kosuke55/SANDISK-2/meshdata/random_shape_shapenet_hanging_render/1010/gan_2000per0-1000obj_1020.pt') # gan hanging # noqa # default='/media/kosuke55/SANDISK-2/meshdata/random_shape_shapenet_pouring_render/1227/pouring_random_20201230_0215_5epoch.pt') # gan pouring # noqa parser.add_argument('--predict-depth', '-pd', type=int, help='predict-depth', default=0) parser.add_argument('--task', '-t', type=str, help='h(hanging) or p(pouring)' 'Not needed if roi size is the same in config.', default='h') args = parser.parse_args() base_dir = args.input_dir pretrained_model = args.pretrained_model config_path = str( Path(osp.abspath(__file__)).parent.parent / 'learning_scripts' / 'config' / 'gray_model.yaml') with open(config_path) as f: config = yaml.safe_load(f) task_type = args.task if task_type == 'p': task_type = 'pouring' else: task_type = 'hanging' target_size = tuple(config['target_size']) depth_range = config['depth_range'] depth_roi_size = config['depth_roi_size'][task_type] print('task type: {}'.format(task_type)) print('depth roi size: {}'.format(depth_roi_size)) device = torch.device('cuda' if torch.cuda.is_available() else 'cpu') model = HPNET(config).to(device) model.load_state_dict(torch.load(pretrained_model), strict=False) model.eval() viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480)) if base_dir is not None: color_paths = list(Path(base_dir).glob('**/color/*.png')) elif args.color is not None \ and args.depth is not None \ and args.camera_info is not None: color_paths = [args.color] else: return False is_first_loop = True try: for color_path in color_paths: if not is_first_loop: viewer.delete(pc) # noqa for c in contact_point_sphere_list: # noqa viewer.delete(c) if base_dir is not None: camera_info_path = str( (color_path.parent.parent / 'camera_info' / color_path.stem).with_suffix('.yaml')) depth_path = str((color_path.parent.parent / 'depth' / color_path.stem).with_suffix('.npy')) color_path = str(color_path) else: camera_info_path = args.camera_info color_path = args.color depth_path = args.depth camera_model = cameramodels.PinholeCameraModel.from_yaml_file( camera_info_path) camera_model.target_size = target_size cv_bgr = cv2.imread(color_path) intrinsics = camera_model.open3d_intrinsic cv_bgr = cv2.resize(cv_bgr, target_size) cv_rgb = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2RGB) color = o3d.geometry.Image(cv_rgb) cv_depth = np.load(depth_path) cv_depth = cv2.resize(cv_depth, target_size, interpolation=cv2.INTER_NEAREST) depth = o3d.geometry.Image(cv_depth) rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( color, depth, depth_trunc=4.0, convert_rgb_to_intensity=False) pcd = o3d.geometry.PointCloud.create_from_rgbd_image( rgbd, intrinsics) trimesh_pc = trimesh.PointCloud(np.asarray(pcd.points), np.asarray(pcd.colors)) pc = skrobot.model.PointCloudLink(trimesh_pc) viewer.add(pc) if config['use_bgr2gray']: gray = cv2.cvtColor(cv_bgr, cv2.COLOR_BGR2GRAY) gray = cv2.resize(gray, target_size)[..., None] / 255. normalized_depth = normalize_depth(cv_depth, depth_range[0], depth_range[1])[..., None] in_feature = np.concatenate((normalized_depth, gray), axis=2).astype(np.float32) else: raise NotImplementedError() transform = transforms.Compose([transforms.ToTensor()]) in_feature = transform(in_feature) in_feature = in_feature.to(device) in_feature = in_feature.unsqueeze(0) confidence, depth, rotation = model(in_feature) confidence = confidence[0, 0:1, ...] confidence_np = confidence.cpu().detach().numpy().copy() * 255 confidence_np = confidence_np.transpose(1, 2, 0) confidence_np[confidence_np <= 0] = 0 confidence_np[confidence_np >= 255] = 255 confidence_img = confidence_np.astype(np.uint8) print(model.rois_list) contact_point_sphere_list = [] roi_image = cv_bgr.copy() for i, (roi, roi_center) in enumerate( zip(model.rois_list[0], model.rois_center_list[0])): if roi.tolist() == [0, 0, 0, 0]: continue roi = roi.cpu().detach().numpy().copy() roi_image = draw_roi(roi_image, roi) hanging_point_x = roi_center[0] hanging_point_y = roi_center[1] v = rotation[i].cpu().detach().numpy() v /= np.linalg.norm(v) rot = rotation_matrix_from_axis(v, [0, 1, 0], 'xy') q = matrix2quaternion(rot) hanging_point = np.array( camera_model.project_pixel_to_3d_ray( [int(hanging_point_x), int(hanging_point_y)])) if args.predict_depth: dep = depth[i].cpu().detach().numpy().copy() dep = unnormalize_depth(dep, depth_range[0], depth_range[1]) * 0.001 length = float(dep) / hanging_point[2] else: depth_roi = make_box(roi_center, width=depth_roi_size[1], height=depth_roi_size[0], img_shape=target_size, xywh=False) depth_roi_clip = cv_depth[depth_roi[0]:depth_roi[2], depth_roi[1]:depth_roi[3]] dep_roi_clip = depth_roi_clip[np.where( np.logical_and( config['depth_range'][0] < depth_roi_clip, depth_roi_clip < config['depth_range'][1]))] dep_roi_clip = np.median(dep_roi_clip) * 0.001 if dep_roi_clip == np.nan: continue length = float(dep_roi_clip) / hanging_point[2] hanging_point *= length contact_point_sphere = skrobot.model.Sphere(0.001, color=[255, 0, 0]) contact_point_sphere.newcoords( skrobot.coordinates.Coordinates(pos=hanging_point, rot=q)) viewer.add(contact_point_sphere) contact_point_sphere_list.append(contact_point_sphere) if is_first_loop: viewer.show() heatmap = overlay_heatmap(cv_bgr, confidence_img) cv2.imshow('heatmap', heatmap) cv2.imshow('roi', roi_image) print('Next data: [ENTER] on image window.\n' 'Quit: [q] on image window.') key = cv2.waitKey(0) cv2.destroyAllWindows() if key == ord('q'): break is_first_loop = False except KeyboardInterrupt: pass
def visualize_data(): data = contrib.get_data() models = morefusion.datasets.YCBVideoModels() colormap = imgviz.label_colormap() scenes = { "pcd": trimesh.Scene(), "grid_target": trimesh.Scene(), "grid_nontarget_empty": trimesh.Scene(), "cad": trimesh.Scene(), } rgb = data["rgb"] depth = data["depth"] K = data["intrinsic_matrix"] pcd = morefusion.geometry.pointcloud_from_depth(depth, fx=K[0, 0], fy=K[1, 1], cx=K[0, 2], cy=K[1, 2]) nonnan = ~np.isnan(depth) geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan]) scenes["pcd"].add_geometry(geom) # scenes["cad"].add_geometry(geom) T_world2cam = None for instance in data["instances"]: if T_world2cam is None: T_world2cam = np.linalg.inv(instance["T_cam2world"]) class_id = instance["class_id"] transform = instance["transform_init"] grid_target = instance["grid_target"] grid_nontarget_empty = instance["grid_nontarget_empty_noground"] cad = models.get_cad(class_id=class_id) # if hasattr(cad.visual, "to_color"): # cad.visual = cad.visual.to_color() scenes["pcd"].add_geometry( cad, node_name=str(instance["id"]), geom_name=str(instance["id"]), transform=transform, ) scenes["cad"].add_geometry( cad, node_name=str(instance["id"]), geom_name=str(instance["id"]), transform=transform, ) transform_vg = ttf.scale_and_translate(scale=instance["pitch"], translate=instance["origin"]) if instance["id"] == 0: color_id = 3 elif instance["id"] == 1: color_id = 1 elif instance["id"] == 2: color_id = 4 else: raise ValueError geom = trimesh.voxel.VoxelGrid( grid_target, transform=transform_vg).as_boxes(colors=colormap[color_id]) scenes["grid_target"].add_geometry(geom) points = np.argwhere(grid_nontarget_empty) points = points * instance["pitch"] + instance["origin"] geom = trimesh.PointCloud(vertices=points, colors=colormap[color_id]) # geom = trimesh.voxel.VoxelGrid( # grid_nontarget_empty, transform=transform_vg # ).as_boxes(colors=colormap[instance["id"] + 1]) scenes["grid_nontarget_empty"].add_geometry(geom) camera_transform = morefusion.extra.trimesh.to_opengl_transform() for scene in scenes.values(): # scene.add_geometry(contrib.grid(scale=0.1), transform=T_world2cam) scene.camera_transform = camera_transform return scenes
def visualize_grids( camera, camera_transform, rgb, pcd, pitch, origin, grid_target, grid_nontarget, grid_empty, ): scenes = {} scene_common = trimesh.Scene(camera=camera) # point_cloud nonnan = ~np.isnan(pcd).any(axis=2) geom = trimesh.PointCloud(vertices=pcd[nonnan], colors=rgb[nonnan]) scene_common.add_geometry(geom, geom_name="point_cloud") # grid_aabb dimensions = np.array(grid_target.shape) center = origin + dimensions / 2 * pitch geom = trimesh.path.creation.box_outline( extents=dimensions * pitch, transform=ttf.translation_matrix(center), ) scene_common.add_geometry(geom, geom_name="grid_aabb") # camera camera_marker = camera.copy() camera_marker.transform = morefusion.extra.trimesh.from_opengl_transform( camera_transform) geom = trimesh.creation.camera_marker(camera_marker, marker_height=0.1) scene_common.add_geometry(geom, geom_name="camera_marker") scenes["occupied"] = trimesh.Scene( camera=scene_common.camera, geometry=scene_common.geometry, camera_transform=camera_transform, ) # grid_target voxel = trimesh.voxel.VoxelGrid( grid_target, ttf.scale_and_translate(pitch, origin), ) geom = voxel.as_boxes(colors=(1.0, 0, 0, 0.5)) scenes["occupied"].add_geometry(geom, geom_name="grid_target") # grid_nontarget voxel = trimesh.voxel.VoxelGrid( grid_nontarget, ttf.scale_and_translate(pitch, origin), ) geom = voxel.as_boxes(colors=(0, 1.0, 0, 0.5)) scenes["occupied"].add_geometry(geom, geom_name="grid_nontarget") scenes["empty"] = trimesh.Scene( camera=scene_common.camera, geometry=scene_common.geometry, camera_transform=camera_transform, ) # grid_empty voxel = trimesh.voxel.VoxelGrid(grid_empty, ttf.scale_and_translate(pitch, origin)) geom = voxel.as_boxes(colors=(0.5, 0.5, 0.5, 0.5)) scenes["empty"].add_geometry(geom, geom_name="grid_empty") return scenes