def _init_regions(self) -> np.ndarray:
"""Divides the map in 1x1 m squares and finds the potentially visible segments.
This function can be further improved by considering occlusions.
Returns:
region_segments: A 2D matrix that contains the segments for each region.
"""
# Obtain map dimensions
x_min, y_min, x_max, y_max = self.bounds()
map_rows, map_cols = math.ceil(y_max - y_min), math.ceil(x_max - x_min)
# Precomputed constants to convert from (x, y) to (row, col) faster
self._XC = math.floor(map_cols / 2.0)
self._YR = map_rows - math.ceil(map_rows / 2.0)
# Find the segments visible from each region
region_segments = np.zeros((map_rows, map_cols), dtype=list)
for y in np.arange(y_max - 0.5, y_min, -1):
for x in np.arange(x_min + 0.5, x_max):
circle = Point(x,
y).buffer(self._sensor_range + 1 / math.sqrt(2))
segments = []
for segment in self._map_segments:
line = LineString(segment)
if line.intersects(circle) and not line.touches(circle):
segments.append(segment)
r, c = self._xy_to_rc((x, y))
region_segments[r][c] = segments
return region_segments
def init_graph(self):
for pre_key in self.nodes.keys():
for cur_key in self.nodes.keys():
if pre_key != cur_key:
line = LineString(
[self.nodes[pre_key], self.nodes[cur_key]])
is_cutted = False
for key in self.save_data.keys():
obstacles = Polygon(self.save_data[key])
if line.intersects(obstacles):
is_cutted = True
if not is_cutted:
self.graph_edges.append(
(pre_key, cur_key,
self.distance(self.nodes[pre_key],
self.nodes[cur_key])))
class Polyline(PolylineInterface):
def __init__(self, vertices, closed):
vertices = np.array(vertices) # NOTE this create a copy
if vertices.size > 0:
assert (len(vertices.shape) == 2)
assert (vertices.shape[0] == 3)
# remove duplicates
dup = np.all(np.isclose(vertices[:2, :-1], vertices[:2, 1:]),
axis=0)
dup = np.append(dup, False)
vertices = vertices[:, ~dup]
else:
# ensure shape is correct
vertices = np.empty(shape=(3, 0))
Polyline._init_internal(vertices, closed, self)
def _init_internal(vertices, closed, object=None):
if object is None:
object = Polyline.__new__(Polyline)
object._vertices = vertices
object._closed = closed
object._cavc_up_to_date = False
object._shapely_up_to_date = False
object._lines_up_to_date = False
return object
def _update_cavc(self):
if not self._cavc_up_to_date:
self._cavc_pline = cavc.Polyline(self._vertices, self._closed)
self._cavc_up_to_date = True
def _update_shapely(self):
if not self._shapely_up_to_date:
if self.is_closed():
self._shapely = Polygon(self.to_lines().T)
else:
self._shapely = LineString(self.to_lines().T)
self._shapely_up_to_date = True
def _update_lines(self):
if self._lines_up_to_date:
return
precision = 1e-3
points = []
n = self._vertices.shape[1]
for i in range(n - int(not self._closed)):
a = self._vertices[:2, i]
b = self._vertices[:2, (i + 1) % n]
bulge = self._vertices[2, i]
if points:
points.pop(-1)
if math.isclose(bulge, 0):
points += [a, b]
else:
rot = np.array([[0, -1], [1, 0]])
on_right = bulge >= 0
if not on_right:
rot = -rot
bulge = abs(bulge)
ab = b - a
chord = np.linalg.norm(ab)
radius = chord * (bulge + 1. / bulge) / 4
center_offset = radius - chord * bulge / 2
center = a + ab / 2 + center_offset / chord * rot.dot(ab)
a_dir = a - center
b_dir = b - center
rad_start = math.atan2(a_dir[1], a_dir[0])
rad_end = math.atan2(b_dir[1], b_dir[0])
# TODO handle case where bulge almost 0 or inf (line or circle)
if not math.isclose(rad_start, rad_end):
if on_right != (rad_start < rad_end):
if on_right:
rad_start -= 2 * math.pi
else:
rad_end -= 2 * math.pi
rad_len = abs(rad_end - rad_start)
if radius > precision:
max_angle = 2 * math.acos(1.0 - precision / radius)
else:
max_angle = math.pi
nb_segments = max(2, math.ceil(rad_len / max_angle) + 1)
angles = np.linspace(rad_start, rad_end, nb_segments + 1)
arc_data = (center.reshape(2, 1) + radius * np.vstack(
(np.cos(angles), np.sin(angles))))
points += np.transpose(arc_data).tolist()
self._lines = np.transpose(np.array(points))
self._lines_up_to_date = True
@property
def raw(self):
return self._vertices
@property
def start(self):
return self._vertices[:2, 0]
@property
def end(self):
return self._vertices[:2, -1]
@property
def bounds(self):
self._update_cavc()
min_x, min_y, max_x, max_y = self._cavc_pline.get_extents()
return np.array([[min_x, min_y], [max_x, max_y]])
@property
def centroid(self):
self._update_shapely()
return self._shapely.centroid
def is_closed(self):
return self._closed
def is_ccw(self):
self._update_cavc()
return self._cavc_pline.get_area() >= 0
# TODO
def is_simple(self):
return True
def reverse(self):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
polyline._vertices = np.flip(polyline._vertices, axis=1)
polyline._vertices[2] = -polyline._vertices[2]
if polyline._closed:
polyline._vertices[:2] = np.roll(polyline._vertices[:2], 1, axis=1)
else:
polyline._vertices[2] = np.roll(polyline._vertices[2], -1)
polyline._vertices[2, -1] = 0.
return polyline
def contains(self, object):
if not self._closed:
return False
if isinstance(object, (list, np.ndarray)):
point = np.array(object)
self._update_cavc()
return self._cavc_pline.get_winding_number(object) != 0
elif isinstance(object, Polyline):
self._update_shapely()
object._update_shapely()
return self._shapely.contains(object._shapely)
else:
raise Exception('Incorrect argument type')
def intersects(self, polyline):
self._update_shapely()
polyline._update_shapely()
return self._shapely.intersects(polyline._shapely)
def affine(self, d, r, s):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
cos = math.cos(r) * s
sin = math.sin(r)
tr_mat = np.array([[cos, -sin, d[0]], [sin, cos, d[1]], [0, 0, 1]])
vertices = polyline._vertices[:-1]
vertices = np.dot(tr_mat, np.insert(vertices, 2, 1., axis=0))
vertices[-1] = polyline._vertices[-1]
polyline._vertices = vertices
return polyline
def offset(self, offset):
self._update_cavc()
cavc_plines = self._cavc_pline.parallel_offset(offset, 0)
polylines = []
for cavc_pline in cavc_plines:
polyline = Polyline._init_internal(cavc_pline.vertex_data(),
cavc_pline.is_closed())
polyline._cavc_pline = cavc_pline
polyline._cavc_up_to_date = True
polylines.append(polyline)
return polylines
def loop(self, limit_angle, selected_radius, loop_radius):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
limit_bulge = math.tan((math.pi - limit_angle) / 4)
ids = np.where(np.abs(polyline._vertices[2]) >= limit_bulge)[0]
cur = np.take(polyline._vertices, ids, axis=1)
next_ids = (ids + 1) % polyline._vertices.shape[1]
next = np.take(polyline._vertices[:2], next_ids, axis=1)
# i, x, y, b, h_theta, vx, vy
data = np.vstack(
(ids, cur, 2 * np.arctan(np.abs(cur[2])), next - cur[:2]))
# i, x, y, b, h_theta, vx, vy, d
data = np.vstack((data, np.linalg.norm(data[-2:], axis=0)))
radius = data[7] / (2 * np.sin(data[4]))
ignored = np.where(np.logical_not(np.isclose(radius,
selected_radius)))[0]
data = np.delete(data, ignored, axis=1)
# i, x, y, b, h_theta, vx, vy, d, h
data = np.vstack((data, (data[7] + 2 * loop_radius * np.sin(data[4])) *
np.tan(data[4]) / 2))
a = data[1:3]
ab = data[5:7]
normalized_ab = ab / data[7]
ab_normal = np.array([-normalized_ab[1], normalized_ab[0]])
h = data[8]
top = a + ab / 2 + ab_normal * h
half_top_side = loop_radius * np.sin(data[4])
new_a = top + normalized_ab * half_top_side
new_b = top - normalized_ab * half_top_side
new_b = np.insert(new_b, 2, 0, axis=0)
new_a = np.vstack((new_a, -1 / data[3]))
for i in reversed(range(data.shape[1])):
id = int(data[0, i] + 0.1)
polyline._vertices[2, id] = 0
polyline._vertices = np.insert(polyline._vertices,
id + 1,
new_b[:, i],
axis=1)
polyline._vertices = np.insert(polyline._vertices,
id + 1,
new_a[:, i],
axis=1)
return polyline
def to_lines(self):
self._update_lines()
return self._lines
def link(self):
for osm_id,road in self.segment_map.items():
road_id = "r{0}".format(osm_id)
segment_link_map = {}
for segment in road:
segment_links = self.link_segment(segment,osm_id)
segment_link_map[segment] = segment_links
for (segment1,segment2) in zip(road[:-1],road[1:]):
assert(segment1[1] == segment2[0])
buildings1 = segment_link_map[segment1]
buildings2 = segment_link_map[segment2]
if segment1[1] in self.int_map:
i_id = "i{0}".format(self.int_map[segment1[1]])
self.add_edge(i_id,road_id, reverse=False)
minDistBuilding1 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[0])
b1_id = "b{0}".format(minDistBuilding1)
minDistBuilding1R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[1])
b1r_id = "b{0}".format(minDistBuilding1R)
minDistBuilding2 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[0])
b2_id = "b{0}".format(minDistBuilding2)
minDistBuilding2R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[1])
b2r_id = "b{0}".format(minDistBuilding2R)
#self.add_edge(b1_id,i_id)
#self.add_edge(b2_id,i_id)
#self.add_edge(b1r_id,i_id)
#self.add_edge(b2r_id,i_id)
self.add_edge(b1_id,b2_id, weight=0.5)
self.add_edge(b1r_id,b2r_id, weight=0.5)
nb = self.nodes[minDistBuilding1]
cp = (nb['x'],nb['y'])
#cpLatlon = to_latlon(cp)
nbR = self.nodes[minDistBuilding1R]
cpR = (nbR['x'],nbR['y'])
#cpRLatlon = to_latlon(cpR)
nb2 = self.nodes[minDistBuilding2]
cp2 = (nb2['x'],nb2['y'])
#cp2Latlon = to_latlon(cp2)
nb2R = self.nodes[minDistBuilding2R]
cp2R = (nb2R['x'],nb2R['y'])
#cp2RLatlon = to_latlon(cp2R)
edgeLine = LineString([cp, cp2])
edgeLine2 = LineString([cpR, cp2R])
roads = self.segment_lookup([cp[0],cp[1]],5)
for rd in roads:
rdLine = LineString([rd[0],rd[1]])
if edgeLine.intersects(rdLine):
self.add_edge(b1_id,i_id)
self.add_edge(b2_id,i_id)
if edgeLine2.intersects(rdLine):
self.add_edge(b1r_id,i_id)
self.add_edge(b2r_id,i_id)
else:
minDist = 99999999.9
bldPair = (buildings1[0][0],buildings2[0][-1])
for building in buildings1[0]:
bld = self.nodes[building]
minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[0])
minBld = self.nodes[minDistBuilding]
dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y'])
if dist < minDist:
minDist = dist
bldPair = (building,minDistBuilding)
b1_id = "b{0}".format(bldPair[0])
b2_id = "b{0}".format(bldPair[1])
minDist = 99999999.9
bldPair = (buildings1[1][0],buildings2[1][-1])
for building in buildings1[1]:
bld = self.nodes[building]
minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[1])
minBld = self.nodes[minDistBuilding]
dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y'])
if dist < minDist:
minDist = dist
bldPair = (building,minDistBuilding)
b1r_id = "b{0}".format(bldPair[0])
b2r_id = "b{0}".format(bldPair[1])
self.add_edge(b1_id,b2_id)
self.add_edge(b1r_id,b2r_id)
start = road[0]
end = road[-1]
buildings1 = segment_link_map[start]
buildings2 = segment_link_map[end]
if start[0] in self.int_map:
i_id = "i{0}".format(self.int_map[start[0]])
minDistBuilding = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[0])
b1_id = "b{0}".format(minDistBuilding)
minDistBuildingR = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[1])
b1r_id = "b{0}".format(minDistBuildingR)
self.add_edge(b1_id,i_id)
self.add_edge(b1r_id,i_id)
self.add_edge(b1_id,b1r_id, weight=0.5)
if end[1] in self.int_map:
i_id = "i{0}".format(self.int_map[end[1]])
minDistBuilding = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[0])
b2_id = "b{0}".format(minDistBuilding)
minDistBuildingR = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[1])
b2r_id = "b{0}".format(minDistBuildingR)
self.add_edge(b2_id,i_id)
self.add_edge(b2r_id,i_id)
self.add_edge(b2_id,b2r_id, weight=0.5)
def categorizeBuildings(self):
def isLeft(a,b,c):
return ((b[0] - a[0])*(c[1] - a[1]) - (b[1] - a[1])*(c[0] - a[0])) > 0;
def getIntersectionArea(bldPoly,line,perpLine,bldID):
if not line.intersects(bldPoly):
return 0.0
pt1 = list(line.coords)[0]
pt2 = list(line.coords)[1]
perppt1 = list(perpLine.coords)[0]
perppt2 = list(perpLine.coords)[1]
dx = perppt2[0] - perppt1[0]
dy = perppt2[1] - perppt1[1]
pt3 = (pt1[0]-dx,pt1[1]-dy)
pt4 = (pt2[0]-dx,pt2[1]-dy)
linePoly = Polygon([pt1,pt3,pt4,pt2])
try:
intersection_area = linePoly.intersection(bldPoly).area
return intersection_area/bldPoly.area
except:
return -1
def overlapsNeighbor(bldPoly,neighborPoly):
try:
intersection_area = bldPoly.intersection(neighborPoly).area
return intersection_area/bldPoly.area > 0.05
except:
return False
Rd2BldLeft = {}
Rd2BldRight = {}
for bld in self.buildingCenters.keys():
bldID = self.buildingCenters[bld][0]
#if bldID < 3700 or bldID > 3800:
#if bldID != 3751:
#continue
roads = self.segment_lookup([bld[0],bld[1]],10)
p1 = (bld[0], bld[1])
p1LatLon = to_latlon(p1)
res = self.buildingKDTree.query([bld[0],bld[1]], 20)
neighbors = []
for item in res[1][1:]: #start from index 1 because index 0 is always the original building bld
buildingID = self.buildingCenters[self.buildingCenters.keys()[item]][0]
neighbor = self.buildings[buildingID]
#this part removes a bug that some neighbor shapes have extra information, which will result in Polygon error later on.
start = neighbor[0]
for i in range(1,len(neighbor)):
if neighbor[i] == start and i > 2:
break
neighbor = neighbor[:i+1]
neighbors.append(neighbor)
roadDict = {}
bldVertices = self.buildingCenters[bld][1]
bldPolygon = Polygon(bldVertices)
for rd in roads:
rdLine = LineString([rd[0],rd[1]])
intersectsSomething = False
p3Intersect = self.perpIntersectPoint(p1,rd[0],rd[1])
if not p3Intersect:
continue
bldVertices.append(p1)
for vertex in bldVertices:
if intersectsSomething:
break
p3 = self.perpIntersectPoint(vertex,rd[0],rd[1])
vertexLatLon = to_latlon(vertex)
if p3:
p3LatLon = to_latlon(p3)
perpLine = LineString([p1,p3])
for neighbor in neighbors:
neighborPoly = Polygon(neighbor)
if overlapsNeighbor(bldPolygon,neighborPoly):
continue
if perpLine.intersects(neighborPoly):
intersectRd = False
intersectionRdArea = 0
if neighborPoly.intersects(rdLine):
intersectRd = True
intersectionRdArea = getIntersectionArea(neighborPoly,rdLine,perpLine,bldID)
if intersectionRdArea > 0.4 or not intersectRd:
#if bldID == 4287 or bldID == 4288:
#print intersectionRdArea
#road_lines.record(0)
#road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[vertexLatLon,p3LatLon]])
intersectsSomething = True
break
for rd2 in roads:
if rd2 == rd:
continue
roadLine2 = LineString([rd2[0],rd2[1]])
if perpLine.intersects(roadLine2):
intersectsSomething = True
break
if not intersectsSomething:
perpLine = LineString([p1,p3Intersect])
if perpLine.length > (3*bldPolygon.length)/4:
continue
#if rdLine.length < (bldPolygon.length/3):
# continue
p3IntersectLatLon = to_latlon(p3Intersect)
road_lines.record(0)
road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[p3IntersectLatLon,p1LatLon]])
if isLeft(rd[0],rd[1],p1):
if rd not in Rd2BldLeft:
Rd2BldLeft[rd] = [bldID]
else:
Rd2BldLeft[rd].append(bldID)
else:
if rd not in Rd2BldRight:
Rd2BldRight[rd] = [bldID]
else:
Rd2BldRight[rd].append(bldID)
return (Rd2BldLeft, Rd2BldRight)