def create_adjacent_lanes(map_api: MapApi):
lanes = map_api.get_all_features_as_gdf("lanes")
all_lane_section_id_set = {
lane.lane_section_id
for lane in lanes.itertuples()
}
adjacent_lanes = []
for lane_section_id in all_lane_section_id_set:
lanes_in_section = map_api.get_lanes_by_lane_section_id(
lane_section_id)
if len(lanes_in_section) < 2:
continue
for lane in lanes_in_section:
left_lane = find_adjacent_lane(lanes_in_section, lane, "left")
right_lane = find_adjacent_lane(lanes_in_section, lane, "right")
if left_lane:
adjacent_lanes.append(
create_adjacent_lane(lane.id, left_lane.id, "left"))
if right_lane:
adjacent_lanes.append(
create_adjacent_lane(lane.id, right_lane.id, "right"))
return adjacent_lanes
def create_intersect_relationships(map_api: MapApi,
base_feature_name,
target_feature_name,
offset=None):
base_features = map_api.get_all_features_as_gdf(
get_plural_name(base_feature_name))
target_features = map_api.get_all_features_as_gdf(
get_plural_name(target_feature_name))
intersect_relationships = []
for target_feature in target_features.itertuples():
target_geometry = target_feature.geometry
if offset:
assert target_geometry.geom_type == "LineString"
target_geometry = map_util.parallel_offset_wrapper(
target_geometry, offset, "left")
intersect_base_features = base_features[
base_features.geometry.intersects(target_geometry)]
for base_feature in intersect_base_features.itertuples():
intersect_relationships.append(
create_intersect_relationship(base_feature_name,
target_feature_name,
base_feature, target_feature))
return intersect_relationships
def create_adjacent_lane_boundaries(map_api: MapApi):
lane_sections = map_api.get_all_features_as_gdf("lane_sections")
lane_boundaries = map_api.get_all_features_as_gdf("lane_boundaries")
adjacent_lane_boundaries = []
for lane_section in lane_sections.itertuples():
lanes_in_section = map_api.get_lanes_by_lane_section_id(
lane_section.id)
candidate_lane_boundaries = lane_boundaries[lane_boundaries.within(
lane_section.geometry.buffer(3))]
for lane in lanes_in_section:
left_lane_boundary = find_adjacent_lane_boundary(
candidate_lane_boundaries, lane, "left")
right_lane_boundary = find_adjacent_lane_boundary(
candidate_lane_boundaries, lane, "right")
if left_lane_boundary:
adjacent_lane_boundaries.append(
create_adjacent_lane_boundary(lane.id,
left_lane_boundary.id,
"left"))
if right_lane_boundary:
adjacent_lane_boundaries.append(
create_adjacent_lane_boundary(lane.id,
right_lane_boundary.id,
"right"))
return adjacent_lane_boundaries
def align_vector_map_to_pointcloud(gpkg_path, pointcloud_path):
pcd = o3d.io.read_point_cloud(str(pointcloud_path))
points = np.asarray(pcd.points)
map_api = MapApi(gpkg_path)
for table_name in map_api.get_table_names():
align_features(map_api, table_name, points)
def get_sorted_lanes_from_left_to_right(map_api: MapApi, lanes_in_section):
left_most_lane = map_api.find_edge_lane(lanes_in_section, "left")
sorted_lanes = [left_most_lane]
base_lane = left_most_lane
while True:
right_lane = map_api.get_adjacent_lane_by_id(base_lane.id, "right")
if not right_lane:
break
else:
sorted_lanes.append(right_lane)
base_lane = right_lane
return sorted_lanes
def create_lane_section_connections(map_api: MapApi):
lanes = map_api.get_all_features_as_gdf("lanes")
all_lane_section_id_set = {lane.lane_section_id for lane in lanes.itertuples()}
lane_section_connections = []
for lane_section_id in all_lane_section_id_set:
lanes_in_section = list(lanes[lanes.lane_section_id == lane_section_id].itertuples())
next_lane_section_id_set = set()
for lane in lanes_in_section:
for next_lane in map_api.get_next_lanes_by_id(lane.id):
next_lane_section_id_set.add(next_lane.lane_section_id)
for next_lane_section_id in next_lane_section_id_set:
lane_section_connections.append(create_lane_section_connection(lane_section_id, next_lane_section_id))
return lane_section_connections
def create_lane_sections(map_api: MapApi):
lanes = map_api.get_all_features_as_gdf("lanes")
all_lane_section_id_set = {lane.lane_section_id for lane in lanes.itertuples()}
lane_sections = []
for lane_section_id in all_lane_section_id_set:
lanes_in_section = map_api.get_lanes_by_lane_section_id(lane_section_id)
sorted_lanes = map_util.get_sorted_lanes_from_left_to_right(map_api, lanes_in_section)
left_most_lane = sorted_lanes[0]
right_most_lane = sorted_lanes[-1]
# Alias
left_half_width = left_most_lane.width / 2
right_half_width = right_most_lane.width / 2
# Shift geometry using width
margin = 0.5
left_geometry = map_util.parallel_offset_wrapper(left_most_lane.geometry, left_half_width + margin, "left")
right_geometry = map_util.parallel_offset_wrapper(right_most_lane.geometry, right_half_width + margin, "right")
start_additional_points = []
end_additional_points = []
for lane in sorted_lanes:
start_additional_points.append(lane.geometry.coords[0])
for lane in reversed(sorted_lanes):
end_additional_points.append(lane.geometry.coords[-1])
exterior = [
[left_geometry.coords[0]]
+ start_additional_points
+ list(right_geometry.coords)
+ end_additional_points
+ list(reversed(left_geometry.coords))
]
interiors = []
coordinates = exterior + interiors
lane_sections.append(create_lane_section(lane_section_id, coordinates))
return lane_sections
def align_features(map_api: MapApi, table_name, points):
gdf = map_api.get_all_features_as_gdf(table_name)
if not gdf.geometry.any():
return
x_min, y_min, x_max, y_max = gdf.total_bounds
filtered_points = filter_points_by_rectangle(points, x_min, x_max, y_min,
y_max)
if table_name in ["traffic_lights"]:
height_offset = 5
else:
height_offset = 0
for i, row in gdf.iterrows():
gdf.loc[i,
"geometry"] = create_new_geometry(filtered_points,
row.geometry, height_offset)
map_api.save_gdf(table_name, gdf)
def create_empty_vector_map(gpkg_path, crs, only_mandatory):
map_api = MapApi(gpkg_path)
for table_name in map_api.get_table_names():
if only_mandatory:
if not map_api.is_mandatory(table_name):
continue
map_api.create_table(table_name, crs)
def create_lane_boundaries(map_api: MapApi):
lane_sections = map_api.get_all_features_as_gdf("lane_sections")
lane_boundaries = []
for lane_section in lane_sections.itertuples():
lanes_in_section = map_api.get_lanes_by_lane_section_id(
lane_section.id)
sorted_lanes = map_util.get_sorted_lanes_from_left_to_right(
map_api, lanes_in_section)
for i in range(len(sorted_lanes)):
lane = sorted_lanes[i]
if i == 0:
left_coordinates = map_util.parallel_offset_wrapper(
lane.geometry, lane.width / 2, "left")
lane_boundaries.append(create_lane_boundary(left_coordinates))
right_coordinates = map_util.parallel_offset_wrapper(
lane.geometry, lane.width / 2, "right")
lane_boundaries.append(create_lane_boundary(right_coordinates))
return lane_boundaries
def create_lane_connections(map_api: MapApi):
lanes = map_api.get_all_features_as_gdf("lanes")
lane_connections = []
for base_lane in lanes.itertuples():
touch_lanes = lanes[lanes.geometry.touches(base_lane.geometry)]
for touch_lane in touch_lanes.itertuples():
p_end = base_lane.geometry.coords[-1]
p_start = touch_lane.geometry.coords[0]
th_dist = 0.01
if Point(p_end).distance(Point(p_start)) < th_dist:
lane_connections.append(
create_lane_connection(base_lane.id, touch_lane.id))
return lane_connections
def vector_map_preprocessor(gpkg_path):
map_api = MapApi(gpkg_path)
map_api.create_tables_if_not_exist()
map_api.fix_schemas()
map_api.save_fiona_objects("lane_connections",
cf.create_lane_connections(map_api))
map_api.save_fiona_objects(
"lanes_stop_lines",
cf.create_intersect_relationships(map_api,
"lane",
"stop_line",
offset=0.5))
map_api.save_fiona_objects(
"lanes_crosswalks",
cf.create_intersect_relationships(map_api, "lane", "crosswalk"))
map_api.save_fiona_objects("adjacent_lanes",
cf.create_adjacent_lanes(map_api))
map_api.save_fiona_objects("lane_sections",
cf.create_lane_sections(map_api))
map_api.save_fiona_objects("lane_section_connections",
cf.create_lane_section_connections(map_api))
map_api.save_fiona_objects("lane_boundaries",
cf.create_lane_boundaries(map_api))
map_api.save_fiona_objects("adjacent_lane_boundaries",
cf.create_adjacent_lane_boundaries(map_api))