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)