def get_instance(): sites = [(600, 550), (300, 400), (50, 100), (800, 100), (950, 980)] objects, elements, corners = [], [], [] wall = [(0, 0), (1000, 0), (1000, 1000), (0, 1000)] objects.append(wall) elements.append(vis.Polygon([vis.Point(x, y) for x, y in wall])) hole1 = [(100, 300), (100, 500), (150, 500), (150, 300)] objects.append(hole1) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole1])) corners += hole1 hole2 = [(300, 700), (300, 800), (550, 800), (550, 700)] objects.append(hole2) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole2])) corners += hole2 hole3 = [(700, 300), (700, 650), (850, 650), (850, 300)] objects.append(hole3) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole3])) corners += hole3 hole4 = [(300, 100), (300, 200), (450, 200), (450, 100)] objects.append(hole4) elements.append(vis.Polygon([vis.Point(x, y) for x, y in hole4])) corners += hole4 env = vis.Environment(elements) return sites, objects, env, corners
def environments(epsilon=0.0000001): wall_choords = [(0, 0), (700, 0), (700, 900), (0, 900)] wall = vis.Polygon([vis.Point(x, y) for x, y in wall_choords]) holes_choords = [[(100, 300), (100, 500), (150, 500), (150, 300)], [(300, 300), (300, 500), (400, 550), (400, 300)], [(90, 700), (250, 750), (220, 600), (150, 600)], [(330, 700), (330, 800), (530, 850), (530, 790)], [(230, 50), (250, 90), (390, 90), (390, 50)]] holes = [] for hc in holes_choords: holes.append(vis.Polygon([vis.Point(x, y) for x, y in hc])) draw_env(wall_choords, holes_choords) env = vis.Environment([wall] + holes) return env, vis.Visibility_Graph(env, epsilon)
def create_cone(point, radio, angle, opening, resolution=1): # Define the list for the points of the cone-shape polygon p = [] # The fisrt point will be the vertex of the cone p.append(vis.Point(point[0], point[1])) # Define the start and end of the arc start = angle - opening end = angle + opening for i in range(start, end, resolution): # Convert start angle from degrees to radians rad = math.radians(i) # Calculate the off-set of the first point of the arc x = radio * math.cos(rad) y = radio * math.sin(rad) # Add the off-set to the vertex point new_x = point[0] + x new_y = point[1] + y # Add the first point of the arc to the list p.append(vis.Point(new_x, new_y)) # Add the last point of the arc rad = math.radians(end) x = radio * math.cos(rad) y = radio * math.sin(rad) new_x = point[0] + x new_y = point[1] + y p.append(vis.Point(new_x, new_y)) return vis.Polygon(p)
def find_obstacles(occ_grid, thresh=125, unknown=0): # occ_grid needs to be cv2 object # returns list of obstacles (list of polygons) # threshold is a little low because I was trying to exclude unknown areas as obstacles grid = occ_grid.copy() grid[np.where(grid == unknown)] = 255 thresh_grid = cv2.threshold(grid, thresh, 255, cv2.THRESH_BINARY)[1] thresh_grid = np.array(thresh_grid, dtype=np.uint8) # cnts = cv2.findContours(thresh.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cnts = cv2.findContours(thresh_grid, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) # set contours as visilibity polygons obstacles = [] for c in cnts: points = [] for p in c: points.append(vis.Point(float(p[0][0]), float(p[0][1]))) obstacle = vis.Polygon(points) obstacle.eliminate_redundant_vertices(redundant_eps) obstacles.append(obstacle) return obstacles
def __get_vis(self): poly = [] for p in self.path.to_polygons(): poly.append(vis.Polygon([vis.Point(*v) for v in p[:-1][::-1]])) env = vis.Environment(poly) vg = vis.Visibility_Graph(env, self.epsilon) return env, vg
def __init__(self,blocker_polygons,width,height): self.environment = vis.Environment() for poly in blocker_polygons: points = [vis.Point(x,y) for x,y in poly] #self.polys.append(vis.Polygon(points)) poly = vis.Polygon(points) if poly.area() > 0: poly.reverse() self.environment.add_hole(poly) self.environment.set_outer_boundary(vis.Polygon([ vis.Point(0,0), vis.Point(width,0), vis.Point(width,height), vis.Point(0,height), ]))
def construct_environment_demo(power_dataframe, battery_capacity): aggregated_power = power_dataframe.cumsum().Power aggregated_power_lower = [] aggregated_power_higher = [] i = 0 for power in aggregated_power.tolist(): aggregated_power_lower.append([i, power]) aggregated_power_higher.append([i, power + battery_capacity]) i += 1 # Construct the upper hole in a reverse order aggregated_power_higher.reverse() aggregated_power_higher.insert(0, aggregated_power_higher[-1]) aggregated_power_higher.insert( 1, [aggregated_power_higher[0][0], aggregated_power_higher[1][1]]) del aggregated_power_higher[-1] # Construct the lower hole in the reverse order aggregated_power_lower.append( [aggregated_power_lower[-1][0], aggregated_power_lower[0][1]]) # close the hole # aggregated_power_lower.append(aggregated_power_lower[0]) # Construct the wall list wall_list = [] wall_list.append([aggregated_power_lower[0][0], 0]) wall_list.append([aggregated_power_lower[-1][0], 0]) wall_list.append(aggregated_power_higher[2]) wall_list.append(aggregated_power_higher[1]) higher_hole_points = [vis.Point(x, y) for x, y in aggregated_power_higher] lower_hole_points = [vis.Point(x, y) for x, y in aggregated_power_lower] wall_points = [vis.Point(x, y) for x, y in wall_list] higher_hole = vis.Polygon(higher_hole_points) lower_hole = vis.Polygon(lower_hole_points) wall = vis.Polygon(wall_points) return wall, higher_hole, lower_hole
def build_fixed_env(self): # Define the points which will be the outer boundary of the environment # Must be COUNTER-CLOCK-WISE(ccw) # p1 = vis.Point(0-self.epsilon*2.0, 0-self.epsilon*2.0) # p2 = vis.Point(self.workspace[0]+self.epsilon*2.0, 0-self.epsilon*2.0) # p3 = vis.Point(self.workspace[0]+self.epsilon*2.0, self.workspace[1]+self.epsilon*2.0) # p4 = vis.Point(0-self.epsilon*2.0,self.workspace[1]+self.epsilon*2.0) p1 = vis.Point(0 - 5, 0 - 5) p2 = vis.Point(self.workspace[0] + 5, 0 - 5) p3 = vis.Point(self.workspace[0] + 5, self.workspace[1] + 5) p4 = vis.Point(0 - 5, self.workspace[1] + 5) # Load the values of the outer boundary polygon in order to draw it later self.wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] self.wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw) # Create the outer boundary polygon walls = vis.Polygon([p1, p2, p3, p4]) self.holes_array.append(walls) for (obs, boundary) in iter(self.obs.items()): xx, yy = boundary.exterior.coords.xy hole_x = [] hole_y = [] point_array = [] for i in range(len(xx)): point = vis.Point(xx[i], yy[i]) hole_x.append(point.x()) hole_y.append(point.y()) if i != (len(xx) - 1): point_array.append(point) hole = vis.Polygon(point_array) # print('Hole in standard form: ',hole.is_in_standard_form()) self.holes_array.append(hole) self.hole_x_array.append(hole_x) self.hole_y_array.append(hole_y)
def vis_graph(poly): P = poly_list(poly) vis_polygons = [] for p in P: vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p])) env = vis.Environment(vis_polygons) vg = vis.Visibility_Graph(env, 0.000001) edges = [] for i, j in itertools.combinations(range(vg.n()), 2): if vg(i, j): edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]]) plt.gca().add_collection(mc.LineCollection(edges, color='g')) print len(edges)
def outer(y_up, y_down, x_left, x_right): ## use arena coordinates #list points ccw a = vis.Point(x_left, y_size - (y_down - 1)) b = vis.Point(x_right, y_size - (y_down - 1)) c = vis.Point(x_right, y_size - (y_up - 1)) d = vis.Point(x_left, y_size - (y_up - 1)) # Values for graph wall_x = [a.x(), b.x(), c.x(), d.x(), a.x()] wall_y = [a.y(), b.y(), c.y(), d.y(), a.y()] # Create the outer boundary polygon walls = vis.Polygon([a, b, c, d]) return wall_x, wall_y, walls
def hole(y_up, y_down, x_left, x_right): ## use arena coordinates #points are listed in cc order #list points cw a = vis.Point(x_left, y_size - (y_down - 1)) b = vis.Point(x_left, y_size - (y_up - 1)) c = vis.Point(x_right, y_size - (y_up - 1)) d = vis.Point(x_right, y_size - (y_down - 1)) # values for graph\ hole_x = [a.x(), b.x(), c.x(), d.x(), a.x()] hole_y = [a.y(), b.y(), c.y(), d.y(), a.y()] # Create the hole polygon hole = vis.Polygon([a, b, c, d]) return hole_x, hole_y, hole
def vis_graph(poly): P = poly_list(poly) vis_polygons = [] for p in P: vis_polygons.append(vis.Polygon([vis.Point(x, y) for x, y in p])) env = vis.Environment(vis_polygons) vg = vis.Visibility_Graph(env, 0.000001) edges = [] G = nx.Graph() for i, j in itertools.combinations(range(vg.n()), 2): if vg(i, j): edges.append([[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]]) G.add_edge(i, j, weight=np.linalg.norm( np.array(edges[-1][0]) - np.array(edges[-1][1]))) plt.gca().add_collection(mc.LineCollection(edges, color='g')) s, t = get_st(env) sp = [i for i in nx.shortest_path(G, s, t, 'weight')] sp_edges = [[[env(i).x(), env(i).y()], [env(j).x(), env(j).y()]] for i, j in zip(sp[:-1], sp[1:])] plt.gca().add_collection(mc.LineCollection(sp_edges, color='r', lw=2))
def calculate_solution(): # The Ilya Constant (TM) epsilon = 0.000000001 # Environment plot (500 x 500 should be enough, right?! *gulps with fear*) p1 = vis.Point(50, 50) p2 = vis.Point(-50, 50) p3 = vis.Point(-50, -50) p4 = vis.Point(50, -50) # Set up our walls (yay) wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] walls = vis.Polygon([p1, p2, p3, p4]) #robot1 = vis.Point(-1,-1) #robot2 = vis.Point(4,4) #obstacle = vis.Polygon([vis.Point(1,6),vis.Point(1,1),vis.Point(5,1),vis.Point(5,5),vis.Point(3,5),vis.Point(3,3),vis.Point(4,3),vis.Point(4,2), # vis.Point(2,2),vis.Point(2,6),vis.Point(6,6),vis.Point(6,0),vis.Point(0,0),vis.Point(0,6),vis.Point(1,6)][::-1]) #obstacle_x = [1, 1, 5, 5, 3, 3, 4, 4, 2, 2, 6, 6, 0, 0, 1][::-1] #obstacle_y = [6, 1, 1, 5, 5, 3, 3, 2, 2, 6, 6, 0, 0, 6, 6][::-1] robot1 = vis.Point(0, 1) robot2 = vis.Point(6, 2) obstacle = vis.Polygon([vis.Point(8,1),vis.Point(4,1),vis.Point(4,4),vis.Point(5,2), vis.Point(8,1)]) obstacle_x = [8, 4, 4, 5, 8] obstacle_y = [1, 1, 4, 2, 1] obstacle2 = vis.Polygon([vis.Point(1,2), vis.Point(1,4), vis.Point(3,4), vis.Point(3,2), vis.Point(1,2)][::-1]) obstacle2_x = [1, 1, 3, 3, 1][::-1] obstacle2_y = [2, 4, 4, 2, 2][::-1] env = vis.Environment([walls, obstacle, obstacle2]) robot1.snap_to_boundary_of(env, epsilon) robot1.snap_to_vertices_of(env, epsilon) isovist = vis.Visibility_Polygon(robot1, env, epsilon) shortest_path = env.shortest_path(robot1, robot2, epsilon) point_x, point_y = save_print(isovist) point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) p.title('Shortest (????) Path') p.xlabel('X Position') p.ylabel('Y Position') p.plot(wall_x, wall_y, 'black') p.plot([robot1.x()], [robot1.y()], 'go') p.plot([robot2.x()], [robot2.y()], 'go') p.plot(obstacle_x, obstacle_y, 'r') p.plot(obstacle2_x, obstacle2_y, 'r') print "Shortest Path length from observer to end: ", shortest_path.length() print "Number of options: ", shortest_path.size() polyline = [] for x in range(0, shortest_path.size()): point = shortest_path.getVertex(x) print "(", point.x(), ", ", point.y(), ")" polyline.append(point) p.show()
def save_images_for_path(data_dir, trial_idx, p_zb, scale=50, eps=1e-7): max_wall = 200 fov = np.pi / 2 files_list = {'obstacles': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_obstacles.csv'), 'inspection_points': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_inspection_points.csv'), 'configurations': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_conf'), 'vertex': os.path.join(data_dir, f'test_p_zb_{trial_idx}_{p_zb}_vertex'), 'results':os.path.join(data_dir, f'test_search_p_zb_{trial_idx}_{p_zb}_result')} # check if test files exist broken_files = False for file_path in files_list.values(): if not os.path.isfile(file_path): broken_files = True if broken_files: print('Broken file') sys.exit() # construct image with obstacles img = np.zeros((101,101)) obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int) obstacles_columns = obstacles.columns.to_list() obstacles['p1'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']),axis=1) obstacles['p2'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']),axis=1) obstacles['p3'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x']+x['width'],x['y']+x['length']),axis=1) obstacles['p4'] = obstacles[obstacles_columns].astype(float).apply(lambda x: vis.Point(x['x'],x['y']+x['length']),axis=1) obstacles['polygon'] = obstacles.apply(lambda x: vis.Polygon([x['p1'], x['p2'], x['p3'], x['p4']]) ,axis=1) # define bounded environment with obstacles p1 = vis.Point(0,0) p2 = vis.Point(101,0) p3 = vis.Point(101,101) p4 = vis.Point(0,101) walls_poly = vis.Polygon([p1, p2, p3, p4]) vis_env = vis.Environment([walls_poly] + obstacles['polygon'].to_list()) # load inspection points csv inspection_points = (pd.read_csv(files_list['inspection_points']) * scale).round(0).astype(int) # load configuration space csv cspace_df = pd.read_csv(files_list['configurations'], delimiter=' ', header=None).drop(columns=[0,6]) # drop already seen points from inspection points with open(files_list['results'],'r') as f: line = f.readlines()[-2] vertices_idxs = [int(x) for x in line.split(' ')[1:-1]] img_copy = img.copy() for j in range(0,len(vertices_idxs)-1): c_point = cspace_df.iloc[j].to_numpy() # compute end-point for vertex links_val = np.rint(compute_links(c_point) * scale).astype(int) ee_val = links_val[-1] # get orientation of end effector ee_orientation = c_point.sum() # set visibility triangle x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov) y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov) x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov) y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov) vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2,y2)]) # define observer if is_in_bounds(ee_val): observer = vis.Point(float(ee_val[0]), float(ee_val[1])) observer.snap_to_boundary_of(vis_env, eps) observer.snap_to_vertices_of(vis_env, eps) isovist = vis.Visibility_Polygon(observer, vis_env, eps) # get environment in points point_x , point_y = save_print(isovist) if len(point_x ) == 0 or len(point_y) == 0: continue point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) poly = Polygon([(x,y) for (x,y) in zip(point_x, point_y)]) visilbe_poly = poly.intersection(vis_tri) if type(visilbe_poly) == Polygon and len(list(visilbe_poly.exterior.coords)) > 0: visilbe_poly_pts = np.array(list(visilbe_poly.exterior.coords)).reshape((-1,1,2)).astype(int) # draw visilbe polygon of the observer cv2.fillPoly(img_copy, [visilbe_poly_pts], 150) # draw obstacles and inspection points obstacles.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1) inspection_points.apply(lambda x: cv2.rectangle(img_copy, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1) # add sample (x,y) cv2.imwrite(f'sample_path_{trial_idx}_{p_zb}_{j}.png', img_copy)
def load_plain_images(data_dir, trial_idx, scale=50): files_list = { 'obstacles': os.path.join(data_dir, f'test_planar_{trial_idx}_obstacles.csv'), 'inspection_points': os.path.join(data_dir, f'test_planar_{trial_idx}_inspection_points.csv'), 'configurations': os.path.join(data_dir, f'test_planar_{trial_idx}_conf'), 'vertex': os.path.join(data_dir, f'test_planar_{trial_idx}_vertex'), 'results': os.path.join(data_dir, f'test_search_{trial_idx}_result') } # check if test files exist for file_path in files_list.values(): if not os.path.isfile(file_path): print(f'{file_path} is missing...') return None, None, None, None, None # construct image with obstacles img = np.zeros((101, 101)) obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int) #obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1) obstacles_columns = obstacles.columns.to_list() obstacles['p1'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'], x['y']), axis=1) obstacles['p2'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'] + x['width'], x['y']), axis=1) obstacles['p3'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'] + x['width'], x['y'] + x['length']), axis=1) obstacles['p4'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'], x['y'] + x['length']), axis=1) obstacles['polygon'] = obstacles.apply( lambda x: vis.Polygon([x['p1'], x['p2'], x['p3'], x['p4']]), axis=1) # define bounded environment with obstacles p1 = vis.Point(0, 0) p2 = vis.Point(101, 0) p3 = vis.Point(101, 101) p4 = vis.Point(0, 101) walls_poly = vis.Polygon([p1, p2, p3, p4]) vis_env = vis.Environment([walls_poly] + obstacles['polygon'].to_list()) # load inspection points csv inspection_points = (pd.read_csv(files_list['inspection_points']) * scale).round(0).astype(int) # draw obstacles and inspection points obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x'] + x[ 'width'], x['y'] + x['length']), 255, -1), axis=1) inspection_points.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1) # get initial starting point cspace_df = pd.read_csv(files_list['configurations'], delimiter=' ', header=None).drop(columns=[0, 6]) start_cpoint = cspace_df.iloc[0].values return img, vis_env, obstacles, inspection_points, start_cpoint
def get_vis_form_from_vx_list(vs): poly = list(map(lambda x: vis.Point(x[1][0], x[1][1]), vs)) walls = vis.Polygon(poly) walls.enforce_standard_form() walls.eliminate_redundant_vertices() return walls
def make_arena_polygon(): #build center diamond polygon #list points cw p1 = vis.Point(23, ym - 16) p2 = vis.Point(22, ym - 15) p3 = vis.Point(22, ym - 13) p4 = vis.Point(23, ym - 12) p5 = vis.Point(25, ym - 12) p6 = vis.Point(26, ym - 13) p7 = vis.Point(26, ym - 15) p8 = vis.Point(25, ym - 16) # Load the values of the hole polygon in order to draw it later diam_x = [ p2.x(), p3.x(), p4.x(), p5.x(), p6.x(), p7.x(), p8.x(), p1.x(), p2.x() ] diam_y = [ p2.y(), p3.y(), p4.y(), p5.y(), p6.y(), p7.y(), p8.y(), p1.y(), p2.y() ] # Create the hole polygon diam = vis.Polygon([p2, p3, p4, p5, p6, p7, p8, p1]) #build the arena wall_x, wall_y, walls = outer( 2, 28, 1, 47) #y_up, y_down, x_left, x_right, use arena format #format y_up, y_down, x_left, x_right, use arena format huecos_input = [ [ym - 6, ym - 7, 1, 7], # [ym - 12, ym - 14, 10, 14], # [ym - 20, ym - 26, 9, 11], # [ym - 5, ym - 7, 21, 27], # [ym - 19, ym - 21, 21, 27], # [ym - 12, ym - 14, 34, 38], [ym - 19, ym - 20, 41, 47], # [ym, ym - 6, 37, 39], # ] huecos = [] for item in huecos_input: huecos.append(hole(item[0], item[1], item[2], item[3])) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes arena_poly = vis.Environment([ walls, huecos[0][2], huecos[1][2], huecos[2][2], huecos[3][2], huecos[4][2], huecos[5][2], huecos[6][2], huecos[7][2], diam ]) return arena_poly, [[wall_x, wall_y], [huecos[0][0], huecos[0][1]], [huecos[1][0], huecos[1][1]], [huecos[2][0], huecos[2][1]], [huecos[3][0], huecos[3][1]], [huecos[4][0], huecos[4][1]], [huecos[5][0], huecos[5][1]], [huecos[6][0], huecos[6][1]], [huecos[7][0], huecos[7][1]], [diam_x, diam_y]]
def __init__(self, filename): """ Constructor of geometry, reads the given xml file (filename) and extracts geometrical features :param filename: File with geometry information """ self.walls = [] self.floor = None self.entrances = {} self.exits = {} self.pedestrians = {} self.edges = [] self.obstacles = {} self.attraction_ground = {} self.attraction_mounted = {} walls, obstacles, entrances, entrances_properties, exits, edges = read_geometry(filename) self.entrances_properties = entrances_properties points = [] points_vis = [] for wall in walls: p1 = Point(wall[0][0], wall[0][1]) p2 = Point(wall[1][0], wall[1][1]) wall = LineString([p1, p2]) self.walls.append(wall) points.append(p1) points_vis.append(vis.Point(float(p1.x), float(p1.y))) for key, door in entrances.items(): p1 = Point(door[0][0], door[0][1]) p2 = Point(door[1][0], door[1][1]) self.entrances[key] = LineString([p1, p2]) # for key, door in exits.items(): line_points = [] for point in door: line_points.append(Point(point[0], point[1])) self.exits[key] = LineString(line_points) holes = [] holes_vis = [] for key, obstacle in obstacles.items(): hole_points = [] hole_points_vis = [] for point in obstacle: hole_points.append((point[0], point[1])) hole_points_vis.append(vis.Point(point[0], point[1])) hole_poly = Polygon(hole_points[::-1]) holes.append(hole_poly) hole_vis = vis.Polygon(hole_points_vis) holes_vis.append(hole_vis) self.obstacles[key] = hole_poly # create polygon holes_poly = cascaded_union(holes) hole_points = [] holes_vis = [] if holes_poly.geom_type == 'Polygon': coords = holes_poly.exterior.coords[::-1] hole_points.append(coords) hole_points_vis = [] for coord in coords[:-1]: hole_points_vis.append(vis.Point(coord[0], coord[1])) hole_vis = vis.Polygon(hole_points_vis[::-1]) hole_vis.enforce_standard_form() holes_vis.append(hole_vis) else: for i in holes_poly: coords = i.exterior.coords[::-1] hole_points.append(coords) hole_points_vis = [] for coord in coords[:-1]: hole_points_vis.append(vis.Point(coord[0], coord[1])) hole_vis = vis.Polygon(hole_points_vis[::-1]) hole_vis.enforce_standard_form() holes_vis.append(hole_vis) self.floor = Polygon(points, hole_points) self.env = vis.Environment([vis.Polygon(points_vis[::-1]), *holes_vis]) if not self.env.is_valid(self.epsilon): raise ValueError('Check geometry!') return
def main(): # FieldPoly.synthesize(50, hole_count=1).plot(c='red', alpha=0.3, lw=10) \ # .get_visible_poly(ThePoint([500, 500]).plot(c='k', lw=5)).plot() # plt.show() # return # work_folder = data_folder['dauka_tutorial_1'] work_folder = Folder('/home/daulet/Desktop/zones') original_path = work_folder['4-3-1_out.tiff'] logger.info("original_path: %s", original_path) original_ds = DataSet(original_path) orig_array = np.where(original_ds.array == -9999, np.nan, original_ds.array) orig_array = np.where(np.isnan(orig_array), np.nanmin(orig_array), orig_array) filtered_array = gaussian_filter(orig_array, sigma=1) otsu_threshold = threshold_otsu(filtered_array) mask = filtered_array > otsu_threshold eroded_mask = binary_erosion(mask, iterations=5) dilated_mask: np.ndarray = binary_dilation(eroded_mask, iterations=5).astype(int) # output to tiff # DataSet.from_array(dilated_mask, original_ds.geo_info) \ # .to_file(str(Path(original_path).with_name('output.tiff')), 'GTiff', no_data_value=0, dtype=gdal.GDT_Byte) geoms: gpd.GeoSeries = vectorize(dilated_mask, work_folder['vectorized.geojson'], original_ds) geom_extent = shg.Polygon(geoms.cascaded_union.envelope.boundary) # FieldPoly().bounds_xy # shg.Polygon(shg.Polygon().boundary) # for geom in geoms: # geom_extent = geom_extent.difference(geom) # otirik_env = FieldPoly(geom_extent).plot(c='red') # # res = otirik_env.get_visible_poly(ThePoint(otirik_env.geometry.centroid)).plot(c='k').plot() # res = otirik_env.get_visible_poly(ThePoint([0, 0])).plot(c='k').plot() # # logger.info("res:\n%s", res) # # visible_zone.plot() # plt.show() print('!!!!!!!!!', geoms) # for i in range(len(geoms)): # x_arr, y_arr = geoms[i].exterior.coords.xy # holes = [[0] * len(x_arr)] * len(geoms) # for j in range(len(x_arr)): # holes[i][j] = vis.Point(x_arr[j], y_arr[j]) # holes_x = holes[i][j].x() vis_polygons = [] x_arr = [] y_arr = [] for i in range(len(geoms)): points = makePoints(geoms[i]) vis_polygons.append(vis.Polygon(points)) x_arr.append(getXPoints(points)) y_arr.append(getYpoints(points)) print('Hole in standard form: ', vis_polygons[i].is_in_standard_form()) points = makePoints(geom_extent) walls = vis.Polygon(points) # vis_polygons.insert(0, walls) env = vis.Environment([walls, vis_polygons[0]]) print('Walls in standard form : ', walls.is_in_standard_form()) print('Environment is valid : ', env.is_valid(epsilon)) observer = vis.Point(673000, 5665000) observer.snap_to_boundary_of(env, epsilon) print('!!!!', vis_polygons, '!!!!!!') observer.snap_to_vertices_of(env, epsilon) # isovist = vis.Visibility_Polygon(observer, env, epsilon) plotter = Plotter() plotter.add_images(mask, eroded_mask, dilated_mask) plotter.plot(lbrtwh=(1e-3, 1e-3, 1 - 1e-3, 1 - 1e-3, 1e-3, 0)).show() return
def compute(v, P): """ Compute the visible part of P from v """ #print("Previsible polygon: %s"%(P,)) # Used for visilibity library epsilon = 0.01 #Using the visilibity library, define the reflex vertex observer = vis.Point(*v[1]) # To put into standard form, do this if not LinearRing(P[0]).is_ccw: ext = P[0][::-1] else: ext = P[0] x_min_idx, x_min = min(enumerate(ext), key=lambda x: x[1]) ext = ext[x_min_idx:] + ext[:x_min_idx] #print x_min_idx, x_min # Define the walls of intersection in Visilibity domain wall_points = [] for point in ext: wall_points.append(vis.Point(*point)) #print 'Walls in standard form : ',vis.Polygon(wall_points).is_in_standard_form() #for i in range(len(vis_intersection_wall_points)): #print vis_intersection_wall_points[i].x(), vis_intersection_wall_points[i].y() #print point.x(), point.y() # Define the holes of intersection in Visilibity domain holes = [] for interior in P[1]: hole_points = [] for point in interior: hole_points.append(vis.Point(*point)) holes.append(hole_points) #print 'Hole in standard form : ',vis.Polygon(hole_points).is_in_standard_form() # Construct a convinient list env = [] env.append(vis.Polygon(wall_points)) for hole in holes: env.append(vis.Polygon(hole)) # Construct the whole envrionemt in Visilibity domain env = vis.Environment(env) # Construct the visible polygon observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) vis_free_space = vis.Visibility_Polygon(observer, env, epsilon) point_x, point_y = save_print(vis_free_space) point_x.append(vis_free_space[0].x()) point_y.append(vis_free_space[0].y()) return point_x, point_y
def testVisilibity(): # Define an epsilon value (should be != 0.0) epsilon = 1e-7 # Define the points which will be the outer boundary of the environment # Must be COUNTER-CLOCK-WISE(ccw) p1 = vis.Point(0, 0) p2 = vis.Point(700, 0) p3 = vis.Point(700, 900) p4 = vis.Point(0, 900) # Load the values of the outer boundary polygon in order to draw it later wall_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] wall_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Outer boundary polygon must be COUNTER-CLOCK-WISE(ccw) # Create the outer boundary polygon walls = vis.Polygon([p1, p2, p3, p4]) # Define the point of the "observer" observer = vis.Point(235, 400) # Uncomment the following line in order to create a cone polygon #walls = create_cone((observer.x(), observer.y()), 500, 270, 30, quality= 3) # Walls should be in standard form print('Walls in standard form : ', walls.is_in_standard_form()) # Now we define some holes for our environment. The holes must be inside # our outer boundary polygon. A hole blocks the observer vision, it works as # an obstacle in his vision sensor. # We define some point for a hole. You can add more points in order to get # the shape you want. # The smalles point should be first p2 = vis.Point(100, 300) p3 = vis.Point(100, 500) p4 = vis.Point(150, 500) p1 = vis.Point(150, 300) # Load the values of the hole polygon in order to draw it later hole_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Note: The point of a hole must be in CLOCK-WISE(cw) order. # Create the hole polygon hole = vis.Polygon([p2, p3, p4, p1]) # Check if the hole is in standard form print('Hole in standard form: ', hole.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(300, 300) p2 = vis.Point(300, 500) p3 = vis.Point(400, 550) p4 = vis.Point(400, 300) # Load the values of the hole polygon in order to draw it later hole1_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole1_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole1 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole1.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p2 = vis.Point(90, 700) p3 = vis.Point(250, 750) p4 = vis.Point(220, 600) p1 = vis.Point(150, 600) # Load the values of the hole polygon in order to draw it later hole2_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole2_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole2 = vis.Polygon([p2, p3, p4, p1]) # Check if the hole is in standard form print('Hole in standard form: ', hole2.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(330, 700) p2 = vis.Point(330, 800) p3 = vis.Point(530, 850) p4 = vis.Point(530, 790) # Load the values of the hole polygon in order to draw it later hole3_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole3_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole3 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole3.is_in_standard_form()) # Define another point of a hole polygon # Remember: the list of points must be CLOCK-WISE(cw) p1 = vis.Point(230, 50) p2 = vis.Point(250, 90) p3 = vis.Point(390, 90) p4 = vis.Point(390, 50) # Load the values of the hole polygon in order to draw it later hole4_x = [p1.x(), p2.x(), p3.x(), p4.x(), p1.x()] hole4_y = [p1.y(), p2.y(), p3.y(), p4.y(), p1.y()] # Create the hole polygon hole4 = vis.Polygon([p1, p2, p3, p4]) # Check if the hole is in standard form print('Hole in standard form: ', hole4.is_in_standard_form()) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes env = vis.Environment([walls, hole, hole2, hole1, hole3, hole4]) # Check if the environment is valid print('Environment is valid : ', env.is_valid(epsilon)) # Define another point, could be used to check if the observer see it, to # check the shortest path from one point to the other, etc. end = vis.Point(330, 525) # Define another point that the 'observer' will see end_visible = vis.Point(415, 45) # Necesary to generate the visibility polygon observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) # Obtein the visibility polygon of the 'observer' in the environmente # previously define isovist = vis.Visibility_Polygon(observer, env, epsilon) # Uncomment the following line to obtein the visibility polygon # of 'end' in the environmente previously define #polygon_vis = vis.Visibility_Polygon(end, env, epsilon) # Obtein the shortest path from 'observer' to 'end' and 'end_visible' # in the environment previously define shortest_path = env.shortest_path(observer, end, epsilon) shortest_path1 = env.shortest_path(observer, end_visible, epsilon) # Print the length of the path print("Shortest Path length from observer to end: ", shortest_path.length()) print("Shortest Path length from observer to end_visible: ", shortest_path1.length()) logger.debug('shortest_path:\n%s', dir(shortest_path)) logger.debug('shortest_path.set_vertices:\n%s', shortest_path.size()) # logger.debug('shortest_path.path:\n%s',shortest_path) # Check if 'observer' can see 'end', i.e., check if 'end' point is in # the visibility polygon of 'observer' print("Can observer see end? ", end._in(isovist, epsilon)) print("Can observer see end_visible? ", end_visible._in(isovist, epsilon)) # Print the point of the visibility polygon of 'observer' and save them # in two arrays in order to draw the polygon later logger.debug('isovist: %s', isovist.n()) return point_x, point_y = save_print(isovist) # Add the first point again because the function to draw, draw a line from # one point to the next one and to close the figure we need the last line # from the last point to the first one point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) # Set the title p.title('VisiLibity Test') # Set the labels for the axis p.xlabel('X Position') p.ylabel('Y Position') # Plot the outer boundary with black color p.plot(wall_x, wall_y, 'black') # Plot the position of the observer with a green dot ('go') p.plot([observer.x()], [observer.y()], 'go') # Plot the position of 'end' with a green dot ('go') p.plot([end.x()], [end.y()], 'go') # Plot the position of 'end_visible' with a green dot ('go') p.plot([end_visible.x()], [end_visible.y()], 'go') # Plot the visibility polygon of 'observer' p.plot(point_x, point_y) # Plot the hole polygon with red color p.plot(hole_x, hole_y, 'r') # Plot the hole polygon with red color p.plot(hole1_x, hole1_y, 'r') # Plot the hole polygon with red color p.plot(hole2_x, hole2_y, 'r') # Plot the hole polygon with red color p.plot(hole3_x, hole3_y, 'r') # Plot the hole polygon with red color p.plot(hole4_x, hole4_y, 'r') # Example of a cone-shape polygon cone_point = vis.Point(440, 420) # cone=create_cone([cone_point.x(),cone_point.y()],150,0,45,3) # cone_x,cone_y=save_print(cone) # cone_x.append(cone_x[0]) # cone_y.append(cone_y[0]) # p.plot([cone_point.x()],[cone_point.y()],'go') # p.plot(cone_x,cone_y) # Show the plot p.show()
def get_geodesic_target(self, rob_pos, target, node_landmark, avoid_target=[]): holes_array = self.holes_array.copy() hole_x_array = self.hole_x_array.copy() hole_y_array = self.hole_y_array.copy() # start = datetime.datetime.now() # Define the point of the "observer" observer = vis.Point(rob_pos[0], rob_pos[1]) # Now we define some holes for our environment. A hole blocks the # observer vision, it works as an obstacle in his vision sensor. # The newly defined holes in this part are tha landmarks that need # to be avoided. for avoid, dist in avoid_target: avoid_x = node_landmark.landmark[avoid][0][0] avoid_y = node_landmark.landmark[avoid][0][1] dist_buffer = dist + self.epsilon if math.sqrt((rob_pos[0] - avoid_x)**2 + (rob_pos[1] - avoid_y)**2) < dist_buffer + 10: if math.sqrt((target[0] - avoid_x)**2 + (target[1] - avoid_y)**2) < dist_buffer: return rob_pos hole_x = [] hole_y = [] point_array = [] theta = -math.pi for i in range(self.discretization + 1): xx = dist_buffer * math.cos(theta) + avoid_x yy = dist_buffer * math.sin(theta) + avoid_y theta = theta - 2 * math.pi / self.discretization pt = vis.Point(round(xx, 2), round(yy, 2)) hole_x.append(pt.x()) hole_y.append(pt.y()) if i != (self.discretization): point_array.append(pt) hole = vis.Polygon(point_array) # print('Hole in standard form: ',hole.is_in_standard_form()) holes_array.append(hole) hole_x_array.append(hole_x) hole_y_array.append(hole_y) # Create environment, wall will be the outer boundary because # is the first polygon in the list. The other polygons will be holes env = vis.Environment(holes_array) # Check if the environment is valid # print('Environment is valid : ',env.is_valid(epsilon)) # Define another point, could be used to check if the observer see it, to # check the shortest path from one point to the other, etc. end = vis.Point(target[0], target[1]) # Necesary to generate the visibility polygon observer.snap_to_boundary_of(env, self.epsilon) observer.snap_to_vertices_of(env, self.epsilon) # Obtein the visibility polygon of the 'observer' in the environmente # previously define isovist = vis.Visibility_Polygon(observer, env, self.epsilon) # Uncomment the following line to obtein the visibility polygon # of 'end' in the environmente previously define #polygon_vis = vis.Visibility_Polygon(end, env, epsilon) # Obtein the shortest path from 'observer' to 'end' and 'end_visible' # in the environment previously define shortest_path = env.shortest_path(observer, end, self.epsilon) route = shortest_path.path() path_x = [] path_y = [] # print ('Points of Polygon: ') for i in range(len(route)): x = route[i].x() y = route[i].y() path_x.append(x) path_y.append(y) # Print the length of the path """ print("\nx\nx\nx\n") print("Shortest Path length from observer to end: ", shortest_path.length()) print("\nx\nx\nx\n") # Check if 'observer' can see 'end', i.e., check if 'end' point is in # the visibility polygon of 'observer' print( "Can observer see end? ", end._in(isovist, self.epsilon)) """ # Print the point of the visibility polygon of 'observer' and save them # in two arrays in order to draw the polygon later point_x, point_y = self.save_print(isovist) # Add the first point again because the function to draw, draw a line from # one point to the next one and to close the figure we need the last line # from the last point to the first one point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) if self.plot_images: # Set the title p.title('VisiLibity Test') # Set the labels for the axis p.xlabel('X Position') p.ylabel('Y Position') # Plot the outer boundary with black color p.plot(self.wall_x, self.wall_y, 'black') # Plot the position of the observer with a green dot ('go') p.plot([observer.x()], [observer.y()], 'go') # Plot the position of 'end' with a green dot ('go') p.plot([end.x()], [end.y()], 'go') # Plot the position of 'end_visible' with a green dot ('go') # Plot the visibility polygon of 'observer' p.plot(point_x, point_y) p.plot(path_x, path_y, 'b') for i in range(len(hole_x_array)): p.plot(hole_x_array[i], hole_y_array[i], 'r') # Show the plot p.show() """ print("\nx\nx\nx\n") print("Shortest Path length from observer to end: ", shortest_path.length()) print("\nx\nx\nx\n") print ('Points of Polygon: ') for i in range(len(route)): x = path_x[i] y = path_y[i] print("%f, %f" %(x,y)) print("\nx\nx\nx\n") print("\nx\nx\nx\n") print("\nx\nx\nx\n") print("\nx\nx\nx\n") """ # NBA_time = (datetime.datetime.now() - start).total_seconds() # print('Time for constructing the NBA: {0:.4f} s'.format(NBA_time)) #If start and goal are within threshold distance then return the start point itself if len(path_x) < 2: return path_x[0], path_y[0] return path_x[1], path_y[1]
def prepare_data(data_dir, dest_name, start_idx=0, scale=50, end_idx=100000, limit_samples=1e6, eps=1e-7): max_wall = 200 fov = np.pi / 2 imgs, c_points = [], [] counter_samples = 0 i = start_idx while (i < end_idx + 1 and limit_samples > 0 and counter_samples < limit_samples): i += 1 files_list = { 'obstacles': os.path.join(data_dir, f'test_planar_{i}_obstacles.csv'), 'inspection_points': os.path.join(data_dir, f'test_planar_{i}_inspection_points.csv'), 'configurations': os.path.join(data_dir, f'test_planar_{i}_conf'), 'vertex': os.path.join(data_dir, f'test_planar_{i}_vertex'), 'results': os.path.join(data_dir, f'test_search_{i}_result') } # check if test files exist broken_files = False for file_path in files_list.values(): if not os.path.isfile(file_path): broken_files = True if broken_files: continue # construct image with obstacles img = np.zeros((101, 101)) obstacles = (pd.read_csv(files_list['obstacles']) * scale).round(0).astype(int) #obstacles.apply(lambda x: cv2.rectangle(img, (x['x'], x['y']), (x['x']+x['width'], x['y']+x['length']), 255, -1), axis=1) obstacles_columns = obstacles.columns.to_list() obstacles['p1'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'], x['y']), axis=1) obstacles['p2'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'] + x['width'], x['y']), axis=1) obstacles['p3'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'] + x['width'], x['y'] + x['length']), axis=1) obstacles['p4'] = obstacles[obstacles_columns].astype(float).apply( lambda x: vis.Point(x['x'], x['y'] + x['length']), axis=1) obstacles['polygon'] = obstacles.apply( lambda x: vis.Polygon([x['p1'], x['p2'], x['p3'], x['p4']]), axis=1) # define bounded environment with obstacles p1 = vis.Point(0, 0) p2 = vis.Point(101, 0) p3 = vis.Point(101, 101) p4 = vis.Point(0, 101) walls_poly = vis.Polygon([p1, p2, p3, p4]) vis_env = vis.Environment([walls_poly] + obstacles['polygon'].to_list()) # load inspection points csv inspection_points = (pd.read_csv(files_list['inspection_points']) * scale).round(0).astype(int) # load configuration space csv cspace_df = pd.read_csv(files_list['configurations'], delimiter=' ', header=None).drop(columns=[0, 6]) # drop already seen points from inspection points with open(files_list['results'], 'r') as f: for line in f: vertices_idxs = [int(x) for x in line.split(' ')[1:-1]] img_copy = img.copy() for j in range(0, len(vertices_idxs) - 1): if (np.random.uniform() < 0.0): c_point = np.random.uniform(low=-np.pi, high=np.pi, size=5) if j > 0: j -= 1 else: c_point = cspace_df.iloc[j].to_numpy() # compute end-point for vertex links_val = np.rint(compute_links(c_point) * scale).astype(int) ee_val = links_val[-1] # get orientation of end effector ee_orientation = c_point.sum() # set visibility triangle x1 = ee_val[0] + max_wall * np.cos(ee_orientation + 0.5 * fov) y1 = ee_val[1] + max_wall * np.sin(ee_orientation + 0.5 * fov) x2 = ee_val[0] + max_wall * np.cos(ee_orientation - 0.5 * fov) y2 = ee_val[1] + max_wall * np.sin(ee_orientation - 0.5 * fov) vis_tri = Polygon([tuple(ee_val), (x1, y1), (x2, y2)]) # define observer if is_in_bounds(ee_val): observer = vis.Point(float(ee_val[0]), float(ee_val[1])) observer.snap_to_boundary_of(vis_env, eps) observer.snap_to_vertices_of(vis_env, eps) isovist = vis.Visibility_Polygon( observer, vis_env, eps) # get environment in points point_x, point_y = save_print(isovist) if len(point_x) == 0 or len(point_y) == 0: continue point_x.append(isovist[0].x()) point_y.append(isovist[0].y()) poly = Polygon([(x, y) for (x, y) in zip(point_x, point_y)]) visilbe_poly = poly.intersection(vis_tri) if type(visilbe_poly) == Polygon and len( list(visilbe_poly.exterior.coords)) > 0: visilbe_poly_pts = np.array( list(visilbe_poly.exterior.coords)).reshape( (-1, 1, 2)).astype(int) # draw visilbe polygon of the observer cv2.fillPoly(img_copy, [visilbe_poly_pts], 150) # draw obstacles and inspection points obstacles.apply(lambda x: cv2.rectangle( img_copy, (x['x'], x['y']), (x['x'] + x[ 'width'], x['y'] + x['length']), 255, -1), axis=1) inspection_points.apply(lambda x: cv2.rectangle( img_copy, (x['x'], x['y']), (x['x'], x['y']), 100, -1), axis=1) # add sample (x,y) imgs.append(np.expand_dims(img_copy, axis=0)) c_points.append( np.expand_dims(cspace_df.iloc[j + 1].to_numpy(), axis=0)) print(f'processed images: {counter_samples}') counter_samples += 1 if i % 100 == 0: print(f'Processed solutions: {i}') # normilize data cs_np = np.concatenate(c_points) / np.pi imgs_np = np.concatenate(imgs).reshape([len(cs_np), -1]) / 255.0 # write h5 file to the same location with h5py.File(os.path.join(data_dir, dest_name), "w") as f: f.create_dataset("images", data=imgs_np, dtype=imgs_np.dtype) f.create_dataset("cpoints", data=cs_np, dtype=cs_np.dtype)
def find_cut_space(P, v): """ Generate the cut space at v using Visilibity library. """ epsilon = 0.0000001 # Using shapely library, compute the cone of bisection # shp_polygon = shapely.geometry.polygon.orient(Polygon(*P)) # shp_cone = shapely.geometry.polygon.orient(Polygon(find_cone_of_bisection(P, v))) shp_polygon = Polygon(*P) #print("Polygon: %s"%shp_polygon) cone_of_bisection = find_cone_of_bisection(P, v) shp_cone = Polygon(find_cone_of_bisection(P, v)) #print("Cone of bisection: %s"%cone_of_bisection) shp_intersection = shp_cone.intersection(shp_polygon) #print("Intersection: %s"%shp_intersection) # import pylab as p # # # Plot the polygon itself # x, y = shp_polygon.exterior.xy # p.plot(x, y) # x, y = shp_cone.exterior.xy # p.plot(x, y) # x, y = shp_intersection.exterior.xy # p.plot(x, y) # p.show() # plot the intersection of the cone with the polygon #intersection_x, intersection_y = shp_intersection.exterior.xy #p.plot(intersection_x, intersection_y) #for interior in shp_intersection.interiors: # interior_x, interior_y = interior.xy # p.plot(interior_x, interior_y) # Plot the reflex vertex #p.plot([observer.x()], [observer.y()], 'go') #p.plot(point_x, point_y) if shp_intersection.geom_type == "MultiPolygon": shp_intersection = shp_intersection[0] #print shp_intersection elif shp_intersection.geom_type == "GeometryCollection": for shape in shp_intersection: if shape.geom_type == "Polygon": shp_intersection = shape break else: #shp_intersection = shapely.geometry.polygon.orient(shp_intersection) shp_intersection = (shp_intersection) #shp_intersection = shapely.geometry.polygon.orient(shp_cone.intersection(shp_polygon)) #Using the visilibity library, define the reflex vertex observer = vis.Point(*v) # Define the walls of intersection in Visilibity domain # To put into standard form, do this exterior_coords = shp_intersection.exterior.coords[:-1] x_min_idx, x_min = min(enumerate(exterior_coords), key=itemgetter(1)) exterior_coords = exterior_coords[7:]+exterior_coords[:7] vis_intersection_wall_points = [] for point in exterior_coords: vis_intersection_wall_points.append(vis.Point(*point)) #print 'Walls in standard form : ',vis.Polygon(vis_intersection_wall_points).is_in_standard_form() #for i in range(len(vis_intersection_wall_points)): #print vis_intersection_wall_points[i].x(), vis_intersection_wall_points[i].y() #print point.x(), point.y() # Define the holes of intersection in Visilibity domain vis_intersection_holes = [] for interior in shp_intersection.interiors: vis_intersection_hole_points = [] for point in list(interior.coords): vis_intersection_hole_points.append(vis.Point(*point)) vis_intersection_holes.append(vis_intersection_hole_points) #print 'Hole in standard form : ',vis.Polygon(vis_intersection_hole_points).is_in_standard_form() # Construct a convinient list env = [] env.append(vis.Polygon(vis_intersection_wall_points)) for hole in vis_intersection_holes: env.append(vis.Polygon(hole)) # Construct the whole envrionemt in Visilibity domain env = vis.Environment(env) # Construct the visible polygon observer.snap_to_boundary_of(env, epsilon) observer.snap_to_vertices_of(env, epsilon) vis_free_space = vis.Visibility_Polygon(observer, env, epsilon) #print vis_free_space.n() def save_print(polygon): end_pos_x = [] end_pos_y = [] for i in range(polygon.n()): x = polygon[i].x() y = polygon[i].y() end_pos_x.append(x) end_pos_y.append(y) return end_pos_x, end_pos_y point_x , point_y = save_print(vis_free_space) point_x.append(vis_free_space[0].x()) point_y.append(vis_free_space[0].y()) ### # At this point, we have visibility polygon. # Now we need to find edges of visbility polygon which are on the boundary shp_visib = shapely.geometry.polygon.orient(Polygon(zip(point_x, point_y)),-1) shp_ls_visib = LineString(shp_visib.exterior.coords[:]) shp_pl_visib = shp_ls_visib.buffer(0.001) shp_ls_exterior = LineString(shp_polygon.exterior) shp_ls_interior = [] for interior in shp_polygon.interiors: shp_ls_interior.append(LineString(interior)) # Start adding cut space on the exterior cut_space = [] common_items = [] #common_items = shp_ls_exterior.intersection(shp_ls_visib) common_items = shp_ls_exterior.intersection(shp_pl_visib) # Filter out very small segments if common_items.geom_type == "MultiLineString": for item in common_items: linestring = item.coords[:] # Examine each edge of the linestring for i in range(len(linestring)-1): edge = linestring[i:i+2] edge_ls = LineString(edge) if edge_ls.length > 0.02: cut_space.append(edge) elif common_items.geom_type == "LineString": # Examine each edge of the linestring linestring = common_items.coords[:] for i in range(len(linestring)-1): edge = linestring[i:i+2] edge_ls = LineString(edge) if edge_ls.length > 0.02: cut_space.append(edge) #print cut_space # Start adding cut space on the holes for interior in shp_polygon.interiors: common_items = interior.intersection(shp_ls_visib) if common_items.geom_type == "GeometryCollection": # print common_items for item in common_items: if item.geom_type == "LineString": cut_space.append(item.coords[:]) elif common_items.geom_type == "LineString": cut_space.append(common_items.coords[:]) #Point, LineString, GeometryCollection #print cut_space # PLOTTING # import pylab as p # # # Plot the polygon itself # x, y = shp_polygon.exterior.xy # p.plot(x, y) # # # plot the intersection of the cone with the polygon # intersection_x, intersection_y = shp_intersection.exterior.xy # p.plot(intersection_x, intersection_y) # # #for interior in shp_intersection.interiors: # # interior_x, interior_y = interior.xy # # p.plot(interior_x, interior_y) # # # Plot the reflex vertex # p.plot([observer.x()], [observer.y()], 'go') # # p.plot(point_x, point_y) # # p.show() #print cut_space return cut_space
def _convert_to_visilibity_polygon(self, points_list): vis_points = [vis.Point(x, y) for x, y in points_list] vis_polygon = vis.Polygon(vis_points) return vis_polygon