def get_cross_section(verts, faces, z=0.1): #z = np.min(verts[:, -1]) + z # 0.5 is the height of husky, actually it should be 0.37 cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) return cross_section
def main(): fn = args.mesh_name verts, faces = load_obj(fn) z = np.min( verts[:, -1] ) + 0.5 # 0.5 is the height of husky, actually it should be 0.37 # cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector #print('verts: {}'.format(verts[0])) cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) for item in cross_section: for i in range(len(item) - 1): #print('item:{}', item[i, 0]) plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b') #print('item:{}'.format(item[i:i+2,0])) if args.trajFilePath: print('Process trajectory file: {}'.format(args.trajFilePath)) points = np.load(args.trajFilePath) for i in range(len(points) - 1): # infos is of structure [{'eye_pos':, 'eye_quat':, 'episode':}, {}] first = points[i] second = points[i + 1] plt.plot([first[0], second[0]], [first[1], second[1]], 'k') ## save image to the path if args.save_name is not None: print('save image to {}'.format(args.save_name)) plt.savefig(args.save_name, dpi=600) else: plt.show()
def slice(self, verts, faces, z, zout=None): block = Block("slice %f" % (float(z))) #FIXME: slice along different axes plane_orig = (0, 0, z) #z height to slice plane_norm = (0, 0, 1) #Crosscut contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm) #Flatten contours if zout is not None: for contour in contours: for segment in contour: segment[2] = zout #Contours to G-code for contour in contours: #print(contour) first = contour[0] block.append("g0 x%f y%f z%f" % (first[0], first[1], first[2])) for segment in contour: block.append("g1 x%f y%f z%f" % (segment[0], segment[1], segment[2])) block.append("g1 x%f y%f z%f" % (first[0], first[1], segment[2])) block.append("( ---------- cut-here ---------- )") if block: del block[-1] if not block: block = None return block
def main(): fn = args.meshFilePath verts, faces = load_obj(fn) z = np.min( verts[:, -1] ) + 0.5 # 0.5 is the height of husky, actually it should be 0.37 # cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector #print('verts: {}'.format(verts[0])) print('meshcut ...') cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) plt.figure() for item in cross_section: for i in range(len(item) - 1): #print('item:{}', item[i, 0]) plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b') #print('item:{}'.format(item[i:i+2,0])) plt.xticks(np.arange(-2, 10, 0.5)) plt.yticks(np.arange(-2, 9, 0.5)) plt.grid(True) ## save image to the path if args.save_name is not None: print('save image to {}'.format(args.save_name)) plt.savefig(args.save_name) else: plt.show()
def main(mesh_file_name): verts, faces = load_obj(mesh_file_name) z = np.min( verts[:, -1] ) + 0.5 # 0.5 is the height of husky, actually it should be 0.37 # cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector #print('verts: {}'.format(verts[0])) print('meshcut ...') cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) n_nodes = sum([len(x) for x in cross_section]) n_edges = n_nodes - len(cross_section) output_file_name = mesh_file_name.replace('.obj', '_2d.obj') with open(output_file_name, 'w') as output_file: output_file.write("# {} vertices, {} edges\n".format(n_nodes, n_edges)) for item in cross_section: for node in item: output_file.write("v {:0.6f} {:0.6f}\n".format( node[0], node[1])) i = 0 for item in cross_section: print("{}".format(len(item))) for _ in range(len(item) - 1): output_file.write("e {} {}\n".format(i, i + 1)) i += 1 i += 1
def main(): fn = args.meshFilePath verts, faces = load_obj(fn) z = np.min( verts[:, -1] ) + 0.5 # 0.5 is the height of husky, actually it should be 0.37 ''' # find better z, because the scene might have numbers of floors if args.trajFilePath: print('Process trajectory file: {}'.format(args.trajFilePath)) infos = np.load(args.trajFilePath) for i in range(len(infos)): first = infos[i]['eye_pos'] if z > first[2]: z = first[2] ''' # cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector #print('verts: {}'.format(verts[0])) print('meshcut ...') cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) plt.figure() for item in cross_section: for i in range(len(item) - 1): #print('item:{}', item[i, 0]) plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b') #print('item:{}'.format(item[i:i+2,0])) print('draw trajectory ...') if args.trajFilePath: print('Process trajectory file: {}'.format(args.trajFilePath)) infos = np.load(args.trajFilePath) for i in range(len(infos) - 1): # infos is of structure [{'eye_pos':, 'eye_quat':, 'episode':}, {}] first = infos[i]['eye_pos'] second = infos[i + 1]['eye_pos'] plt.plot([first[0], second[0]], [first[1], second[1]], 'k') # draw start point as yellow circle and end point as yellow star start_point = infos[0]['eye_pos'] end_point = infos[len(infos) - 1]['eye_pos'] plt.plot(start_point[0], start_point[1], color='r', marker='o') plt.plot(end_point[0], end_point[1], color='r', marker='*') #plt.show() #print('start_point: {}'.format(start_point)) #print('end_point: {}'.format(end_point)) ## draw target on image if args.targetX is not None and args.targetY is not None: print('draw target to image ...') plt.plot(args.targetX, args.targetY, color='y', marker='*') ## save image to the path plt.title('{}'.format(args.save_name[args.save_name.rfind('/') + 1:-4])) if args.save_name is not None: print('save image to {}'.format(args.save_name)) plt.savefig(args.save_name) else: plt.show()
def mesh(model_id="", episode=0, waypoint=False): C1 = '\033[91m' C1END = '\033[0m' print(C1 + "PLOTTING EPISODE:" + C1END) plt.style.use('default') # switches back to matplotlib style fn = os.path.join( os.path.expanduser("~"), "PycharmProjects/Gibson_Exercise/gibson/assets/dataset/") + str( model_id) + "/mesh_z_up.obj" verts, faces = load_obj(fn) z = np.min(verts[:, -1]) + 0.5 # your robot height cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) plt.figure(figsize=(16, 8)) plt.subplot(1, 2, 1) for item in cross_section: for i in range(len(item) - 1): plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'k') plt.title('Map of Navigation') plt.xlabel('X Position') plt.ylabel('Y Position') plt.grid(True) if waypoint: #df = pandas.read_csv('euharlee_waypoints_sort.csv') df = pandas.read_csv(WAY_PATH + str('euharlee_waypoints_clipped_sort.csv')) #df = pandas.read_csv('aloha_waypoints_clipped_sort.csv') points = df.values length = len(points) sp = np.zeros((length, 3)) ang = np.zeros((length, 1)) gp = np.zeros((length, 3)) complexity = np.zeros((length, 1)) for r in range(length): sp[r] = np.array([points[r][2], points[r][3], points[r][4]]) ang[r] = np.array([points[r][5]]) gp[r] = np.array([points[r][6], points[r][7], points[r][8]]) complexity[r] = np.array([points[r][10] / points[r][9]]) print("ModelID:", points[0][1]) plt.plot(sp[episode % len(sp) - 1][0], sp[episode % len(sp) - 1][1], 'r*') plt.plot(gp[episode % len(sp) - 1][0], gp[episode % len(sp) - 1][1], 'g*') print("(%i) Nav Complexity ---> %.3f" % (episode, complexity[episode % len(sp) - 1])) debug = 0 if debug: plt.show()
def get_zslice(z_ind): """Use meshcut to get z slices of mesh, create 2D masks of each slice, and combine slices to get 3D mask. :param z_ind: index of z slice mask to cut from mesh :return: 2D image as binary mask of z slice at this index """ im = Image.fromarray(np.zeros((imsize, imsize), np.uint8).T) if zmin < z_ind < zmax: cut = meshcut.cross_section(verts_shift, faces, [0, 0, z_ind], [0, 0, 1]) polygon = cut[0][:, :2] draw = ImageDraw.Draw(im) draw.polygon(polygon.round().astype(np.uint8).flatten().tolist(), fill=255) return np.array(im) / 255
def slice_with_plane(self, origin: np.ndarray, normal: np.ndarray) -> list: # Returns a list with the slices (in case we have a concave mesh) def intersect_edges(vertices_a: np.ndarray, vertices_b: np.ndarray) -> np.ndarray: # Returns a mask of all edges that *might* be sliced by the plane # Adapted from https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection epsilon = 1e-6 diff_dot_n = np.dot(origin - vertices_a, normal) l_dot_n = np.dot(vertices_b - vertices_a, normal) t = diff_dot_n / l_dot_n # Edge is completely contained in plane mask_full = abs(l_dot_n) < epsilon mask_full &= abs(diff_dot_n) < epsilon # One intersection mask_point = 0.0 <= t mask_point &= t <= 1.0 return mask_full | mask_point def preprocess_triangles(triangles: np.ndarray) -> np.ndarray: # Pre-processes the triangles by each of their edges mask_a = intersect_edges(triangles[0::3], triangles[1::3]) mask_b = intersect_edges(triangles[1::3], triangles[2::3]) mask_c = intersect_edges(triangles[2::3], triangles[0::3]) return mask_a | mask_b | mask_c mask = preprocess_triangles(self.triangles) try: return meshcut.cross_section(self.vertices, self.indices[mask], np.array(origin), np.array(normal)) except AssertionError: # Meshcut doesn't want to slice print( f'WARNING: Mesh {self.name} (id = {self.id}) cannot be sliced, fix your mesh!' ) return []
def slice(self, verts, faces, z, zout=None, axis='z'): tags = '[slice]' if axis == 'z': tags = '[slice,minz:%f]' % (float(z)) block = Block("slice %s%f %s" % (axis, float(z), tags)) #FIXME: slice along different axes if axis == 'x': plane_orig = (z, 0, 0) plane_norm = (1, 0, 0) elif axis == 'y': plane_orig = (0, z, 0) plane_norm = (0, 1, 0) else: plane_orig = (0, 0, z) #z height to slice plane_norm = (0, 0, 1) #Crosscut contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm) #Flatten contours if zout is not None: for contour in contours: for segment in contour: segment[2] = zout #Contours to G-code for contour in contours: #print(contour) gtype = 0 for segment in contour: block.append("g%s x%f y%f z%f" % (gtype, segment[0], segment[1], segment[2])) gtype = 1 block.append( "g1 x%f y%f z%f" % (contour[0][0], contour[0][1], contour[0][2])) #Close shape block.append("( ---------- cut-here ---------- )") if block: del block[-1] if not block: block = None return block
def gen_map(obj_filepath, mesh_dir, img_filename_format='floor_{}.png'): vertices, faces = load_obj_np(obj_filepath) xmin, ymin, _ = vertices.min(axis=0) xmax, ymax, _ = vertices.max(axis=0) max_length = np.max( [np.abs(xmin), np.abs(ymin), np.abs(xmax), np.abs(ymax)]) max_length = np.ceil(max_length).astype(np.int) with open(os.path.join(mesh_dir, 'floors.txt')) as f: floors = map(float, f.readlines()) floors = sorted(floors) print(floors) for i_floor, floor in enumerate(floors): z = float(floor) + 0.5 cross_section = meshcut.cross_section(vertices, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) floor_map = np.ones((2 * max_length * 100, 2 * max_length * 100)) for item in cross_section: for i in range(len(item) - 1): x1, x2 = (item[i:i + 2, 0] + max_length) * 100 y1, y2 = (item[i:i + 2, 1] + max_length) * 100 cv2.line(floor_map, (x1, y1), (x2, y2), color=(0, 0, 0), thickness=2) cur_img = Image.fromarray((floor_map * 255).astype(np.uint8)) #cur_img = Image.fromarray(np.flipud(cur_img)) img_filename = img_filename_format.format(i_floor) cur_img.save(os.path.join(mesh_dir, img_filename)) write_yaml(mesh_dir, np.array(cur_img), img_filename, 'floor_{}.yaml'.format(i_floor), resolution=0.01)
def slice(self, verts, faces, z, zout=None, axis='z'): tags = '[slice]' if axis=='z': tags = '[slice,minz:%f]'%(float(z)) block = Block("slice %s%f %s"%(axis,float(z),tags)) #FIXME: slice along different axes if axis == 'x': plane_orig = (z, 0, 0) plane_norm = (1, 0, 0) elif axis == 'y': plane_orig = (0, z, 0) plane_norm = (0, 1, 0) else: plane_orig = (0, 0, z) #z height to slice plane_norm = (0, 0, 1) #Crosscut contours = meshcut.cross_section(verts, faces, plane_orig, plane_norm) #Flatten contours if zout is not None: for contour in contours: for segment in contour: segment[2] = zout #Contours to G-code for contour in contours: #print(contour) gtype = 0 for segment in contour: block.append("g%s x%f y%f z%f"%(gtype, segment[0],segment[1],segment[2])) gtype = 1 block.append("g1 x%f y%f z%f"%(contour[0][0],contour[0][1],contour[0][2])) #Close shape block.append("( ---------- cut-here ---------- )") if block: del block[-1] if not block: block = None return block
def main(scene_idx): meshFilePath = 'gibson/assets/dataset/{}_for_rrt/mesh_z_up.obj'.format( Test_Scenes[scene_idx]) verts, faces = load_obj(meshFilePath) z = mapper_scene2z[Test_Scenes[ scene_idx]] # 0.5 is the height of husky, actually it should be 0.37 # cut the mesh with a surface whose value on z-axis is plane_orig, and its normal is plane_normal vector #print('verts: {}'.format(verts[0])) print('meshcut ...') cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) plt.figure() for item in cross_section: for i in range(len(item) - 1): #print('item:{}', item[i, 0]) plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'b') #print('item:{}'.format(item[i:i+2,0])) left_x, bottom_y, right_x, top_y = mapper_scene2xyticks[ Test_Scenes[scene_idx]] x_major_ticks = np.arange(left_x, right_x, 0.5) x_minor_ticks = np.arange(left_x, right_x, 0.5) y_major_ticks = np.arange(bottom_y, top_y, 0.5) y_minor_ticks = np.arange(bottom_y, top_y, 0.5) plt.xticks(x_major_ticks) #plt.xticks(x_minor_ticks, minor=True) plt.yticks(y_major_ticks) #plt.yticks(y_minor_ticks, minor=True) plt.grid(True) ## save image to the path file_addr = '/home/reza/Datasets/GibsonEnv/my_code/rrt/scene_grid_map' img_name = '{}_grid.png'.format(Test_Scenes[scene_idx]) plt.savefig('{}/{}'.format(file_addr, img_name), bbox_inches='tight', dpi=(400)) plt.close()
def line_map(superp, path, writing_path, multi_floor, object_map=False, draw=False, resolution=0.04): ''' generate the line map :param superp: how many superpostion for the line map :param path: reading and saving path :param draw: whether to display the map :param resolution: set default to be 0.4 :return: the significant parameters that influence the map ''' _, _, floor_height, robot_h, min_height = multi_floor fn = path + 'mesh_z_up.obj' verts, faces = load_obj(fn) # robot_h = np.min(verts[:, -1]) + 0.5 # your robot height #probably using this to optimize the algo plt.figure(0) x_max_list = [] y_max_list = [] x_min_list = [] y_min_list = [] cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, floor_height), plane_normal=(0, 0, 1)) for item in cross_section: #create the map x_max, y_max = np.max(item[:, :2], axis=0) # find the largest x and y x_min, y_min = np.min(item[:, :2], axis=0) # find the minimum x and y x_max_list.append(x_max) y_max_list.append(y_max) x_min_list.append(x_min) y_min_list.append(y_min) min_point = (np.min(x_min_list), np.min(y_min_list)) max_point = (np.max(x_max_list), np.max(y_max_list)) raw_map = np.zeros((int( (max_point[1] - min_point[1]) / resolution) + int(2 / resolution), int((max_point[0] - min_point[0]) / resolution) + int(2 / resolution))) #generate the map if object_map == True: rawmap_with_obj = raw_map.copy() z = min_height cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) for item in cross_section: for i in range(len(item) - 1): (x1, y1) = transform_real_to_cv(item[i, 0], item[i, 1], min_point, max_point, resolution) (x2, y2) = transform_real_to_cv(item[i + 1, 0], item[i + 1, 1], min_point, max_point, resolution) cv2.line(rawmap_with_obj, (x1, y1), (x2, y2), 150, 2) return z, rawmap_with_obj, max_point, min_point, resolution for i in range(superp): z = min_height + (i + 1) * ( robot_h - min_height ) / superp #make a croos section between floor height and robot_h cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) for item in cross_section: for i in range(len(item) - 1): (x1, y1) = transform_real_to_cv(item[i, 0], item[i, 1], min_point, max_point, resolution) (x2, y2) = transform_real_to_cv(item[i + 1, 0], item[i + 1, 1], min_point, max_point, resolution) cv2.line(raw_map, (x1, y1), (x2, y2), 150, 2) if draw == True: cv2.namedWindow('testwindow') cv2.imshow('testwindow', raw_map) cv2.waitKey(0) cv2.imwrite(writing_path + 'raw_map.png', raw_map) return robot_h, raw_map, max_point, min_point, resolution
def mesh(model_id="", waypoint=False): C1 = '\033[91m' C1END = '\033[0m' print(C1 + "PLOTTING EPISODE:" + C1END) plt.style.use('default') # switches back to matplotlib style fn = os.path.join( os.path.expanduser("~"), "PycharmProjects/Gibson_Exercise/gibson/assets/dataset/") + str( model_id) + "/mesh_z_up.obj" verts, faces = load_obj(fn) z = np.min(verts[:, -1]) + 0.5 # your robot height cross_section = meshcut.cross_section(verts, faces, plane_orig=(0, 0, z), plane_normal=(0, 0, 1)) plt.figure(figsize=(8, 8)) for item in cross_section: for i in range(len(item) - 1): plt.plot(item[i:i + 2, 0], item[i:i + 2, 1], 'k') plt.title('Map of Navigation') plt.xlabel('X Position') plt.ylabel('Y Position') plt.grid(True) if waypoint: #df = pandas.read_csv(WAY_PATH + str('aloha_waypoints_sort_test.csv')) #df = pandas.read_csv(WAY_PATH + str('aloha_waypoints_clipped_sort.csv')) df = pandas.read_csv(WAY_PATH + str('euharlee_waypoints_sort_test.csv')) #df = pandas.read_csv(WAY_PATH + str('euharlee_waypoints_clipped_sort.csv')) points = df.values length = len(points) sp = np.zeros((length, 3)) ang = np.zeros((length, 1)) gp = np.zeros((length, 3)) complexity = np.zeros((length, 1)) for r in range(length): sp[r] = np.array([points[r][2], points[r][3], points[r][4]]) ang[r] = np.array([points[r][5]]) gp[r] = np.array([points[r][6], points[r][7], points[r][8]]) complexity[r] = np.array([points[r][10] / points[r][9]]) for k in range(length): plt.plot(sp[k][0], sp[k][1], 'r*') plt.plot(gp[k][0], gp[k][1], 'g*') line = plt.plot([sp[k][0], gp[k][0]], [sp[k][1], gp[k][1]], color='dodgerblue', linewidth=1) m1 = (sp[k][0] + gp[k][0]) / 2 m2 = (sp[k][1] + gp[k][1]) / 2 plt.annotate(s='', xy=(gp[k][0], gp[k][1]), xytext=(sp[k][0], sp[k][1]), arrowprops=dict(arrowstyle='->', color='grey')) #plt.arrow([sp[k][0], gp[k][0]], [sp[k][1], gp[k][1]], [dx],[dy], shape='full', lw=0, length_includes_head=True, head_width=.05) print("%i Waypoint Navigation Complexity ---> %.3f" % (k + 1, complexity[k])) debug = 1 if debug: plt.savefig(os.path.join(SAVE_PATH + 'waypoints_map_test.png')) #plt.savefig(os.path.join(SAVE_PATH + 'waypoints_map.png')) plt.show()