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
