def draw_quad_3d(camera, ax, frame_color="k", prop_color=None, opt_options=None, fill_alpha=0.5): if opt_options is not None: r_prop = camera.prop_rad frame_size = camera.frame_rad*2 else: r_prop = 0.1 frame_size = 0.4 if prop_color is None: prop_color = frame_color pos = camera.pose norm = camera.wall_normal pitch = np.arccos(norm[2]) if np.abs(pitch % 2*np.pi) <= 0.00001: warnings.warn("Normal Vector: " + str(norm) + " results in an invalid conversion to euler angles") yaw = 0 # purely cosmetic, but could be nice to deal with this... else: yaw = np.arccos((norm[0] / np.sin(pitch))) roll = 0 R = rot3d_from_rtp(np.array([roll, pitch, yaw]), unit="radians") # draw frame frame_verts = [ frame_size / 2 * np.array([[np.sqrt(2) / 2, np.sqrt(2) / 2, 0], [-np.sqrt(2) / 2, -np.sqrt(2) / 2, 0]]), frame_size / 2 * np.array([[np.sqrt(2) / 2, -np.sqrt(2) / 2, 0], [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0]])] for i in range(2): for j in range(2): frame_verts[i][j, :] = np.dot(R, frame_verts[i][j, :]) + pos[:3] ax.plot(frame_verts[0][:, 0], frame_verts[0][:, 1], frame_verts[0][:, 2], frame_color) ax.plot(frame_verts[1][:, 0], frame_verts[1][:, 1], frame_verts[1][:, 2], frame_color) # draw props R = r_prop # np.linspace(0, r_prop, 2) u = np.linspace(0, 2 * np.pi, 8) x = np.squeeze(np.outer(R, np.cos(u))) y = np.squeeze(np.outer(R, np.sin(u))) z = np.zeros(x.size) prop_coords = np.array([x, y, z]).transpose() prop_coords = np.vstack((prop_coords, prop_coords[0, :])) # rotate! for p in range(len(prop_coords)): prop_coords[p, :] = np.dot(R, prop_coords[p, :]) # shift and plot for i in range(2): for j in range(2): pc = prop_coords + frame_verts[i][j, :] # fill polygon collection = convert_points_to_Poly3D(pc, line_width=0.0, alpha=fill_alpha) collection.set_edgecolor(prop_color) collection.set_facecolor(prop_color) ax.add_collection3d(collection) ax.plot(pc[:, 0], pc[:, 1], pc[:, 2], "black") # for testing
def map_line_to_edge(environment, x, poly_order="CCW"): """ DEPRECATED Maps a particle weight into a position in 2D space, by mapping the distance along the perimeter edge :param environment: Environment class instance :param x: particle weight :param poly_order: polygon point order (clockwise ("CW") or counter clockwise ("CCW"). determines sign of normal direction offset :return: point position, p, and normal direction euler angles, n """ p = np.array([-np.inf, -np.inf]) cumulative_dist = [0] perimeter = environment.perch_perimeter poly_bounds = environment.perch_regions x = x % perimeter n = -np.inf # initialize normal direction as inf as a debugging precaution while n == -np.inf: for pb in poly_bounds: d = dist(pb[0, :], pb[1, :]) cumulative_dist.append(cumulative_dist[-1] + d) if cumulative_dist[-1] >= x: # find intersection point dist_on_edge = x - cumulative_dist[-2] norm_dist = dist_on_edge / d p[0] = norm_dist * (pb[0, 0] - pb[1, 0]) + pb[1, 0] p[1] = norm_dist * (pb[0, 1] - pb[1, 1]) + pb[1, 1] # get normal direction as an angle WRT the +x direction edge_dir = (np.arctan2(pb[1, 1] - pb[0, 1], pb[1, 0] - pb[0, 0])) * 180 / np.pi if poly_order == "CCW": n = edge_dir - 90 # normal direction to the edge else: n = edge_dir + 90 break if n == -np.inf: print("Warning: invalid edge normal") print(x) print(cumulative_dist) R = rot3d_from_rtp(np.array([0, 0, n])) n = R[:, 0] return p, n
def draw_camera_3d(camera, ax, color='c', tgt_steps=4, fov_steps=7, use_score_range=False, environment=None): """ Draws a camera's FOV in 3D :param camera: Camera class instance containing camera parameters and pose :param ax: figure ax handle :param color: color code for the camera :param tgt_steps: number of steps over which to approximate opacity (corresponding to information density) change of camera :param fov_steps: number of steps in both horizontal and vertical fov directions. used to approximate camera view being limited by obstacles. Definitely more computationally expensive to have this enabled. :param use_score_range: (defaults to false) determines whether the camera's score range should be used over the default opacity values (which make the visualization a bit cleaner). :param environment: class instance containing environment information. if this parameter is left empty, then the naive visualization mode will be used (fov_steps=1, range is left unlimited) :return: """ d_to_floor = camera.pose[2] # assumes target is on the ground. d_to_ceil = 2.7 - camera.pose[2] if environment is None: fov_steps = 1 obstacles = None mesh_env = False else: obstacles = environment.obstacles mesh_env = environment.opt_options.mesh_env if environment.floor is not None: d_to_floor = np.dot(camera.pose[:3] - environment.floor.centroid, -environment.gravity_direction) if environment.ceil is not None: d_to_ceil = np.dot(camera.pose[:3] - environment.floor.centroid, -environment.gravity_direction) r_steps = np.linspace(camera.range[0], camera.range[1], tgt_steps+1) if use_score_range: a_steps = np.linspace(camera.score_range[0], camera.score_range[1], tgt_steps) else: a_steps = np.linspace(0.6, 0.2, tgt_steps) for h in range(fov_steps): for v in range(fov_steps): # see if index is on one of the pyramid edges. if not, only draw end cap draw_walls = np.array([False, False, False, False, True, True]) if h == 0: # left side draw_walls[0] = True if h == fov_steps-1: # right side draw_walls[2] = True if v == 0: # bottom side draw_walls[3] = True if v == fov_steps-1: # top side draw_walls[1] = True # check intersection of center of sub-pyramid sub_fov = camera.fov/(fov_steps) # TODO: make sure things are oriented appropriately. in plot pyramid, things seem to be rotated 90 # RECALL: CAMERA FWD IS Z, RIGHT IS X, DOWN IS Y; ROLL IS Z, PITCH IS X, YAW IS Y # TODO: VERIFY # reorient camera euler angles to match drawing function's orientation # (from X right, Y down, Z fwd -> X fwd, Y left, Z up) cam_pose = np.array([camera.pose[-1], -camera.pose[-3], -camera.pose[-2]]) # TODO VERIFY SIGN CHANGE R_sub = rot3d_from_rtp(cam_pose + # start with initial camera pose np.array([0, (v+.5)*sub_fov[1], (h+.5)*sub_fov[0]]) - # add on the sub_pose # deviation (plus a half step to get to the center of the step position) np.array([0, camera.fov[1]/2, camera.fov[0]/2])) # and subtract off half the fov # R_sub = rot3d_from_rtp(camera.pose[3:] + # start with initial camera pose # np.array([0, (v+.5)*sub_fov[1], (h+.5)*sub_fov[0]]) - # add on the sub_pose # # deviation (plus a half step to get to the center of the step position) # np.array([0, camera.fov[1]/2, camera.fov[0]/2])) # and subtract off half the fov # R_sub = rot3d_from_rtp(-camera.pose[3:] + # start with initial camera pose # np.array([(v + .5) * sub_fov[1], (h + .5) * sub_fov[0], 0]) - # add on the sub_pose # # deviation (plus a half step to get to the center of the step position) # np.array([camera.fov[1]/2, camera.fov[0]/2, 0])) # and subtract off half the fov # find intersection... vec_dir = R_sub[:, 0] if vec_dir[-1] > 0: # pointing upwards. look at ceiling intersection d = np.abs(d_to_ceil / vec_dir[-1]) # length of vector to ground in direction defined by R_sub else: # pointing downwards, look at floor intersection d = np.abs(d_to_floor / vec_dir[-1]) # length of vector to ground in direction defined by R_sub d = min([camera.range[-1], d]) # take min of the ground intersection distance and the camera range e = d*vec_dir + camera.pose[:3] # point representing the end of the field of view of the current sub-view if mesh_env: # obstructed, obstruction_point, _ = camera.get_mesh_obstruction(point=e, # clusters_remeshed= # environment.cluster_env_remeshed, # opt_options=environment.opt_options, # tree=environment.tree, # find_closest_intersection=True) obstructed, obstruction_point, _ = camera.get_obstruction_mesh_obb_tree(points=np.array([e]), environment=environment, find_closest_intersection=True) if obstructed[0]: d = dist(obstruction_point[0], camera.pose[:3]) elif obstacles is not None: obstructed, obstruction_point = camera.get_obstruction(e, obstacles) if obstructed: d = dist(obstruction_point, camera.pose[:3]) for i in range(tgt_steps): d_range = np.copy(r_steps[i:i+2]) # Reset front and back draw_walls[4] = False draw_walls[5] = False if i == 0: # draw the front surface on the starting point draw_walls[4] = True if d_range[0] >= d: break elif d_range[1] > d: d_range[-1] = d # if obstructed, or if it's the last step, draw the back wall draw_walls[5] = True elif i == tgt_steps-1: draw_walls[5] = True if any(draw_walls): draw_pyramid_3d(position=camera.pose[:3], R=R_sub, fov=sub_fov, height_range=d_range, ax=ax, color=color, alpha=a_steps[i], draw_walls=draw_walls)
def optimize(seg_env_prototype=None, target_prototype=None, cluster_env_path=None, full_env_path=None, N_its=1, enable_user_confirmation=True, preloaded_vars=None, visualize_with_vedo=False): if preloaded_vars is None: env, camera_list, optimization_options, pso_options = initialize( seg_env_prototype, target_prototype, cluster_env_path, full_env_path, load_full_env=True) if enable_user_confirmation: # surface_confirmation.confirm_surfaces(environment=env, N_max=10) surface_confirmation_demo.confirm_surfaces(environment=env, N_max=10) env.post_process_environment() preloaded_vars = { 'env': copy.deepcopy(env), 'camera_list': copy.deepcopy(camera_list), 'optimization_options': copy.deepcopy(optimization_options), 'pso_options': copy.deepcopy(pso_options) } # SAVE THIS! pickle_env = open("../test/preloaded_environment_apartment.p", 'wb') pickle.dump(preloaded_vars, pickle_env) pickle_env.close() else: # work around to the seg fault issue... load everything in advance, then just pass it in! env = preloaded_vars['env'] camera_list = initialize_camera_list() optimization_options = initialize_opt_options() pso_options = initialize_pso_options() env.vedo_mesh = vedo.mesh.Mesh(env.obs_mesh) env.opt_options = optimization_options env.correct_normals() env.n_points = optimization_options.n_points env.generate_integration_points() env.perch_regions = [] env.perch_area = 0 env.set_surface_as_perchable() optimization_options.log_performance = False # env.plot_environment() # PSO: # base dimension for simple 2d problem is 2. scales up depending on selected opt camera_particle_dimension = optimization_options.get_particle_size() n_cams = len(camera_list) N_iterations = pso_options["N_iterations"] N_particles = pso_options["N_particles"] if pso_options["greedy_search"]: particle_dimension = camera_particle_dimension num_optimizations = n_cams else: particle_dimension = n_cams * camera_particle_dimension num_optimizations = 1 # for logging pso_keypoints = np.zeros([N_its, num_optimizations, N_iterations, 3]) optimization_options.search_time = np.zeros( [N_its, num_optimizations, N_iterations + 1]) optimization_options.best_fitness = np.zeros( [N_its, num_optimizations, N_iterations + 1]) optimization_options.pts_searched = np.zeros( [N_its, num_optimizations, N_iterations + 1]) bounds = (np.zeros(particle_dimension), np.ones(particle_dimension) ) # particle boundaries # for velocity update: # 'w' is velocity decay aka inertia, # 'c1' is "cognitive parameter", e.g. attraction to particle best, # 'c2' is "social parameter", e.g. attraction to local/global best # 'k' = number of neighbors to consider # 'p' = the Minkowski p-norm. 1 for absolute val dist, 2 for norm dist options = { 'c1': pso_options["pso_c1"], 'c2': pso_options["pso_c2"], 'w': pso_options["pso_w"], 'k': pso_options["pso_k"], 'p': pso_options["pso_p"] } optimal_cameras = PlacedCameras() fig_num = 1 if pso_options['individual_surface_opt']: num_surface_loops = len(env.perch_regions) surface_number = range(num_surface_loops) N_particles = env.assign_particles_to_surfaces( N_particles, pso_options["pso_k"], neighborhood_search=pso_options["local_search"]) else: num_surface_loops = 1 surface_number = [-1] N_particles = [N_particles] # STORE FOR ANALYSIS LATER if optimization_options.log_performance: pso_best_fitnesses = np.zeros(num_optimizations) pso_points_searched = np.zeros(num_optimizations) pso_search_time = np.zeros(num_optimizations) pso_keypoints = [] for i in range(num_optimizations): pso_keypoints.append([[], [], [], []]) # store, for each optimization, time, num particles searched, fitness, standard deviation bh = pso_options["boundary_handling"] # for i in range(num_optimizations): i = 0 while i < num_optimizations: if optimization_options.log_performance: optimization_options.data_index = 0 start_time = datetime.now() if pso_options["greedy_search"]: search_cameras_list = [copy.deepcopy(camera_list[i])] else: search_cameras_list = camera_list best_cost = np.finfo(float).max best_pos_surf = -1 best_pos = np.zeros(particle_dimension) for j in range(num_surface_loops): # Future work: investigate velocity clamping... optimization_options.surface_number = j # if pso_options["local_search"]: # optimizer = ps.single.LocalBestPSO(n_particles=N_particles[j], dimensions=particle_dimension, # options=options, bounds=bounds, bh_strategy=bh) # else: # optimizer = ps.single.GlobalBestPSO(n_particles=N_particles[j], dimensions=particle_dimension, # options=options, bounds=bounds, bh_strategy=bh) optimization_options.surface_number = surface_number[j] # this flag gets reset if the current search has too little variance optimization_options.continue_searching = True optimization_options.stagnant_loops = 0 if visualize_with_vedo: plt1 = vedo.Plotter(title='Confirm Perch Location', pos=[0, 0], interactive=False, sharecam=False) plt1.clear() # draw wireframe lineset of camera frustum # env_mesh = trimesh.load(env.full_env_path) env_mesh = trimesh.load( '/home/simon/catkin_ws/src/mesh_partition/datasets/' + env.name + '_1m_pt1.ply') R = np.zeros([4, 4]) R[:3, :3] = env.R env_mesh.vertices = trimesh.transform_points( env_mesh.vertices, R) env_mesh_vedo = vedo.mesh.Mesh(env_mesh) target_mesh_pymesh = env.generate_target_mesh(shape='box') target_mesh = trimesh.Trimesh(target_mesh_pymesh.vertices, target_mesh_pymesh.faces) target_mesh_vedo = vedo.mesh.Mesh(target_mesh) target_colors = 0.5 * np.ones([len(target_mesh.faces), 4]) target_colors[:, 0] *= 0 target_colors[:, 2] *= 0 target_mesh_vedo.alpha(0.6) target_mesh_vedo.cellIndividualColors(target_colors, alphaPerCell=True) env_mesh.visual.face_colors[:, -1] = 255 env_mesh_vedo.cellIndividualColors( env_mesh.visual.face_colors / 255, alphaPerCell=True) geom_list = [env_mesh_vedo, target_mesh_vedo] if env.name == 'office3' or env.name == 'apartment': env_mesh2 = trimesh.load( '/home/simon/catkin_ws/src/mesh_partition/datasets/' + env.name + '_1m_pt2.ply') env_mesh_vedo2 = vedo.mesh.Mesh(env_mesh2) env_mesh2.visual.face_colors[:, -1] = 150 env_mesh_vedo2.cellIndividualColors( env_mesh2.visual.face_colors / 255, alphaPerCell=True) geom_list.append(env_mesh_vedo2) for s in env.perch_regions: surf_mesh = trimesh.Trimesh(vertices=s.points, faces=s.faces) vedo_surf_mesh = vedo.mesh.Mesh(surf_mesh) vedo_surf_mesh.color('g') vedo_surf_mesh.opacity(0.7) geom_list.append(vedo_surf_mesh) for i_ in range(len(optimal_cameras.cameras)): quad_mesh = trimesh.load( "/home/simon/catkin_ws/src/perch_placement/src/ui/models/white-red-black_quad2.ply" ) R = rot3d_from_x_vec( optimal_cameras.cameras[i_].wall_normal) R2 = rot3d_from_rtp(np.array([0, -90, 0])) R_aug = np.zeros([4, 4]) R_aug[:3, :3] = R.dot(R2) R_aug[:3, -1] = optimal_cameras.cameras[i_].pose[:3] quad_mesh.vertices = trimesh.transform_points( quad_mesh.vertices, R_aug) quad_mesh_vedo = vedo.mesh.Mesh(quad_mesh) quad_mesh_vedo.cellIndividualColors( quad_mesh.visual.face_colors / 255, alphaPerCell=True) pymesh_frustum = optimal_cameras.cameras[ i_].generate_discrete_camera_mesh(degrees_per_step=20, environment=env) pymesh_verts = pymesh_frustum.vertices.copy() pymesh_verts.flags.writeable = True pymesh_faces = pymesh_frustum.faces.copy() pymesh_faces.flags.writeable = True frustum = trimesh.Trimesh( vertices=pymesh_frustum.vertices.copy(), faces=pymesh_frustum.faces.copy()) vedo_frustum = vedo.mesh.Mesh(frustum) vedo_frustum.alpha(0.3) vedo_frustum.color("b") quad_mesh_vedo.color('o') geom_list.append(quad_mesh_vedo) geom_list.append(vedo_frustum) for actor in geom_list: plt1.add(actor) else: plt1 = None # if pso_options["multi_threading"]: # # noinspection PyTypeChecker # surf_best_cost, surf_best_pos = optimizer.optimize(evaluate_swarm, iters=N_iterations, environment=env, # cameras=search_cameras_list, # placed_cameras=optimal_cameras, # opt_options=optimization_options, # n_processes=multiprocessing.cpu_count(), # vedo_plt=plt1) # else: # surf_best_cost, surf_best_pos = optimizer.optimize(evaluate_swarm, iters=N_iterations, environment=env, # cameras=search_cameras_list, # placed_cameras=optimal_cameras, # opt_options=optimization_options, # vedo_plt=plt1) pso_params = PSO_Hyperparameters(w=pso_options["pso_w"], c1=pso_options["pso_c1"], c2=pso_options["pso_c2"], lr=1, k=pso_options["pso_k"], p=pso_options["pso_p"], N_particles=N_particles[j], N_iterations=N_iterations) surf_best_cost, surf_best_pos = run_pso( fitness_function=evaluate_swarm, pso_hyper_parameters=pso_params, environment=env, cameras=search_cameras_list, placed_cameras=optimal_cameras, opt_options=optimization_options, local_pso=True) if surf_best_cost < best_cost: best_cost = copy.deepcopy(surf_best_cost) best_pos = copy.deepcopy(surf_best_pos) if pso_options["individual_surface_opt"]: best_pos_surf = j # print("Surface " + str(j) + " has lowest cost so far.. ") # print("Particle: " + str(best_pos)) if optimization_options.log_performance: pso_search_time[i] = (datetime.now() - start_time).total_seconds() pso_best_fitnesses[i] = best_cost if pso_options["greedy_search"]: search_cameras_list_copy = copy.deepcopy(search_cameras_list) optimization_options.surface_number = best_pos_surf best_cam = convert_particle_to_state( environment=env, particle=best_pos, cameras=search_cameras_list_copy, opt_options=optimization_options)[0] optimal_cameras.cameras.append(copy.deepcopy(best_cam)) if enable_user_confirmation: if confirm_perch_placement( environment=env, placed_cameras=optimal_cameras.cameras, focus_id=i): best_cam_covariances = evaluate_camera_covariance( environment=env, cameras=[best_cam]) optimal_cameras.append_covariances(best_cam_covariances) i += 1 else: optimal_cameras.cameras.pop() env.remove_rejected_from_perch_space(camera=best_cam, r=0.3) else: best_cam_covariances = evaluate_camera_covariance( environment=env, cameras=[best_cam]) optimal_cameras.append_covariances(best_cam_covariances) i += 1 evaluate_discrete_coverage(env.n_points, optimal_cameras, plot=True) return optimal_cameras.cameras
def confirm_perch_placement(environment, placed_cameras, focus_id): global approved global user_responded plt1.keyPressFunction = perch_keyfunc plt1.clear() approved = False plt2.clear() dense_env = vedo.load( '/home/simon/catkin_ws/src/mesh_partition/datasets/' + environment.name + '_1m.ply') pv_dense_env = pv.read( '/home/simon/catkin_ws/src/mesh_partition/datasets/' + environment.name + '_1m.ply') plt2.add(dense_env) plt2.show() plt3 = pv.Plotter(notebook=False) plt3.add_mesh(pv_dense_env) # plt3.show(interactive=False) # draw wireframe lineset of camera frustum env_mesh = trimesh.load( '/home/simon/catkin_ws/src/mesh_partition/datasets/' + environment.name + '_1m.ply') # env_mesh = trimesh.load(environment.full_env_path) R = np.zeros([4, 4]) R[:3, :3] = environment.R env_mesh.vertices = trimesh.transform_points(env_mesh.vertices, R) env_mesh_vedo = vedo.mesh.Mesh(env_mesh) target_mesh_pymesh = environment.generate_target_mesh(shape='box') target_mesh = trimesh.Trimesh(target_mesh_pymesh.vertices, target_mesh_pymesh.faces) target_mesh_vedo = vedo.mesh.Mesh(target_mesh) target_colors = 0.5 * np.ones([len(target_mesh.faces), 4]) target_colors[:, 0] *= 0.0 target_colors[:, 2] *= 0.0 target_mesh_vedo.alpha(0.6) target_mesh_vedo.cellIndividualColors(target_colors, alphaPerCell=True) plt2.add(target_mesh_vedo) env_mesh.visual.face_colors[:, -1] = 255.0 env_mesh_vedo.cellIndividualColors(env_mesh.visual.face_colors / 255.0, alphaPerCell=True) geom_list = [env_mesh_vedo, target_mesh_vedo] for s in environment.perch_regions: surf_mesh = trimesh.Trimesh(vertices=s.points, faces=s.faces) vedo_surf_mesh = vedo.mesh.Mesh(surf_mesh) vedo_surf_mesh.color('g') vedo_surf_mesh.opacity(0.5) geom_list.append(vedo_surf_mesh) for i in range(len(placed_cameras)): quad_mesh = trimesh.load( "/home/simon/catkin_ws/src/perch_placement/src/ui/models/white-red-black_quad2.ply" ) # offset mesh coords to match camera pose # eul = placed_cameras[i].pose[3:] # USE WALL NORMAL, NOT CAMERA POSE # R = rot3d_from_rtp(np.array([eul[2], -eul[0], -eul[1]])) R = rot3d_from_x_vec(placed_cameras[i].wall_normal) R2 = rot3d_from_rtp(np.array([0, -90, 0])) R_aug = np.zeros([4, 4]) R_aug[:3, :3] = R.dot(R2) R_aug[:3, -1] = placed_cameras[i].pose[:3] quad_mesh.vertices = trimesh.transform_points(quad_mesh.vertices, R_aug) quad_mesh_vedo = vedo.mesh.Mesh(quad_mesh) quad_mesh_vedo.cellIndividualColors(quad_mesh.visual.face_colors / 255, alphaPerCell=True) quad_mesh_pv = pv.read( "/home/simon/catkin_ws/src/perch_placement/src/ui/models/white-red-black_quad2.ply" ) pymesh_frustum = placed_cameras[i].generate_discrete_camera_mesh( degrees_per_step=10, environment=environment) pymesh_verts = pymesh_frustum.vertices.copy() pymesh_verts.flags.writeable = True pymesh_faces = pymesh_frustum.faces.copy() pymesh_faces.flags.writeable = True if i == focus_id: frustum = trimesh.Trimesh(vertices=pymesh_frustum.vertices.copy(), faces=pymesh_frustum.faces.copy()) vedo_frustum = vedo.mesh.Mesh(frustum) vedo_frustum.alpha(0.3) vedo_frustum.color("c") # geom_list.append(frustum_lines) geom_list.append(quad_mesh_vedo) geom_list.append(vedo_frustum) print("cam pose: " + str(placed_cameras[i].pose)) pose = placed_cameras[i].pose plt2.camera.SetPosition(pose[0], pose[1], pose[2]) R = rot3d_from_rtp(np.array([pose[-1], -pose[-3], -pose[-2]])) print("R: " + str(R)) focus = pose[:3] + R[:, 0] print("focus: " + str(focus)) plt2.camera.SetFocalPoint(focus[0], focus[1], focus[2]) plt2.camera.SetViewUp(R[:, 2]) plt2.camera.SetDistance(5) plt2.camera.SetClippingRange([0.2, 10]) plt2.camera.SetViewAngle(placed_cameras[i].fov[-1] * 1.1) plt2.show(resetcam=False) plt3.set_position(pose[:3]) plt3.set_viewup(R[:, 2]) plt3.set_focus(focus) plt3.show(auto_close=False, interactive=False) else: # vedo_frustum.alpha(0.1) # vedo_frustum.color("p") quad_mesh_vedo.color('o') # geom_list.append(frustum_lines) geom_list.append(quad_mesh_vedo) # geom_list.append(vedo_frustum) plt2.add(quad_mesh_vedo) plt3.add_mesh(quad_mesh_pv) # testing: test = (-plt3.get_image_depth(fill_value=0) / placed_cameras[0].range[1]) test[test > 1] = 1.0 test[test < 0] = 0.0 test = np.round(test * np.iinfo(np.uint16).max) test = test.astype(np.uint16) # test_cv = cv.normalize(-test / placed_cameras[0].range[1] * 255, 0, 255, cv.NORM_MINMAX) cv.imshow('test', test) cv.waitKey() for actor in geom_list: plt1.add(actor) plt1.add( vedo.Text2D( "Press 'y' to approve placement. Press 'n' to reject. " "\nPress 'f' for front culling, 'b' for back culling, 'c' to disable culling " "\nPress 'q' when done", pos='bottom-right', c='dg', bg='g', font='Godsway')) plt1.camera.SetPosition(7.8 * np.cos(-145 * np.pi / 180.0), 7.8 * np.sin(-145 * np.pi / 180.0), 3.) plt1.camera.SetFocalPoint(-0.026929191045848594, 0.5783514020506139, 0.8268966663940324) plt1.camera.SetViewUp(np.array([0, 0, 1])) plt1.camera.SetDistance(7.8) plt1.camera.SetClippingRange([0.25, 10]) plt1.show(resetcam=False) return approved
def evaluate_particle(particle, environment, cameras, placed_cameras=PlacedCameras(), opt_options=CameraPlacementOptions(), debug=False, vedo_plt=None, maximization=False): """ This function evaluates a single particle using the heuristic function defined in evaluate_arrangement() :param particle: individual particle as np.array :param environment: Environment class instance :param cameras: a list of Camera objects corresponding to the cameras which have already been placed in an environment :param placed_cameras: PlacedCameras class instance :param opt_options: CameraPlacementOptions class containing opt opt :param debug: flag, if true print and draw relevant information. :return: score of particle """ # convert particle into camera pose; evaluate edge normal at each camera position cameras = convert_particle_to_state(environment=environment, particle=particle, cameras=cameras, opt_options=opt_options, debug=debug) # plot particles to debug... # if debug: # if environment.dimension == 2: # plot_particle_2d(particle=np.squeeze(particle), environment=environment, cameras=cameras, # placed_cameras=placed_cameras, view_time=0.0001, figure_number=figure_number) # else: # # if len(placed_cameras.cameras) > 0: # plot_particle_3d(particle=particle, environment=environment, cameras=cameras, placed_cameras=placed_cameras, # view_time=0.0001, figure_number=figure_number) score = evaluate_arrangement_covariance(environment=environment, cameras=cameras, placed_cameras=placed_cameras, debug=debug, maximization=maximization) if vedo_plt is not None: if len(placed_cameras.cameras) >= 10: geom_list = [] for i in range(len(cameras)): pymesh_frustum = cameras[i].generate_discrete_camera_mesh(degrees_per_step=5, environment=environment) if len(pymesh_frustum.faces) > 0 and not np.isnan(pymesh_frustum.vertices).any(): pymesh_verts = pymesh_frustum.vertices.copy() pymesh_verts.flags.writeable = True pymesh_faces = pymesh_frustum.faces.copy() pymesh_faces.flags.writeable = True frustum = trimesh.Trimesh(vertices=pymesh_frustum.vertices.copy(), faces=pymesh_frustum.faces.copy()) vedo_frustum = vedo.mesh.Mesh(frustum) vedo_frustum.alpha(0.2) vedo_frustum.color("c") quad_mesh = trimesh.load( "/home/simon/catkin_ws/src/perch_placement/src/ui/models/white-red-black_quad2.ply") R = rot3d_from_x_vec(cameras[i].wall_normal) R2 = rot3d_from_rtp(np.array([0, -90, 0])) R_aug = np.zeros([4, 4]) R_aug[:3, :3] = R.dot(R2) R_aug[:3, -1] = cameras[i].pose[:3] quad_mesh.vertices = trimesh.transform_points(quad_mesh.vertices, R_aug) quad_mesh_vedo = vedo.mesh.Mesh(quad_mesh) quad_mesh_vedo.cellIndividualColors(quad_mesh.visual.face_colors / 255, alphaPerCell=True) geom_list.append(quad_mesh_vedo) geom_list.append(vedo_frustum) for actor in geom_list: vedo_plt.add(actor) # p_.camera_position = [ # (R * np.cos(t), R * np.sin(t), z), # (c[0], c[1], c[2]), # (-0.026929191045848594, 0.5783514020506139, 0.8268966663940324), # (0, 0, 1), # ] vedo_plt.camera.SetPosition(7*np.cos(-145*np.pi/180.0), 7*np.sin(-145*np.pi/180.0), 6.25) vedo_plt.camera.SetFocalPoint(-0.026929191045848594, 0.5783514020506139, 0.9268966663940324) vedo_plt.camera.SetViewUp(np.array([0, 0, 1])) vedo_plt.camera.SetDistance(7.8) vedo_plt.camera.SetClippingRange([0.25, 30]) vedo_plt.camera vedo_plt.show(interactive=False, rate=30, resetcam=False, fullscreen=True) time.sleep(0.5) actors = vedo_plt.actors for i in range(len(cameras)): vedo_plt.remove(actors.pop()) vedo_plt.remove(actors.pop()) # plot_particle_3d(particle=particle, environment=environment, cameras=cameras, placed_cameras=placed_cameras, # view_time=0.0001, figure_number=0) # print("Particle score: " + str(score) + "; Pose: " + str(cameras[0].pose)) if debug: print(score) return score