| import os |
| import numpy as np |
| from pyquaternion import Quaternion |
| from shapely import affinity, ops |
| from shapely.geometry import LineString, box, MultiPolygon, MultiLineString |
|
|
| from nuplan.common.maps.nuplan_map.map_factory import get_maps_api |
| from nuplan.common.maps.maps_datatypes import SemanticMapLayer |
|
|
| class VectorizedLocalMap(object): |
| CLASS2LABEL = { |
| SemanticMapLayer.LANE: 0, |
| SemanticMapLayer.LANE_CONNECTOR: 0, |
| SemanticMapLayer.CROSSWALK: 1, |
| SemanticMapLayer.ROADBLOCK: 2, |
| SemanticMapLayer.INTERSECTION: 2, |
| SemanticMapLayer.CARPARK_AREA: 2, |
| SemanticMapLayer.ROADBLOCK_CONNECTOR: 2, |
| SemanticMapLayer.WALKWAYS: 3 |
| } |
| def __init__( |
| self, |
| map_root, |
| map_version='nuplan-maps-v1.0', |
| patch_size=(100, 100), |
| map_classes={ |
| 'centerline': [SemanticMapLayer.LANE, SemanticMapLayer.LANE_CONNECTOR], |
| 'ped_crossing': [SemanticMapLayer.CROSSWALK], |
| 'road_boundary': [SemanticMapLayer.ROADBLOCK, SemanticMapLayer.INTERSECTION, SemanticMapLayer.CARPARK_AREA], |
| |
| }, |
| need_merged=['road_boundary'], |
| ): |
| super().__init__() |
| self.map_classes = map_classes |
| self.patch_size = patch_size |
| self.need_merged = need_merged |
| self.MAP_APIS_DICT = { |
| "us-pa-pittsburgh-hazelwood" : get_maps_api(map_root, map_version, "us-pa-pittsburgh-hazelwood"), |
| "sg-one-north" : get_maps_api(map_root, map_version, "sg-one-north"), |
| "us-ma-boston" : get_maps_api(map_root, map_version, "us-ma-boston"), |
| "us-nv-las-vegas-strip" : get_maps_api(map_root, map_version, "us-nv-las-vegas-strip") |
| } |
|
|
| def get_patch_coord(self, patch_box, patch_angle: float = 0.0): |
| """ |
| Convert patch_box to shapely Polygon coordinates. |
| :param patch_box: Patch box defined as [x_center, y_center, height, width]. |
| :param patch_angle: Patch orientation in degrees. |
| :return: Box Polygon for patch_box. |
| """ |
| patch_x, patch_y, patch_h, patch_w = patch_box |
|
|
| x_min = patch_x - patch_w / 2.0 |
| y_min = patch_y - patch_h / 2.0 |
| x_max = patch_x + patch_w / 2.0 |
| y_max = patch_y + patch_h / 2.0 |
|
|
| patch = box(x_min, y_min, x_max, y_max) |
| patch = affinity.rotate(patch, patch_angle, origin=(patch_x, patch_y), use_radians=False) |
|
|
| return patch |
|
|
| def gen_vectorized_samples(self, e2g_T, e2g_R, map_location): |
| ''' |
| use lidar2global to get gt map layers |
| ''' |
| x, y = (e2g_T[0], e2g_T[1]) |
| patch_angle = Quaternion(e2g_R).yaw_pitch_roll[0] |
| patch_box = (x, y, self.patch_size[0], self.patch_size[1]) |
| patch_angle = patch_angle / np.pi * 180 |
|
|
| vectors = [] |
| for idx, class_name in enumerate(self.map_classes): |
| geom = self.get_map_geom(patch_box, patch_angle, self.map_classes[class_name], map_location) |
| if class_name in self.need_merged: |
| line_list = self.merge_polys_to_lines(geom) |
| else: |
| line_list = self.geoms_to_lines(geom) |
|
|
| for line in line_list: |
| vectors.append({ |
| 'pts': line.astype(np.float32), |
| 'pts_num': line.shape[0], |
| 'type': idx |
| }) |
|
|
| return vectors |
| |
| def gen_drivable_area(self, e2g_T, e2g_R, map_location,no_transform=False): |
| x, y = (e2g_T[0], e2g_T[1]) |
| patch_angle = Quaternion(e2g_R).yaw_pitch_roll[0] |
| patch_box = (x, y, self.patch_size[0], self.patch_size[1]) |
| patch_angle = patch_angle / np.pi * 180 |
|
|
| geom = self.get_map_geom(patch_box, patch_angle, self.map_classes['road_boundary'], map_location,no_transform) |
| multi_polygon = self.merge_polys_to_one(geom) |
| return multi_polygon |
|
|
| def get_map_geom(self, patch_box, patch_angle, layer_names, map_location, no_transform=False): |
| patch_x = patch_box[0] |
| patch_y = patch_box[1] |
|
|
| patch = self.get_patch_coord(patch_box, patch_angle) |
| map_api = self.MAP_APIS_DICT[map_location] |
| map_geom = [] |
| for layer_name in layer_names: |
| records = map_api._get_proximity_map_object(patch, layer_name) |
| for record in records: |
| if layer_name in [SemanticMapLayer.LANE, SemanticMapLayer.LANE_CONNECTOR]: |
| line = record.baseline_path.linestring |
| new_line = line.intersection(patch) |
| if new_line.is_empty: |
| continue |
| if not no_transform: |
| new_line = affinity.rotate(new_line, -patch_angle, origin=(patch_x, patch_y), use_radians=False) |
| new_line = affinity.affine_transform(new_line, |
| [1.0, 0.0, 0.0, 1.0, -patch_x, -patch_y]) |
| map_geom.append((layer_name, new_line)) |
| elif layer_name in [SemanticMapLayer.CROSSWALK, SemanticMapLayer.WALKWAYS, SemanticMapLayer.ROADBLOCK, SemanticMapLayer.INTERSECTION]: |
| polygon = record.polygon |
| if polygon.is_valid: |
| new_polygon = polygon.intersection(patch) |
| if new_polygon.is_empty: |
| continue |
| new_polygon = affinity.rotate(new_polygon, -patch_angle, |
| origin=(patch_x, patch_y), use_radians=False) |
| new_polygon = affinity.affine_transform(new_polygon, |
| [1.0, 0.0, 0.0, 1.0, -patch_x, -patch_y]) |
| if new_polygon.geom_type == 'Polygon': |
| new_polygon = MultiPolygon([new_polygon]) |
| map_geom.append((layer_name, new_polygon)) |
| return map_geom |
|
|
| def _one_type_line_geom_to_instances(self, line_geom): |
| line_instances = [] |
|
|
| for line in line_geom: |
| if not line.is_empty: |
| if line.geom_type == 'MultiLineString': |
| for single_line in line.geoms: |
| line_instances.append(single_line) |
| elif line.geom_type == 'LineString': |
| line_instances.append(line) |
| else: |
| print(line.geom_type) |
| raise NotImplementedError |
| |
| line_instances = [np.asarray(line.coords) for line in line_instances if len(line.coords) > 1] |
| return line_instances |
|
|
| def geoms_to_lines(self, geoms): |
| lines = [] |
| for layer_name, geom in geoms: |
| if geom.geom_type == 'MultiPolygon': |
| lines.extend(self.poly_to_lines(geom)) |
| else: |
| lines.append(geom) |
|
|
| return self._one_type_line_geom_to_instances(lines) |
|
|
| def poly_to_lines(self, poly): |
|
|
| max_x = self.patch_size[1] / 2 |
| max_y = self.patch_size[0] / 2 |
| local_patch = box(-max_x + 0.2, -max_y + 0.2, max_x - 0.2, max_y - 0.2) |
|
|
| if poly.geom_type == 'Polygon': |
| poly = MultiPolygon([poly]) |
| exteriors = [] |
| interiors = [] |
| for p in poly.geoms: |
| exteriors.append(p.exterior) |
| for inter in p.interiors: |
| interiors.append(inter) |
| |
| results = [] |
| for ext in exteriors: |
| if ext.is_ccw: |
| ext.coords = list(ext.coords)[::-1] |
| lines = ext.intersection(local_patch) |
| if isinstance(lines, MultiLineString): |
| lines = ops.linemerge(lines) |
| results.append(lines) |
|
|
| for inter in interiors: |
| if not inter.is_ccw: |
| inter.coords = list(inter.coords)[::-1] |
| lines = inter.intersection(local_patch) |
| if isinstance(lines, MultiLineString): |
| lines = ops.linemerge(lines) |
| results.append(lines) |
| return results |
|
|
| def merge_polys_to_lines(self, polygon_geom): |
| roads = [poly[1] for poly in polygon_geom] |
|
|
| exteriors = [] |
| interiors = [] |
|
|
| union_segments = ops.unary_union(roads) |
| if union_segments.geom_type != 'MultiPolygon': |
| union_segments = MultiPolygon([union_segments]) |
| for poly in union_segments.geoms: |
| exteriors.append(poly.exterior) |
| for inter in poly.interiors: |
| interiors.append(inter) |
|
|
| max_x = self.patch_size[1] / 2 |
| max_y = self.patch_size[0] / 2 |
|
|
| |
| local_patch = box(-max_x + 0.2, -max_y + 0.2, max_x - 0.2, max_y - 0.2) |
| results = [] |
| for ext in exteriors: |
| if ext.is_ccw: |
| ext.coords = list(ext.coords)[::-1] |
| lines = ext.intersection(local_patch) |
| if isinstance(lines, MultiLineString): |
| lines = ops.linemerge(lines) |
| results.append(lines) |
|
|
| for inter in interiors: |
| if not inter.is_ccw: |
| inter.coords = list(inter.coords)[::-1] |
| lines = inter.intersection(local_patch) |
| if isinstance(lines, MultiLineString): |
| lines = ops.linemerge(lines) |
| results.append(lines) |
|
|
| return self._one_type_line_geom_to_instances(results) |
|
|
| def merge_polys_to_one(self, polygon_geom): |
| roads = [poly[1] for poly in polygon_geom] |
|
|
| union_segments = ops.unary_union(roads) |
| if union_segments.geom_type != 'MultiPolygon': |
| union_segments = MultiPolygon([union_segments]) |
| |
| return union_segments |
|
|
