From bcc3b6c23480a4c66bad174c0b4b8d3672566210 Mon Sep 17 00:00:00 2001 From: Yu-Hsiang Chen <82202284+bob020416@users.noreply.github.com> Date: Thu, 23 Jul 2026 11:59:20 +0800 Subject: [PATCH 1/2] Add validated HetroD type-specific valid regions --- .../audit_hetrod_valid_region_gt.py | 446 ++++++++++++ scenarionet-converter/hetrod_regions.py | 131 ++++ scenarionet-converter/hetrod_scene.py | 171 ++++- .../visualize_hetrod_valid_regions.py | 640 ++++++++++++++++++ 4 files changed, 1380 insertions(+), 8 deletions(-) create mode 100644 scenarionet-converter/audit_hetrod_valid_region_gt.py create mode 100644 scenarionet-converter/hetrod_regions.py create mode 100644 scenarionet-converter/visualize_hetrod_valid_regions.py diff --git a/scenarionet-converter/audit_hetrod_valid_region_gt.py b/scenarionet-converter/audit_hetrod_valid_region_gt.py new file mode 100644 index 0000000..196da3d --- /dev/null +++ b/scenarionet-converter/audit_hetrod_valid_region_gt.py @@ -0,0 +1,446 @@ +#!/usr/bin/env python3 +"""Audit HetroD lenient valid regions against all ScenarioNet GT states.""" + +from __future__ import annotations + +import argparse +import json +import pickle +import re +import sys +from collections import defaultdict +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.patches import Polygon as PolygonPatch +from shapely.affinity import affine_transform +from shapely import intersects_xy + +from visualize_hetrod_valid_regions import ( + AGENT_REGION_TYPES, + TYPE_COLORS, + build_agent_region_geometries, + parse_osm, + polygon_components, +) +from hetrod_regions import ( + HETROD_VALID_REGION_MARGIN_M, + HETROD_VALID_REGION_POLICY, +) + + +# NumPy 2 pickles refer to numpy._core while the CAT-K environment uses NumPy 1. +if not hasattr(np, "_core"): + import numpy.core + import numpy.core.multiarray + import numpy.core.numeric + + sys.modules.setdefault("numpy._core", numpy.core) + sys.modules.setdefault("numpy._core.multiarray", numpy.core.multiarray) + sys.modules.setdefault("numpy._core.numeric", numpy.core.numeric) + + +FUTURE_START_INDEX = 11 +DIRECT_WAY_TYPES = { + "line_thin", + "line_thick", + "virtual", + "wall", + "curbstone", + "fence", + "road_border", +} +TRACK_TO_REGION_TYPE = { + "VEHICLE": "vehicle", + "CYCLIST": "cyclist", + "PEDESTRIAN": "pedestrian", +} + + +def load_pickle(path: Path): + with path.open("rb") as handle: + return pickle.load(handle) + + +def location_from_scenario_id(scenario_id: str) -> str: + match = re.search(r"_loc([0-5])_", scenario_id) + if not match: + raise ValueError(f"Cannot infer location from {scenario_id}") + return f"location{match.group(1)}" + + +def fit_raw_to_scenario_affine( + parsed_map: dict[str, object], + scenario: dict, +) -> tuple[np.ndarray, dict[str, float]]: + raw_points = [] + scenario_points = [] + for way_id, way in parsed_map["ways"].items(): + if way["tags"].get("type") not in DIRECT_WAY_TYPES: + continue + feature = scenario["map_features"].get(str(way_id)) + if not feature or "polyline" not in feature: + continue + raw = np.asarray(way["points"], dtype=np.float64) + transformed = np.asarray(feature["polyline"], dtype=np.float64)[:, :2] + if raw.shape != transformed.shape: + continue + raw_points.append(raw) + scenario_points.append(transformed) + if not raw_points: + raise ValueError(f"No direct OSM way matches in scenario {scenario['id']}") + + raw = np.concatenate(raw_points, axis=0) + transformed = np.concatenate(scenario_points, axis=0) + design = np.column_stack([raw, np.ones(len(raw))]) + coefficients = np.linalg.lstsq(design, transformed, rcond=None)[0] + errors = np.linalg.norm(design @ coefficients - transformed, axis=1) + return coefficients, { + "num_alignment_points": int(len(raw)), + "rmse_m": float(np.sqrt(np.mean(errors**2))), + "max_error_m": float(errors.max()), + } + + +def shapely_affine_parameters(coefficients: np.ndarray) -> list[float]: + return [ + float(coefficients[0, 0]), + float(coefficients[1, 0]), + float(coefficients[0, 1]), + float(coefficients[1, 1]), + float(coefficients[2, 0]), + float(coefficients[2, 1]), + ] + + +def inverse_transform_points(points: np.ndarray, coefficients: np.ndarray) -> np.ndarray: + homogeneous = np.eye(3, dtype=np.float64) + homogeneous[:2, :2] = coefficients[:2, :].T + homogeneous[:2, 2] = coefficients[2, :] + inverse = np.linalg.inv(homogeneous) + return ( + np.column_stack([points, np.ones(len(points))]) @ inverse.T + )[:, :2] + + +def box_corners( + centers: np.ndarray, + headings: np.ndarray, + lengths: np.ndarray, + widths: np.ndarray, +) -> np.ndarray: + forward = np.column_stack([np.cos(headings), np.sin(headings)]) + lateral = np.column_stack([-np.sin(headings), np.cos(headings)]) + forward *= (0.5 * lengths)[:, None] + lateral *= (0.5 * widths)[:, None] + return np.stack( + [ + centers + forward + lateral, + centers + forward - lateral, + centers - forward + lateral, + centers - forward - lateral, + ], + axis=1, + ) + + +def points_inside(geometry, points: np.ndarray) -> np.ndarray: + return np.asarray(intersects_xy(geometry, points[:, 0], points[:, 1]), dtype=bool) + + +def new_counter() -> dict[str, int]: + return { + "num_scenarios": 0, + "num_agents": 0, + "num_agent_steps": 0, + "num_center_inside": 0, + "num_footprint_inside": 0, + "num_agents_all_centers_inside": 0, + "num_agents_all_footprints_inside": 0, + } + + +def finalize_counter(counter: dict[str, int]) -> dict[str, float | int]: + steps = counter["num_agent_steps"] + agents = counter["num_agents"] + return { + **counter, + "center_inside_rate": ( + counter["num_center_inside"] / steps if steps else None + ), + "footprint_inside_rate": ( + counter["num_footprint_inside"] / steps if steps else None + ), + "agents_all_centers_inside_rate": ( + counter["num_agents_all_centers_inside"] / agents if agents else None + ), + "agents_all_footprints_inside_rate": ( + counter["num_agents_all_footprints_inside"] / agents if agents else None + ), + } + + +def draw_audit_panel( + ax, + geometry, + point_groups: dict[str, list[np.ndarray]], + title: str, +) -> None: + for polygon in polygon_components(geometry): + ax.add_patch( + PolygonPatch( + np.asarray(polygon.exterior.coords), + closed=True, + facecolor="#DCEAF4", + edgecolor="#333333", + linewidth=0.7, + alpha=0.75, + ) + ) + for interior in polygon.interiors: + ax.add_patch( + PolygonPatch( + np.asarray(interior.coords), + closed=True, + facecolor="white", + edgecolor="#555555", + linewidth=0.6, + ) + ) + + styles = { + "inside": ("#4D4D4D", 1.0, 0.08, "footprint inside"), + "corner_outside": ("#F39C12", 5.0, 0.45, "center in, corner out"), + "center_outside": ("#D62728", 8.0, 0.72, "center outside"), + } + for key, (color, size, alpha, label) in styles.items(): + arrays = point_groups.get(key, []) + if not arrays: + continue + points = np.concatenate(arrays, axis=0) + if len(points) > 100_000: + stride = int(np.ceil(len(points) / 100_000)) + points = points[::stride] + ax.scatter( + points[:, 0], + points[:, 1], + s=size, + c=color, + alpha=alpha, + linewidths=0, + label=label, + zorder=4, + ) + min_x, min_y, max_x, max_y = geometry.bounds + margin = max(8.0, 0.04 * max(max_x - min_x, max_y - min_y)) + ax.set_xlim(min_x - margin, max_x + margin) + ax.set_ylim(min_y - margin, max_y + margin) + ax.set_aspect("equal", adjustable="box") + ax.grid(color="#DDDDDD", linestyle=":", linewidth=0.4) + ax.set_title(title, fontsize=9, fontweight="bold") + ax.tick_params(labelsize=6) + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--gt-dir", required=True, type=Path) + parser.add_argument("--maps-dir", required=True, type=Path) + parser.add_argument("--output-dir", required=True, type=Path) + parser.add_argument( + "--shared-surface", + action="store_true", + help=( + "Audit a permissive physical-surface policy: cyclists may use all " + "mapped vehicle/cyclist/pedestrian surfaces and pedestrians may use " + "all mapped surfaces. Vehicle regions remain unchanged." + ), + ) + parser.add_argument( + "--footprint-margin-m", + type=float, + default=0.0, + help="Optional diagnostic outward margin around each valid region.", + ) + parser.add_argument("--vehicle-margin-m", type=float) + parser.add_argument("--cyclist-margin-m", type=float) + parser.add_argument("--pedestrian-margin-m", type=float) + args = parser.parse_args() + args.output_dir.mkdir(parents=True, exist_ok=True) + + parsed_maps = {} + raw_regions = {} + margins_by_type = { + "vehicle": ( + args.footprint_margin_m + if args.vehicle_margin_m is None + else args.vehicle_margin_m + ), + "cyclist": ( + args.footprint_margin_m + if args.cyclist_margin_m is None + else args.cyclist_margin_m + ), + "pedestrian": ( + args.footprint_margin_m + if args.pedestrian_margin_m is None + else args.pedestrian_margin_m + ), + } + for index in range(6): + location = f"location{index}" + parsed = parse_osm(args.maps_dir / f"{location}.osm") + parsed_maps[location] = parsed + regions = build_agent_region_geometries( + parsed, + shared_surface=args.shared_surface, + margin_m=args.footprint_margin_m, + margins_by_type=margins_by_type, + ) + raw_regions[location] = regions + + counters = defaultdict(new_counter) + scenario_sets = defaultdict(set) + point_groups = defaultdict(lambda: defaultdict(list)) + alignment_reports = [] + scenario_paths = sorted( + path + for path in args.gt_dir.glob("*.pkl") + if path.name not in {"dataset_summary.pkl", "dataset_mapping.pkl"} + ) + for scenario_index, path in enumerate(scenario_paths, start=1): + scenario = load_pickle(path) + scenario_id = str(scenario["id"]) + location = location_from_scenario_id(scenario_id) + coefficients, alignment = fit_raw_to_scenario_affine( + parsed_maps[location], scenario + ) + alignment_reports.append({"scenario_id": scenario_id, **alignment}) + affine_parameters = shapely_affine_parameters(coefficients) + + scenario_types = set() + for track in scenario["tracks"].values(): + region_type = TRACK_TO_REGION_TYPE.get(str(track.get("type", ""))) + if region_type is None: + continue + state = track["state"] + validity = np.asarray(state["valid"], dtype=bool) + validity[:FUTURE_START_INDEX] = False + if not validity.any(): + continue + scenario_types.add(region_type) + geometry = affine_transform( + raw_regions[location][region_type], affine_parameters + ) + centers = np.asarray(state["position"], dtype=np.float64)[validity, :2] + headings = np.asarray(state["heading"], dtype=np.float64)[validity] + lengths = np.asarray(state["length"], dtype=np.float64)[validity] + widths = np.asarray(state["width"], dtype=np.float64)[validity] + center_inside = points_inside(geometry, centers) + corners = box_corners(centers, headings, lengths, widths) + corner_inside = points_inside(geometry, corners.reshape(-1, 2)).reshape( + -1, 4 + ) + footprint_inside = corner_inside.all(axis=1) + + key = (location, region_type) + counter = counters[key] + counter["num_agents"] += 1 + counter["num_agent_steps"] += int(len(centers)) + counter["num_center_inside"] += int(center_inside.sum()) + counter["num_footprint_inside"] += int(footprint_inside.sum()) + counter["num_agents_all_centers_inside"] += int(center_inside.all()) + counter["num_agents_all_footprints_inside"] += int( + footprint_inside.all() + ) + + raw_centers = inverse_transform_points(centers, coefficients) + point_groups[key]["inside"].append(raw_centers[footprint_inside]) + point_groups[key]["corner_outside"].append( + raw_centers[center_inside & ~footprint_inside] + ) + point_groups[key]["center_outside"].append( + raw_centers[~center_inside] + ) + + for region_type in scenario_types: + scenario_sets[(location, region_type)].add(scenario_id) + + if scenario_index % 100 == 0: + print(f"Audited {scenario_index}/{len(scenario_paths)} scenarios") + + results = {} + for location in parsed_maps: + results[location] = {} + for region_type in AGENT_REGION_TYPES: + key = (location, region_type) + counters[key]["num_scenarios"] = len(scenario_sets[key]) + results[location][region_type] = finalize_counter(counters[key]) + + alignment_rmse = np.asarray( + [report["rmse_m"] for report in alignment_reports], dtype=np.float64 + ) + alignment_max = np.asarray( + [report["max_error_m"] for report in alignment_reports], dtype=np.float64 + ) + report = { + "policy": ( + "diagnostic-shared-surface" + if args.shared_surface + else ( + HETROD_VALID_REGION_POLICY + if margins_by_type == HETROD_VALID_REGION_MARGIN_M + else "diagnostic-type-specific" + ) + ), + "footprint_margin_m": args.footprint_margin_m, + "margins_by_type_m": margins_by_type, + "split": args.gt_dir.parent.name, + "future_start_index": FUTURE_START_INDEX, + "num_scenarios": len(scenario_paths), + "coverage": results, + "alignment": { + "mean_rmse_m": float(alignment_rmse.mean()), + "max_rmse_m": float(alignment_rmse.max()), + "max_point_error_m": float(alignment_max.max()), + }, + } + report_path = args.output_dir / "gt_valid_region_coverage.json" + report_path.write_text(json.dumps(report, indent=2), encoding="utf-8") + + fig, axes = plt.subplots(6, 3, figsize=(15, 25), dpi=130) + for row, location in enumerate(parsed_maps): + for column, region_type in enumerate(AGENT_REGION_TYPES): + stats = results[location][region_type] + center_rate = stats["center_inside_rate"] + footprint_rate = stats["footprint_inside_rate"] + title = ( + f"{region_type.title()} · center {center_rate:.2%} · " + f"footprint {footprint_rate:.2%}" + if center_rate is not None + else f"{region_type.title()} · no GT" + ) + draw_audit_panel( + axes[row, column], + raw_regions[location][region_type], + point_groups[(location, region_type)], + title, + ) + axes[row, 0].set_ylabel(location, fontweight="bold") + handles, labels = axes[0, 0].get_legend_handles_labels() + fig.legend(handles, labels, loc="lower center", ncol=3) + fig.suptitle( + f"HetroD {report['policy']} valid-region GT audit · " + f"{len(scenario_paths)} validation scenarios", + fontsize=16, + fontweight="bold", + ) + fig.tight_layout(rect=(0, 0.025, 1, 0.975)) + fig.savefig(args.output_dir / "all_locations_gt_coverage.png") + plt.close(fig) + print(f"Wrote {report_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scenarionet-converter/hetrod_regions.py b/scenarionet-converter/hetrod_regions.py new file mode 100644 index 0000000..a5c00be --- /dev/null +++ b/scenarionet-converter/hetrod_regions.py @@ -0,0 +1,131 @@ +"""HetroD lanelet semantics used by conversion and region visualization.""" + +from __future__ import annotations + +from collections.abc import Iterable + + +HETROD_VALID_REGION_SCHEMA_VERSION = "1.2" +HETROD_VALID_REGION_POLICY = "type-specific-v2-buffered" +HETROD_LENIENT_MIN_HOLE_AREA_M2 = 5.0 +HETROD_VALID_REGION_MARGIN_M = { + "vehicle": 0.75, + "cyclist": 0.75, + "pedestrian": 2.0, +} + +VEHICLE_REGION_SUBTYPES = frozenset( + { + "road", + "intersection", + "parking", + "emergency_lane", + } +) +CYCLIST_ONLY_REGION_SUBTYPES = frozenset( + { + "cycleway", + "bicycle_lane", + "bike_lane", + } +) +PEDESTRIAN_REGION_SUBTYPES = frozenset( + { + "walkway", + "crosswalk", + "footway", + "pedestrian", + } +) +SHARED_LENIENT_REGION_SUBTYPES = frozenset({"freespace"}) + +AGENT_REGION_TYPES = ("vehicle", "cyclist", "pedestrian") + + +def normalize_lanelet_subtype(subtype: object) -> str: + """Return a stable lowercase subtype for OSM/JOSM lanelet tags.""" + value = str(subtype or "unknown").strip().lower() + return value or "unknown" + + +def valid_agent_types_for_lanelet(subtype: object) -> tuple[str, ...]: + """Return the HetroD agent types allowed by one lanelet subtype.""" + normalized = normalize_lanelet_subtype(subtype) + valid_for: list[str] = [] + if normalized in VEHICLE_REGION_SUBTYPES: + valid_for.extend(("vehicle", "cyclist")) + if normalized in CYCLIST_ONLY_REGION_SUBTYPES: + valid_for.append("cyclist") + if normalized in PEDESTRIAN_REGION_SUBTYPES: + valid_for.append("pedestrian") + if normalized in SHARED_LENIENT_REGION_SUBTYPES: + valid_for.extend(AGENT_REGION_TYPES) + return tuple(valid_for) + + +def is_review_only_subtype(subtype: object) -> bool: + return False + + +def infer_lanelet_subtype(tags: dict[str, object]) -> tuple[str, str | None]: + """Infer the two known missing-subtype patterns in the challenge maps.""" + subtype = normalize_lanelet_subtype(tags.get("subtype")) + if subtype != "unknown": + return subtype, None + + bicycle = str(tags.get("bicycle", "")).strip().lower() + one_way = str(tags.get("one_way", "")).strip().lower() + motorcycle = str(tags.get("participant:vehicle:motorcycle", "")).strip().lower() + speed_limit = str(tags.get("speed_limit", "")).strip() + if bicycle == "no" and one_way == "no": + return "walkway", "missing_subtype_inferred_from_bicycle_no_and_one_way_no" + if one_way == "yes" and (motorcycle == "yes" or speed_limit): + return "road", "missing_subtype_inferred_from_motor_vehicle_tags" + return subtype, "missing_subtype_unresolved" + + +def valid_agent_types_for_osm_tags( + tags: dict[str, object], +) -> tuple[str, tuple[str, ...], str | None]: + subtype, inference = infer_lanelet_subtype(tags) + return subtype, valid_agent_types_for_lanelet(subtype), inference + + +def region_definition() -> dict[str, object]: + """Serializable description embedded in generated HetroD scenarios.""" + return { + "schema_version": HETROD_VALID_REGION_SCHEMA_VERSION, + "policy": HETROD_VALID_REGION_POLICY, + "vehicle_lanelet_subtypes": sorted(VEHICLE_REGION_SUBTYPES), + "cyclist_lanelet_subtypes": sorted( + VEHICLE_REGION_SUBTYPES | CYCLIST_ONLY_REGION_SUBTYPES + ), + "pedestrian_lanelet_subtypes": sorted(PEDESTRIAN_REGION_SUBTYPES), + "shared_lenient_lanelet_subtypes": sorted(SHARED_LENIENT_REGION_SUBTYPES), + "crosswalk_is_pedestrian_valid": True, + "unknown_inference_rules": { + "bicycle=no and one_way=no": "walkway", + "one_way=yes and motorcycle=yes or speed_limit present": "road", + }, + "polygon_encoding": { + "exterior": "float32[N,2]", + "holes": "list[float32[M,2]]", + }, + "minimum_preserved_hole_area_m2": HETROD_LENIENT_MIN_HOLE_AREA_M2, + "boundary_margin_m_by_agent_type": dict(HETROD_VALID_REGION_MARGIN_M), + "boundary_margin_rationale": ( + "Minimum validated type-specific margins yielding at least 90% " + "pedestrian GT center coverage in every represented validation location." + ), + } + + +def count_region_memberships( + memberships: Iterable[Iterable[str]], +) -> dict[str, int]: + counts = {agent_type: 0 for agent_type in AGENT_REGION_TYPES} + for valid_for in memberships: + for agent_type in valid_for: + if agent_type in counts: + counts[agent_type] += 1 + return counts diff --git a/scenarionet-converter/hetrod_scene.py b/scenarionet-converter/hetrod_scene.py index 1f4699e..6a741be 100644 --- a/scenarionet-converter/hetrod_scene.py +++ b/scenarionet-converter/hetrod_scene.py @@ -15,6 +15,14 @@ from scipy.interpolate import interp1d import utm +from hetrod_regions import ( + HETROD_LENIENT_MIN_HOLE_AREA_M2, + HETROD_VALID_REGION_MARGIN_M, + HETROD_VALID_REGION_SCHEMA_VERSION, + region_definition, + valid_agent_types_for_osm_tags, +) + try: from metadrive.scenario import ScenarioDescription as SD from metadrive.type import MetaDriveType @@ -417,30 +425,105 @@ def build_feature_from_way(way_id, wdata): if feature is not None: map_features[way_id] = feature + def polygon_components(geometry): + if geometry.is_empty: + return [] + if geometry.geom_type == "Polygon": + return [geometry] + if geometry.geom_type in {"MultiPolygon", "GeometryCollection"}: + polygons = [] + for component in geometry.geoms: + polygons.extend(polygon_components(component)) + return polygons + return [] + + def serialize_region_geometry(geometries, margin_m=0.0): + if not geometries: + return [] + merged = unary_union(geometries) + if margin_m: + merged = merged.buffer(float(margin_m)) + records = [] + for polygon in polygon_components(merged): + preserved_interiors = [ + interior + for interior in polygon.interiors + if Polygon(interior).area >= HETROD_LENIENT_MIN_HOLE_AREA_M2 + ] + polygon = Polygon( + polygon.exterior.coords, + holes=[interior.coords for interior in preserved_interiors], + ) + polygon = orient(polygon, sign=1.0) + records.append( + { + "exterior": np.asarray( + polygon.exterior.coords, dtype=np.float32 + ), + "holes": [ + np.asarray(interior.coords, dtype=np.float32) + for interior in polygon.interiors + ], + } + ) + records.sort( + key=lambda record: ( + -Polygon(record["exterior"]).area, + float(record["exterior"][:, 0].min()), + float(record["exterior"][:, 1].min()), + ) + ) + return records + + region_only_geometries = { + "vehicle": [], + "cyclist": [], + "pedestrian": [], + } + for rel in relations: if rel["tags"].get("type") != "multipolygon": continue - if rel["tags"].get("subtype") != "building": + multipolygon_subtype, valid_for, _ = valid_agent_types_for_osm_tags( + rel["tags"] + ) + is_building = multipolygon_subtype == "building" + if not is_building and not valid_for: continue outer_lines = [] + inner_lines = [] for member in rel["members"]: - if member["type"] != "way" or member.get("role") != "outer": + if member["type"] != "way" or member.get("role") not in {"outer", "inner"}: continue way = ways.get(member["ref"]) if way is None: continue coords = [nodes[node_id][:2] for node_id in way["nd_refs"] if node_id in nodes] if len(coords) >= 2: - outer_lines.append(LineString(coords)) + target = outer_lines if member.get("role") == "outer" else inner_lines + target.append(LineString(coords)) if not outer_lines: continue - building_polygons = list(polygonize(unary_union(outer_lines))) - building_polygons.sort( + relation_polygons = list(polygonize(unary_union(outer_lines))) + relation_polygons.sort( key=lambda polygon: (-polygon.area, polygon.bounds[0], polygon.bounds[1]) ) - for polygon_index, polygon in enumerate(building_polygons): + if not is_building: + relation_geometry = unary_union(relation_polygons) + if inner_lines: + inner_polygons = list(polygonize(unary_union(inner_lines))) + if inner_polygons: + relation_geometry = relation_geometry.difference( + unary_union(inner_polygons) + ) + for agent_type in valid_for: + region_only_geometries[agent_type].extend( + polygon_components(relation_geometry) + ) + continue + for polygon_index, polygon in enumerate(relation_polygons): # The valid traffic region is outside a building. Clockwise exterior # rings put that exterior on the left side of the directed edge. polygon = orient(polygon, sign=-1.0) @@ -528,10 +611,21 @@ def is_left_on_left(centerline, left, step=5): lane_poly = lane_poly.buffer(0) speed_limit_kmh = parse_speed_limit_kmh(rel["tags"]) + lanelet_subtype, valid_for, subtype_inference = ( + valid_agent_types_for_osm_tags(rel["tags"]) + ) lane_id = f"{rel['id']}" map_features[lane_id] = { "type": MetaDriveType.LANE_SURFACE_STREET, "polyline": center.astype(np.float32), + # Keep standard ScenarioNet lane semantics for existing consumers, + # while retaining the complete lanelet surface for HetroD's + # type-specific valid-region evaluator. + "polygon": np.asarray(lane_poly.exterior.coords, dtype=np.float32), + "hetrod_lanelet_subtype": lanelet_subtype, + "hetrod_valid_for": list(valid_for), + "hetrod_subtype_inference": subtype_inference, + "hetrod_valid_region_schema_version": HETROD_VALID_REGION_SCHEMA_VERSION, "entry_lanes": [], "exit_lanes": [], "left_neighbor": [], @@ -623,7 +717,39 @@ def is_left_on_left(centerline, left, step=5): map_features[lane_id]["entry_lanes"] = entry_lanes map_features[lane_id]["exit_lanes"] = exit_lanes - return map_features, (0, 0) + valid_regions = { + **region_definition(), + "vehicle": [], + "cyclist": [], + "pedestrian": [], + } + region_geometries = { + agent_type: list(geometries) + for agent_type, geometries in region_only_geometries.items() + } + for feature in map_features.values(): + polygon = feature.get("polygon") + if not isinstance(polygon, np.ndarray) or len(polygon) < 3: + continue + valid_for = list(feature.get("hetrod_valid_for", [])) + if feature.get("type") == MetaDriveType.CROSSWALK: + valid_for.append("pedestrian") + for agent_type in set(valid_for): + if agent_type in region_geometries: + geometry = Polygon(np.asarray(polygon)[:, :2]) + if not geometry.is_valid: + geometry = geometry.buffer(0) + region_geometries[agent_type].extend( + polygon_components(geometry) + ) + + for agent_type, geometries in region_geometries.items(): + valid_regions[agent_type] = serialize_region_geometry( + geometries, + margin_m=HETROD_VALID_REGION_MARGIN_M[agent_type], + ) + + return map_features, (0, 0), valid_regions def get_osm_map_for_location(loc_id, osm_file, xUtmOrigin, yUtmOrigin): @@ -692,6 +818,7 @@ def create_scenario_from_csv( scenario_data, map_features, map_center, + valid_regions, scenario_id, dataset_version, xUtmOrigin, @@ -709,6 +836,7 @@ def create_scenario_from_csv( scenario[SD.METADATA]["scenario_id"] = scenario_id scenario[SD.METADATA]["metadrive_processed"] = False scenario[SD.METADATA]["id"] = scenario_id + scenario[SD.METADATA]["hetrod_valid_regions"] = copy.deepcopy(valid_regions) scenario_map = copy.deepcopy(map_features) frames = list(sampled_frames) @@ -881,6 +1009,7 @@ def create_scenario_from_csv( scenario[SD.METADATA]["objects_of_interest"] = [] scenario[SD.METADATA]["source_file"] = source_file or "hetroD_tracks.csv" scenario[SD.METADATA]["track_length"] = num_frames + scenario[SD.METADATA]["hetrod_valid_region_definition"] = region_definition() scenario_variants = [] for agent_id in [fallback_id]: @@ -904,6 +1033,18 @@ def create_scenario_from_csv( first_i = int(np.where(sdc_track["valid"] > 0)[0][0]) origin_xy = sdc_track["position"][first_i, :2] + for agent_type in ("vehicle", "cyclist", "pedestrian"): + records = sc[SD.METADATA]["hetrod_valid_regions"].get(agent_type, []) + for record in records: + record["exterior"] = np.asarray( + record["exterior"], dtype=np.float32 + ).copy() + record["exterior"][:, :2] -= origin_xy + for index, hole in enumerate(record.get("holes", [])): + hole = np.asarray(hole, dtype=np.float32).copy() + hole[:, :2] -= origin_xy + record["holes"][index] = hole + for feat in sc[SD.MAP_FEATURES].values(): for k in ("polyline", "polygon"): if k in feat and isinstance(feat[k], np.ndarray): @@ -929,6 +1070,17 @@ def create_scenario_from_csv( c, s = math.cos(-psi0), math.sin(-psi0) R = np.array([[c, -s], [s, c]], dtype=float) + for agent_type in ("vehicle", "cyclist", "pedestrian"): + records = sc[SD.METADATA]["hetrod_valid_regions"].get(agent_type, []) + for record in records: + exterior = np.asarray(record["exterior"], dtype=np.float32).copy() + exterior[:, :2] = (R @ exterior[:, :2].T).T + record["exterior"] = exterior + for index, hole in enumerate(record.get("holes", [])): + hole = np.asarray(hole, dtype=np.float32).copy() + hole[:, :2] = (R @ hole[:, :2].T).T + record["holes"][index] = hole + for feat in sc[SD.MAP_FEATURES].values(): for k in ("polyline", "polygon"): if k in feat and isinstance(feat[k], np.ndarray): @@ -1059,7 +1211,9 @@ def convert_prefix_to_scenarios(prefix, data_dir, maps_dir, dataset_name, datase if osm_file is None: raise FileNotFoundError(f"No OSM file found for locationId={loc_id}") - map_features, map_center = get_osm_map_for_location(loc_id, osm_file, xUtm, yUtm) + map_features, map_center, valid_regions = get_osm_map_for_location( + loc_id, osm_file, xUtm, yUtm + ) entries = [] for i, seg in enumerate(segments, start=1): @@ -1068,6 +1222,7 @@ def convert_prefix_to_scenarios(prefix, data_dir, maps_dir, dataset_name, datase seg["rows"], map_features, map_center, + valid_regions, scenario_id, dataset_version, xUtm, diff --git a/scenarionet-converter/visualize_hetrod_valid_regions.py b/scenarionet-converter/visualize_hetrod_valid_regions.py new file mode 100644 index 0000000..34d91ea --- /dev/null +++ b/scenarionet-converter/visualize_hetrod_valid_regions.py @@ -0,0 +1,640 @@ +#!/usr/bin/env python3 +"""Visualize candidate type-specific valid regions in HetroD OSM maps. + +This intentionally reads the OSM maps directly so organizers can audit the +semantic conversion before rebuilding any ScenarioNet ground truth. +""" + +from __future__ import annotations + +import argparse +import json +import math +import xml.etree.ElementTree as ET +from collections import Counter +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.patches import Patch, Polygon as PolygonPatch +from shapely.geometry import Polygon +from shapely.ops import unary_union + +from hetrod_regions import ( + AGENT_REGION_TYPES, + HETROD_LENIENT_MIN_HOLE_AREA_M2, + HETROD_VALID_REGION_MARGIN_M, + HETROD_VALID_REGION_POLICY, + valid_agent_types_for_osm_tags, +) + + +TYPE_COLORS = { + "vehicle": "#2878B5", + "cyclist": "#2CA25F", + "pedestrian": "#F28E2B", +} +BACKGROUND_COLOR = "#D9D9D9" +FREESPACE_COLOR = "#8E63B0" +BOUNDARY_COLOR = "#262626" +EARTH_RADIUS_M = 6_378_137.0 + + +def local_xy(lat: np.ndarray, lon: np.ndarray) -> np.ndarray: + """Project one small map with a local equirectangular approximation.""" + lat0 = math.radians(float(np.mean(lat))) + lon0 = math.radians(float(np.mean(lon))) + x = EARTH_RADIUS_M * (np.radians(lon) - lon0) * math.cos(lat0) + y = EARTH_RADIUS_M * (np.radians(lat) - math.radians(float(np.mean(lat)))) + return np.column_stack([x, y]) + + +def resample_polyline(points: np.ndarray, count: int) -> np.ndarray: + if len(points) == count: + return points.copy() + distances = np.concatenate( + [[0.0], np.cumsum(np.linalg.norm(np.diff(points, axis=0), axis=1))] + ) + if distances[-1] <= 1e-9: + return np.repeat(points[:1], count, axis=0) + targets = np.linspace(0.0, distances[-1], count) + return np.column_stack( + [np.interp(targets, distances, points[:, axis]) for axis in range(2)] + ) + + +def join_way_parts(parts: list[np.ndarray]) -> np.ndarray | None: + """Join lanelet boundary ways by their nearest endpoints.""" + remaining = [part for part in parts if len(part) >= 2] + if not remaining: + return None + joined = remaining.pop(0).copy() + while remaining: + candidates = [] + for index, part in enumerate(remaining): + candidates.extend( + [ + (np.linalg.norm(joined[-1] - part[0]), index, False, False), + (np.linalg.norm(joined[-1] - part[-1]), index, True, False), + (np.linalg.norm(joined[0] - part[-1]), index, False, True), + (np.linalg.norm(joined[0] - part[0]), index, True, True), + ] + ) + _, index, reverse, prepend = min(candidates, key=lambda item: item[0]) + part = remaining.pop(index) + if reverse: + part = part[::-1] + if prepend: + joined = np.vstack([part[:-1], joined]) + else: + joined = np.vstack([joined, part[1:]]) + return joined + + +def closed_rings_from_way_parts( + parts: list[np.ndarray], + endpoint_tolerance_m: float = 0.2, +) -> list[np.ndarray]: + """Assemble multipolygon outer members without requiring Shapely.""" + remaining = [part.copy() for part in parts if len(part) >= 2] + rings = [] + while remaining: + joined = remaining.pop(0) + while remaining and not np.allclose( + joined[0], joined[-1], atol=endpoint_tolerance_m + ): + candidates = [] + for index, part in enumerate(remaining): + candidates.extend( + [ + (np.linalg.norm(joined[-1] - part[0]), index, False), + (np.linalg.norm(joined[-1] - part[-1]), index, True), + ] + ) + distance, index, reverse = min(candidates, key=lambda item: item[0]) + if distance > endpoint_tolerance_m: + break + part = remaining.pop(index) + if reverse: + part = part[::-1] + joined = np.vstack([joined, part[1:]]) + if not np.allclose(joined[0], joined[-1], atol=endpoint_tolerance_m): + joined = np.vstack([joined, joined[:1]]) + if len(joined) >= 4: + rings.append(joined) + return rings + + +def polygon_from_boundaries( + left: np.ndarray | None, + right: np.ndarray | None, +) -> np.ndarray | None: + if left is None or right is None: + return None + if np.dot(left[-1] - left[0], right[-1] - right[0]) < 0: + right = right[::-1] + count = max(len(left), len(right)) + left = resample_polyline(left, count) + right = resample_polyline(right, count) + polygon = np.vstack([left, right[::-1], left[:1]]) + if len(polygon) < 4 or not np.isfinite(polygon).all(): + return None + return polygon + + +def polygon_area(polygon: np.ndarray) -> float: + x = polygon[:, 0] + y = polygon[:, 1] + return 0.5 * abs(float(np.dot(x, np.roll(y, 1)) - np.dot(y, np.roll(x, 1)))) + + +def polygon_components(geometry) -> list[Polygon]: + if geometry.is_empty: + return [] + if geometry.geom_type == "Polygon": + return [geometry] + if geometry.geom_type in {"MultiPolygon", "GeometryCollection"}: + polygons = [] + for component in geometry.geoms: + polygons.extend(polygon_components(component)) + return polygons + return [] + + +def valid_polygon(exterior: np.ndarray, holes: list[np.ndarray] | None = None): + geometry = Polygon(exterior, holes=holes or []) + return geometry if geometry.is_valid else geometry.buffer(0) + + +def fill_small_holes(geometry): + cleaned = [] + for polygon in polygon_components(geometry): + holes = [ + interior.coords + for interior in polygon.interiors + if Polygon(interior).area >= HETROD_LENIENT_MIN_HOLE_AREA_M2 + ] + cleaned.append(Polygon(polygon.exterior.coords, holes=holes)) + return unary_union(cleaned) + + +def build_agent_region_geometries( + parsed: dict[str, object], + *, + shared_surface: bool = False, + margin_m: float = 0.0, + margins_by_type: dict[str, float] | None = None, +) -> dict[str, object]: + """Build type-specific regions, with optional lenient shared-surface policy.""" + regions = {} + for agent_type in AGENT_REGION_TYPES: + geometries = [ + valid_polygon( + lanelet["polygon"], + holes=lanelet.get("holes", []), + ) + for lanelet in parsed["lanelets"] + if agent_type in lanelet["valid_for"] + ] + if agent_type == "pedestrian": + geometries.extend( + valid_polygon(polygon) for polygon in parsed["crosswalks"] + ) + regions[agent_type] = fill_small_holes(unary_union(geometries)) + if shared_surface: + shared = fill_small_holes(unary_union(list(regions.values()))) + regions["cyclist"] = shared + regions["pedestrian"] = shared + margins = margins_by_type or { + agent_type: margin_m for agent_type in AGENT_REGION_TYPES + } + regions = { + agent_type: ( + geometry.buffer(float(margins.get(agent_type, 0.0))) + if margins.get(agent_type, 0.0) + else geometry + ) + for agent_type, geometry in regions.items() + } + return regions + + +def parse_osm(path: Path) -> dict[str, object]: + root = ET.parse(path).getroot() + raw_nodes = {} + for node in root.findall("node"): + raw_nodes[node.attrib["id"]] = ( + float(node.attrib["lat"]), + float(node.attrib["lon"]), + ) + node_ids = list(raw_nodes) + lat = np.asarray([raw_nodes[node_id][0] for node_id in node_ids]) + lon = np.asarray([raw_nodes[node_id][1] for node_id in node_ids]) + xy = local_xy(lat, lon) + nodes = {node_id: point for node_id, point in zip(node_ids, xy)} + + ways = {} + for way in root.findall("way"): + way_id = way.attrib["id"] + refs = [nd.attrib["ref"] for nd in way.findall("nd")] + tags = {tag.attrib["k"]: tag.attrib["v"] for tag in way.findall("tag")} + points = np.asarray([nodes[ref] for ref in refs if ref in nodes]) + ways[way_id] = {"points": points, "tags": tags} + + lanelets = [] + for relation in root.findall("relation"): + tags = {tag.attrib["k"]: tag.attrib["v"] for tag in relation.findall("tag")} + relation_type = tags.get("type") + subtype, valid_for, inference = valid_agent_types_for_osm_tags(tags) + if relation_type == "multipolygon": + if not valid_for: + continue + outer_parts = [ + ways[member.attrib["ref"]]["points"] + for member in relation.findall("member") + if member.attrib.get("type") == "way" + and member.attrib.get("role") == "outer" + and member.attrib["ref"] in ways + ] + inner_parts = [ + ways[member.attrib["ref"]]["points"] + for member in relation.findall("member") + if member.attrib.get("type") == "way" + and member.attrib.get("role") == "inner" + and member.attrib["ref"] in ways + ] + outer_geometry = unary_union( + [ + Polygon(ring) + for ring in closed_rings_from_way_parts(outer_parts) + ] + ) + if inner_parts: + inner_geometry = unary_union( + [ + Polygon(ring) + for ring in closed_rings_from_way_parts(inner_parts) + ] + ) + outer_geometry = outer_geometry.difference(inner_geometry) + for polygon_index, geometry in enumerate( + polygon_components(outer_geometry) + ): + lanelets.append( + { + "id": f"{relation.attrib['id']}:{polygon_index}", + "subtype": subtype, + "valid_for": valid_for, + "polygon": np.asarray(geometry.exterior.coords), + "holes": [ + np.asarray(interior.coords) + for interior in geometry.interiors + ], + "source": "multipolygon", + "inference": inference, + } + ) + continue + if relation_type != "lanelet": + continue + members = [ + { + "ref": member.attrib["ref"], + "role": member.attrib.get("role", ""), + "type": member.attrib.get("type", ""), + } + for member in relation.findall("member") + ] + left = join_way_parts( + [ + ways[member["ref"]]["points"] + for member in members + if member["type"] == "way" + and member["role"] == "left" + and member["ref"] in ways + ] + ) + right = join_way_parts( + [ + ways[member["ref"]]["points"] + for member in members + if member["type"] == "way" + and member["role"] == "right" + and member["ref"] in ways + ] + ) + polygon = polygon_from_boundaries(left, right) + if polygon is None: + continue + count = max(len(left), len(right)) + aligned_right = right + if np.dot(left[-1] - left[0], right[-1] - right[0]) < 0: + aligned_right = right[::-1] + left_resampled = resample_polyline(left, count) + right_resampled = resample_polyline(aligned_right, count) + centerline = (left_resampled + right_resampled) / 2.0 + signs = [] + for point_index in range(0, len(centerline) - 1, 5): + direction = centerline[point_index + 1] - centerline[point_index] + left_offset = left_resampled[point_index] - centerline[point_index] + signs.append( + np.sign( + direction[0] * left_offset[1] + - direction[1] * left_offset[0] + ) + ) + if signs and np.mean(signs) <= 0: + centerline = centerline[::-1] + lanelets.append( + { + "id": relation.attrib["id"], + "subtype": subtype, + "valid_for": valid_for, + "polygon": polygon, + "holes": [], + "centerline": centerline, + "source": "lanelet", + "inference": inference, + } + ) + + crosswalks = [] + boundaries = [] + for way in ways.values(): + points = way["points"] + if len(points) < 2: + continue + way_type = way["tags"].get("type", "") + if way_type in {"zebra", "zebra_marking"}: + if len(points) == 2: + direction = points[1] - points[0] + length = np.linalg.norm(direction) + if length > 1e-9: + perpendicular = np.array([-direction[1], direction[0]]) / length + offset = 1.5 * perpendicular + points = np.vstack( + [ + points[0] + offset, + points[1] + offset, + points[1] - offset, + points[0] - offset, + points[0] + offset, + ] + ) + elif not np.allclose(points[0], points[-1]): + points = np.vstack([points, points[:1]]) + crosswalks.append(points) + if way_type in {"wall", "curbstone", "fence", "road_border"}: + boundaries.append(points) + + return { + "lanelets": lanelets, + "crosswalks": crosswalks, + "boundaries": boundaries, + "ways": ways, + } + + +def draw_location( + axes: np.ndarray, + location: str, + parsed: dict[str, object], + *, + shared_surface: bool = False, + margin_m: float = 0.0, + margins_by_type: dict[str, float] | None = None, +) -> dict[str, object]: + lanelets = parsed["lanelets"] + crosswalks = parsed["crosswalks"] + boundaries = parsed["boundaries"] + all_points = [lanelet["polygon"] for lanelet in lanelets] + all_points.extend(crosswalks) + all_points.extend(boundaries) + extent = np.concatenate(all_points, axis=0) + x_margin = max(8.0, 0.04 * float(np.ptp(extent[:, 0]))) + y_margin = max(8.0, 0.04 * float(np.ptp(extent[:, 1]))) + limits = ( + float(extent[:, 0].min() - x_margin), + float(extent[:, 0].max() + x_margin), + float(extent[:, 1].min() - y_margin), + float(extent[:, 1].max() + y_margin), + ) + + subtype_counts = Counter(lanelet["subtype"] for lanelet in lanelets) + inference_counts = Counter( + lanelet["inference"] for lanelet in lanelets if lanelet.get("inference") + ) + stats = { + "location": location, + "lanelet_subtypes": dict(sorted(subtype_counts.items())), + "subtype_inferences": dict(sorted(inference_counts.items())), + } + region_geometries = build_agent_region_geometries( + parsed, + shared_surface=shared_surface, + margin_m=margin_m, + margins_by_type=margins_by_type, + ) + for ax, agent_type in zip(axes, AGENT_REGION_TYPES): + for lanelet in lanelets: + polygon = lanelet["polygon"] + valid_for = lanelet["valid_for"] + is_active = agent_type in valid_for + facecolor = "none" if is_active else BACKGROUND_COLOR + alpha = 0.75 if is_active else 0.10 + edgecolor = ( + FREESPACE_COLOR + if lanelet["subtype"] == "freespace" + else ("#555555" if is_active else "#AAAAAA") + ) + ax.add_patch( + PolygonPatch( + polygon, + closed=True, + facecolor=facecolor, + edgecolor=edgecolor, + linewidth=0.75 if lanelet["subtype"] == "freespace" else 0.45, + linestyle="--" if lanelet["subtype"] == "freespace" else "-", + alpha=alpha, + ) + ) + merged = region_geometries[agent_type] + merged_components = polygon_components(merged) + for geometry in merged_components: + ax.add_patch( + PolygonPatch( + np.asarray(geometry.exterior.coords), + closed=True, + facecolor=TYPE_COLORS[agent_type], + edgecolor="#333333", + linewidth=0.65, + alpha=0.46, + ) + ) + for interior in geometry.interiors: + ax.add_patch( + PolygonPatch( + np.asarray(interior.coords), + closed=True, + facecolor="white", + edgecolor="#555555", + linewidth=0.6, + alpha=1.0, + ) + ) + for boundary in boundaries: + ax.plot( + boundary[:, 0], + boundary[:, 1], + color=BOUNDARY_COLOR, + linewidth=0.6, + alpha=0.65, + ) + area = float(merged.area) + stats[agent_type] = { + "num_polygons_after_union": len(merged_components), + "union_area_m2": area, + } + ax.set_title( + f"{agent_type.title()}\n{len(merged_components)} union polygons · {area:,.0f} m²", + fontsize=10, + fontweight="bold", + ) + ax.set_xlim(limits[0], limits[1]) + ax.set_ylim(limits[2], limits[3]) + ax.set_aspect("equal", adjustable="box") + ax.grid(color="#DDDDDD", linewidth=0.4, linestyle=":") + ax.tick_params(labelsize=6) + ax.set_xlabel("local X (m)", fontsize=7) + axes[0].set_ylabel(f"{location}\nlocal Y (m)", fontsize=9, fontweight="bold") + return stats + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--maps-dir", required=True, type=Path) + parser.add_argument("--output-dir", required=True, type=Path) + parser.add_argument("--shared-surface", action="store_true") + parser.add_argument( + "--margin-m", + type=float, + help="Uniform override; defaults to the standard per-type margins.", + ) + parser.add_argument("--vehicle-margin-m", type=float) + parser.add_argument("--cyclist-margin-m", type=float) + parser.add_argument("--pedestrian-margin-m", type=float) + args = parser.parse_args() + args.output_dir.mkdir(parents=True, exist_ok=True) + + map_paths = [args.maps_dir / f"location{index}.osm" for index in range(6)] + missing = [str(path) for path in map_paths if not path.is_file()] + if missing: + raise FileNotFoundError(f"Missing maps: {missing}") + + parsed_maps = {path.stem: parse_osm(path) for path in map_paths} + fallback_margins = ( + HETROD_VALID_REGION_MARGIN_M + if args.margin_m is None + else {agent_type: args.margin_m for agent_type in AGENT_REGION_TYPES} + ) + margins_by_type = { + "vehicle": ( + fallback_margins["vehicle"] + if args.vehicle_margin_m is None + else args.vehicle_margin_m + ), + "cyclist": ( + fallback_margins["cyclist"] + if args.cyclist_margin_m is None + else args.cyclist_margin_m + ), + "pedestrian": ( + fallback_margins["pedestrian"] + if args.pedestrian_margin_m is None + else args.pedestrian_margin_m + ), + } + margin_label = " · ".join( + f"{agent_type} {margin:g} m" + for agent_type, margin in margins_by_type.items() + ) + all_stats = [] + for location, parsed in parsed_maps.items(): + fig, axes = plt.subplots(1, 3, figsize=(15, 5), dpi=150) + stats = draw_location( + np.asarray(axes), + location, + parsed, + shared_surface=args.shared_surface, + margin_m=0.0 if args.margin_m is None else args.margin_m, + margins_by_type=margins_by_type, + ) + stats["policy"] = ( + "diagnostic-shared-surface" + if args.shared_surface + else HETROD_VALID_REGION_POLICY + ) + stats["margins_by_type_m"] = margins_by_type + all_stats.append(stats) + fig.suptitle( + f"HetroD {location}: shared-surface valid regions\n{margin_label}" + if args.shared_surface + else f"HetroD {location}: type-specific valid regions\n{margin_label}", + fontsize=14, + fontweight="bold", + ) + fig.legend( + handles=[ + Patch(facecolor=TYPE_COLORS["vehicle"], label="active valid region"), + Patch(facecolor=BACKGROUND_COLOR, label="other lanelet"), + Patch( + facecolor="none", + edgecolor=FREESPACE_COLOR, + linestyle="--", + label="freespace (shared lenient region)", + ), + Patch(facecolor="none", edgecolor=BOUNDARY_COLOR, label="physical boundary"), + ], + loc="lower center", + ncol=4, + fontsize=8, + ) + fig.tight_layout(rect=(0, 0.07, 1, 0.94)) + fig.savefig(args.output_dir / f"{location}_valid_regions.png") + plt.close(fig) + + fig, axes = plt.subplots(6, 3, figsize=(15, 25), dpi=130) + for row, (location, parsed) in enumerate(parsed_maps.items()): + draw_location( + axes[row], + location, + parsed, + shared_surface=args.shared_surface, + margin_m=0.0 if args.margin_m is None else args.margin_m, + margins_by_type=margins_by_type, + ) + fig.suptitle( + ( + "HetroD shared-surface valid regions by location and agent type\n" + f"Vehicle surface preserved · cyclist/pedestrian surfaces shared · " + f"{margin_label}" + if args.shared_surface + else + "HetroD type-specific valid regions by location and agent type\n" + f"Per-type OSM semantics preserved · {margin_label}" + ), + fontsize=16, + fontweight="bold", + ) + fig.tight_layout(rect=(0, 0, 1, 0.975)) + fig.savefig(args.output_dir / "all_locations_valid_regions.png") + plt.close(fig) + + stats_path = args.output_dir / "valid_region_stats.json" + stats_path.write_text(json.dumps(all_stats, indent=2), encoding="utf-8") + print(f"Wrote {len(map_paths)} location figures, overview, and {stats_path}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From 09a63cd8d65e35a42d49cd3cae4a9df87be26f09 Mon Sep 17 00:00:00 2001 From: Yu-Hsiang Chen <82202284+bob020416@users.noreply.github.com> Date: Mon, 3 Aug 2026 22:31:29 +0800 Subject: [PATCH 2/2] Add transition-aware pedestrian valid regions --- scenarionet-converter/hetrod_regions.py | 58 ++++++++++++++++---- scenarionet-converter/hetrod_scene.py | 72 ++++++++++++++++++++----- 2 files changed, 106 insertions(+), 24 deletions(-) diff --git a/scenarionet-converter/hetrod_regions.py b/scenarionet-converter/hetrod_regions.py index a5c00be..b4c71f3 100644 --- a/scenarionet-converter/hetrod_regions.py +++ b/scenarionet-converter/hetrod_regions.py @@ -5,14 +5,18 @@ from collections.abc import Iterable -HETROD_VALID_REGION_SCHEMA_VERSION = "1.2" -HETROD_VALID_REGION_POLICY = "type-specific-v2-buffered" +HETROD_VALID_REGION_SCHEMA_VERSION = "1.3" +HETROD_VALID_REGION_POLICY = "type-specific-v3-pedestrian-transition-aware" HETROD_LENIENT_MIN_HOLE_AREA_M2 = 5.0 HETROD_VALID_REGION_MARGIN_M = { "vehicle": 0.75, "cyclist": 0.75, - "pedestrian": 2.0, + "pedestrian_core": 0.75, + "pedestrian_crosswalk": 1.5, + "pedestrian_road": 0.0, } +HETROD_PEDESTRIAN_CORRIDOR_MARGIN_M = 1.5 +HETROD_PEDESTRIAN_TRANSITION_TIME_SLACK_S = 1.0 VEHICLE_REGION_SUBTYPES = frozenset( { @@ -29,17 +33,25 @@ "bike_lane", } ) -PEDESTRIAN_REGION_SUBTYPES = frozenset( +PEDESTRIAN_CORE_REGION_SUBTYPES = frozenset( { "walkway", - "crosswalk", "footway", "pedestrian", } ) +PEDESTRIAN_CROSSWALK_REGION_SUBTYPES = frozenset({"crosswalk"}) SHARED_LENIENT_REGION_SUBTYPES = frozenset({"freespace"}) AGENT_REGION_TYPES = ("vehicle", "cyclist", "pedestrian") +HETROD_VALID_REGION_KEYS = ( + "vehicle", + "cyclist", + "pedestrian", + "pedestrian_core", + "pedestrian_crosswalk", + "pedestrian_road", +) def normalize_lanelet_subtype(subtype: object) -> str: @@ -56,7 +68,10 @@ def valid_agent_types_for_lanelet(subtype: object) -> tuple[str, ...]: valid_for.extend(("vehicle", "cyclist")) if normalized in CYCLIST_ONLY_REGION_SUBTYPES: valid_for.append("cyclist") - if normalized in PEDESTRIAN_REGION_SUBTYPES: + if normalized in ( + PEDESTRIAN_CORE_REGION_SUBTYPES + | PEDESTRIAN_CROSSWALK_REGION_SUBTYPES + ): valid_for.append("pedestrian") if normalized in SHARED_LENIENT_REGION_SUBTYPES: valid_for.extend(AGENT_REGION_TYPES) @@ -100,9 +115,29 @@ def region_definition() -> dict[str, object]: "cyclist_lanelet_subtypes": sorted( VEHICLE_REGION_SUBTYPES | CYCLIST_ONLY_REGION_SUBTYPES ), - "pedestrian_lanelet_subtypes": sorted(PEDESTRIAN_REGION_SUBTYPES), + "pedestrian_lanelet_subtypes": sorted( + PEDESTRIAN_CORE_REGION_SUBTYPES + | PEDESTRIAN_CROSSWALK_REGION_SUBTYPES + ), + "pedestrian_core_lanelet_subtypes": sorted( + PEDESTRIAN_CORE_REGION_SUBTYPES + ), + "pedestrian_crosswalk_lanelet_subtypes": sorted( + PEDESTRIAN_CROSSWALK_REGION_SUBTYPES + ), + "pedestrian_road_transition_lanelet_subtypes": sorted( + VEHICLE_REGION_SUBTYPES + ), "shared_lenient_lanelet_subtypes": sorted(SHARED_LENIENT_REGION_SUBTYPES), - "crosswalk_is_pedestrian_valid": True, + "crosswalk_is_pedestrian_transition": True, + "map_unsupported_gt_policy": "exclude_and_report", + "pedestrian_transition_time_budget": "gt_transition_frames_plus_slack", + "pedestrian_transition_time_slack_s": ( + HETROD_PEDESTRIAN_TRANSITION_TIME_SLACK_S + ), + "pedestrian_gt_road_corridor_margin_m": ( + HETROD_PEDESTRIAN_CORRIDOR_MARGIN_M + ), "unknown_inference_rules": { "bicycle=no and one_way=no": "walkway", "one_way=yes and motorcycle=yes or speed_limit present": "road", @@ -112,10 +147,11 @@ def region_definition() -> dict[str, object]: "holes": "list[float32[M,2]]", }, "minimum_preserved_hole_area_m2": HETROD_LENIENT_MIN_HOLE_AREA_M2, - "boundary_margin_m_by_agent_type": dict(HETROD_VALID_REGION_MARGIN_M), + "boundary_margin_m_by_region": dict(HETROD_VALID_REGION_MARGIN_M), "boundary_margin_rationale": ( - "Minimum validated type-specific margins yielding at least 90% " - "pedestrian GT center coverage in every represented validation location." + "Vehicle/cyclist and permanent pedestrian core use a 0.75 m " + "boundary allowance. Crosswalk and per-agent GT road transitions " + "use 1.5 m; unsupported GT frames are excluded and reported." ), } diff --git a/scenarionet-converter/hetrod_scene.py b/scenarionet-converter/hetrod_scene.py index 6a741be..44ebbd6 100644 --- a/scenarionet-converter/hetrod_scene.py +++ b/scenarionet-converter/hetrod_scene.py @@ -18,7 +18,12 @@ from hetrod_regions import ( HETROD_LENIENT_MIN_HOLE_AREA_M2, HETROD_VALID_REGION_MARGIN_M, + HETROD_VALID_REGION_KEYS, HETROD_VALID_REGION_SCHEMA_VERSION, + PEDESTRIAN_CORE_REGION_SUBTYPES, + PEDESTRIAN_CROSSWALK_REGION_SUBTYPES, + SHARED_LENIENT_REGION_SUBTYPES, + VEHICLE_REGION_SUBTYPES, region_definition, valid_agent_types_for_osm_tags, ) @@ -476,11 +481,21 @@ def serialize_region_geometry(geometries, margin_m=0.0): return records region_only_geometries = { - "vehicle": [], - "cyclist": [], - "pedestrian": [], + key: [] for key in HETROD_VALID_REGION_KEYS } + def pedestrian_region_layer(subtype): + if subtype in PEDESTRIAN_CROSSWALK_REGION_SUBTYPES: + return "pedestrian_crosswalk" + if subtype in ( + PEDESTRIAN_CORE_REGION_SUBTYPES + | SHARED_LENIENT_REGION_SUBTYPES + ): + return "pedestrian_core" + if subtype in VEHICLE_REGION_SUBTYPES: + return "pedestrian_road" + return None + for rel in relations: if rel["tags"].get("type") != "multipolygon": continue @@ -522,6 +537,11 @@ def serialize_region_geometry(geometries, margin_m=0.0): region_only_geometries[agent_type].extend( polygon_components(relation_geometry) ) + pedestrian_layer = pedestrian_region_layer(multipolygon_subtype) + if pedestrian_layer is not None: + region_only_geometries[pedestrian_layer].extend( + polygon_components(relation_geometry) + ) continue for polygon_index, polygon in enumerate(relation_polygons): # The valid traffic region is outside a building. Clockwise exterior @@ -719,9 +739,7 @@ def is_left_on_left(centerline, left, step=5): valid_regions = { **region_definition(), - "vehicle": [], - "cyclist": [], - "pedestrian": [], + **{key: [] for key in HETROD_VALID_REGION_KEYS}, } region_geometries = { agent_type: list(geometries) @@ -742,12 +760,40 @@ def is_left_on_left(centerline, left, step=5): region_geometries[agent_type].extend( polygon_components(geometry) ) + subtype = feature.get("hetrod_lanelet_subtype") + pedestrian_layer = ( + "pedestrian_crosswalk" + if feature.get("type") == MetaDriveType.CROSSWALK + else pedestrian_region_layer(subtype) + ) + if pedestrian_layer is not None: + region_geometries[pedestrian_layer].extend( + polygon_components(geometry) + ) - for agent_type, geometries in region_geometries.items(): - valid_regions[agent_type] = serialize_region_geometry( + for region_key, geometries in region_geometries.items(): + if region_key == "pedestrian": + continue + valid_regions[region_key] = serialize_region_geometry( geometries, - margin_m=HETROD_VALID_REGION_MARGIN_M[agent_type], + margin_m=HETROD_VALID_REGION_MARGIN_M.get(region_key, 0.0), ) + pedestrian_compatibility_geometries = ( + region_geometries["pedestrian_core"] + + region_geometries["pedestrian_crosswalk"] + ) + valid_regions["pedestrian"] = serialize_region_geometry( + [ + geometry.buffer( + HETROD_VALID_REGION_MARGIN_M[ + "pedestrian_crosswalk" + if index >= len(region_geometries["pedestrian_core"]) + else "pedestrian_core" + ] + ) + for index, geometry in enumerate(pedestrian_compatibility_geometries) + ] + ) return map_features, (0, 0), valid_regions @@ -1033,8 +1079,8 @@ def create_scenario_from_csv( first_i = int(np.where(sdc_track["valid"] > 0)[0][0]) origin_xy = sdc_track["position"][first_i, :2] - for agent_type in ("vehicle", "cyclist", "pedestrian"): - records = sc[SD.METADATA]["hetrod_valid_regions"].get(agent_type, []) + for region_key in HETROD_VALID_REGION_KEYS: + records = sc[SD.METADATA]["hetrod_valid_regions"].get(region_key, []) for record in records: record["exterior"] = np.asarray( record["exterior"], dtype=np.float32 @@ -1070,8 +1116,8 @@ def create_scenario_from_csv( c, s = math.cos(-psi0), math.sin(-psi0) R = np.array([[c, -s], [s, c]], dtype=float) - for agent_type in ("vehicle", "cyclist", "pedestrian"): - records = sc[SD.METADATA]["hetrod_valid_regions"].get(agent_type, []) + for region_key in HETROD_VALID_REGION_KEYS: + records = sc[SD.METADATA]["hetrod_valid_regions"].get(region_key, []) for record in records: exterior = np.asarray(record["exterior"], dtype=np.float32).copy() exterior[:, :2] = (R @ exterior[:, :2].T).T