From 32910a3a1b51ef9e8d4a79a0a27b8066bd1214c9 Mon Sep 17 00:00:00 2001 From: Hidde Boekema <33418973+hboekema@users.noreply.github.com> Date: Thu, 1 May 2025 13:51:49 +0200 Subject: [PATCH] Add View-of-Delft Prediction (VoD-P) dataset (#99) * Create main branch * Initial commit * add setup.py * move tools from md * solve import conflict * refactor * get a unified func * rename * batch convert nuscenes * change summary file to json * remove set as well * move writing summary to metadrive * show import error * nuplan ok * clean example * waymo * all convert is ready now * source file to data * update get nuplan parameters * get all scenarios * format * add pg converter * fix nuplan bug * suppres tf warning * combine dataset function * test script * add test to github page * add test script * test script * add step condition to verofy * test scenarios * remove logging information * filter function * test filter * sdc filter test * add filter test * finish filter * multiprocess verify * multi_processing test * small dataset test! * multi-processing test * format * auto reduce worker num * use is_scenario_file to determine * build new dataset from error logs * add new test * add common utils * move all test genrtaed file to tmp, add genertae error set test * provide scripts * add test * reanme * scale up test * add script for parsing data * disable info output * multi processing get files * multi-process writing * test waymo converter * add local test for generating local dataset * set waymo origin path * merge automatically * batch generation * add combine API * fix combine bug * test combine data * add more test * fix bug * more test * add num works to script arguments * fix bug * add dataset to gitignore * test more scripts * update error message * .sh * fix bug * fix bug * 16 workers * remove annotation * install md for github test * fix bug * fix CI * fix test * add filters to combine script * fix test * Fix bug for generating dataset (#2) * update parameters for scripts * update write function * modify waymo script * use exist ok instead of overwrite * remove TODO * rename to comvine_dataset * use exist_ok and force_overwrite together * format * test * creat env for each thread * restore * fix bug * fix pg bug * fix * fix bug * add assert * don't return done info * to dict * add test * only compare sdc * no store mao * release memory * add start index to argumen * test * format some settings/flags * add tmp path * add tmp dir * test all scripts * suppress warning * suppress warning * format * test memory leak * fix memory leak * remove useless functions * imap * thread-1 process for avoiding memory leak * add list() * rename * verify existence * verify completeness * test * add test * add default value * add limit * use script * add anotation * test script * fix bug * fix bug * add author4 * add overwrite * fix bug * fix * combine overwrite * fix bug * gpu007 * add result save dir * adjust sequence * fix test bug * disable bash scri[t * add episode length limit * move scripts to root dir * format * fix test * Readme (#3) * rename to merge dataset * add -d for operation * test move * add move function * test remove * format * dataset -> database * add readme * format.sh * test assert * rename to database in .sh * Update README.md * rename scripts and update readme * remove repeat calculation * update radius * Add come updates for Neurips paper (#4) * scenarionet training * wandb * train utils * fix callback * run PPO * use pg test * save path * use torch * add dependency * update ignore * update training * large model * use curriculum training * add time to exp name * storage_path * restore * update training * use my key * add log message * check seed * restore callback * restore call bacl * add log message * add logging message * restore ray1.4 * length 500 * ray 100 * wandb * use tf * more levels * add callback * 10 worker * show level * no env horizon * callback result level * more call back * add diffuculty * add mroen stat * mroe stat * show levels * add callback * new * ep len 600 * fix setup * fix stepup * fix to 3.8 * update setup * parallel worker! * new exp * add callback * lateral dist * pg dataset * evaluate * modify config * align config * train single RL * update training script * 100w eval * less eval to reveal * 2000 env eval * new trianing * eval 1000 * update eval * more workers * more worker * 20 worker * dataset to database * split tool! * split dataset * try fix * train 003 * fix mapping * fix test * add waymo tqdm * utils * fix bug * fix bug * waymo * int type * 8 worker read * disable * read file * add log message * check existence * dist 0 * int * check num * suprass warning * add filter API * filter * store map false * new * ablation * filter * fix * update filyter * reanme to from * random select * add overlapping checj * fix * new training sceheme * new reward * add waymo train script * waymo different config * copy raw data * fix bug * add tqdm * update readme * waymo * pg * max lateral dist 3 * pg * crash_done instead of penalty * no crash done * gpu * update eval script * steering range penalty * evaluate * finish pg * update setup * fix bug * test * fix * add on line * train nuplan * generate sensor * udpate training * static obj * multi worker eval * filx bug * use ray for testing * eval! * filter senario * id filter * fox bug * dist = 2 * filter * eval * eval ret * ok * update training pg * test before use * store data=False * collect figures * capture pic --------- Co-authored-by: Quanyi Li * Make video (#5) * generate accident scene * construction PG * no object * accident prob * capture script * update nuscenes toolds * make video * format * fix test * update readme * update readme * format * format * Update video/webpage/code * Update env (#7) * add capture script * gymnasium API * training with gymnasium API * update readme (#9) * Rebuttal (#15) * pg+nuplan train * Need map * use gym wrapper * use createGymWrapper * doc * use all scenarios! * update 80000 scenario * train script * config readthedocs * format * fix doc * add requirement * fix path * readthedocs * doc * reactive traffic example * Doc-example (#18) * reactive traffic example * structure * structure * waymo example * rename and add doc * finish example * example * start from 2 * fix build error * Update doc (#20) * Add list.py and desc * add operations * add structure * update readme * format * update readme * more doc * toc tree * waymo example * add PG * PG+waymo+nuscenes * add nuPlan setup instruction * fix command style by removing .py * Colab exp (#22) * add example * add new workflow * fix bug * pull asset automatically * add colab * fix test * add colab to readme * Update README.md (#23) * Update readme (#24) * update figure * add colab to doc * More info (#28) * boundary to exterior * rename copy to cp, avoiding bugs * add connectivity and sidewalk/cross for nuscenes * update lane type * add semantic renderer * restore * nuplan works * format * md versio>=0.4.1.2 * Loose numpy version (#30) * disable using pip extra requirement installation * loose numpy * waymo * waymo version * add numpy hint * restore * Add to note * add hint * Update document, add a colab example for reading data, upgrade numpy dependency (#34) * Minor update to docs * WIP * adjust numpy requirement * prepare example for reading data from SN dataset * prepare example for reading data from SN dataset * clean * Update Citation information (#37) * Update Sensor API in scripts (#39) * add semantic cam * update API * format * Update the citation in README.md (#40) * Optimize waymo converter (#44) * use generator for waymo * :wqadd preprocessor * use generator * Use Waymo Protos Directly (#38) * use protos directly * format protos --------- Co-authored-by: Quanyi Li * rename to unix style * Update nuScenes & Waymo Optimization (#47) * update can bus * Create LICENSE * update waymo doc * protobuf requirement * just warning * Add warning for proto * update PR template * fix length bug * try sharing nusc * imu heading * fix 161 168 * add badge * fix doc * update doc * format * update cp * update nuscenes interface * update doc * prediction nuscenes * use drivable aread for nuscenes * allow converting prediction * format * fix bug * optimize * clean RAM * delete more * restore to * add only lane * use token * add warning * format * fix bug * add simulation section * Add support to AV2 (#48) * add support to av2 --------- Co-authored-by: Alan-LanFeng * add nuscenes tracks and av2 bound (#49) * add nuscenes tracks to predict * ad av2 boundary type * 1. add back map center to restore original coordinate in nuScnes (#51) * 1. add back map center to restore the original coordinate in nuScenes * Use the utils from MetaDrive to update object summaries; update ScenarioDescription doc (#52) * Update * update * update * update * add trigger (#57) * Add test for waymo example (#58) * add test script * test first 10 scenarios * add dependency * add dependency * Update the script for generating multi-sensors images (#61) * fix broken script * format code * introduce offscreen rendering * try debug * fix * fix * up * up * remove fix * fix * WIP * fix a bug in nusc converter (#60) * fix a typo (#62) * Update waymo.rst (#59) * Update waymo.rst * Update waymo.rst * Fix a bug in Waymo conversion: GPU should be disable (#64) * Update waymo.rst * Update waymo.rst * allow generate all data * update readme * update * better logging info * more info * up * fix * add note on GPU * better log * format * Fix nuscenes (#67) * fix bug * fix a potential bug * update av2 documentation (#75) * fix av2 sdc_track_indx (#72) (#76) * Add View-of-Delft Prediction (VoD-P) dataset * Reformat VoD code * Add documentation for VoD dataset * Reformat convert_vod.py --------- Co-authored-by: Quanyi Li <785878978@qq.com> Co-authored-by: QuanyiLi Co-authored-by: Quanyi Li Co-authored-by: PENG Zhenghao Co-authored-by: Govind Pimpale Co-authored-by: Alan <36124025+Alan-LanFeng@users.noreply.github.com> Co-authored-by: Alan-LanFeng Co-authored-by: Yunsong Zhou <75066007+ZhouYunsong-SJTU@users.noreply.github.com> --- documentation/datasets.rst | 1 + documentation/index.rst | 1 + documentation/operations.rst | 51 ++- documentation/vod.rst | 109 +++++ scenarionet/convert_vod.py | 95 +++++ scenarionet/converter/vod/__init__.py | 0 scenarionet/converter/vod/type.py | 90 +++++ scenarionet/converter/vod/utils.py | 558 ++++++++++++++++++++++++++ 8 files changed, 904 insertions(+), 1 deletion(-) create mode 100644 documentation/vod.rst create mode 100644 scenarionet/convert_vod.py create mode 100644 scenarionet/converter/vod/__init__.py create mode 100644 scenarionet/converter/vod/type.py create mode 100644 scenarionet/converter/vod/utils.py diff --git a/documentation/datasets.rst b/documentation/datasets.rst index e8a2dbc..33af5a1 100644 --- a/documentation/datasets.rst +++ b/documentation/datasets.rst @@ -22,6 +22,7 @@ We will fix it as best as we can and record it in the troubleshooting section fo - :ref:`lyft` - :ref:`new_data` - :ref:`argoverse2` +- :ref:`vod` diff --git a/documentation/index.rst b/documentation/index.rst index 520a56c..462250e 100644 --- a/documentation/index.rst +++ b/documentation/index.rst @@ -55,6 +55,7 @@ Please feel free to contact us if you have any suggestion or idea! PG.rst lyft.rst argoverse2.rst + vod.rst new_data.rst diff --git a/documentation/operations.rst b/documentation/operations.rst index 297d28b..4f49d35 100644 --- a/documentation/operations.rst +++ b/documentation/operations.rst @@ -162,6 +162,55 @@ However, Lyft is now a part of Woven Planet and the new data has to be parsed vi We are working on support this new toolkit to support the new Lyft dataset. Detailed guide is available at Section :ref:`nuscenes`. +Convert VoD +------------------------------------ + +.. code-block:: text + + python -m scenarionet.convert_vod [-h] [--database_path DATABASE_PATH] + [--dataset_name DATASET_NAME] + [--split + {v1.0-trainval,v1.0-test,train,train_val,val,test}] + [--dataroot DATAROOT] [--map_radius MAP_RADIUS] + [--future FUTURE] [--past PAST] [--overwrite] + [--num_workers NUM_WORKERS] + + Build database from VOD scenarios + + optional arguments: + -h, --help show this help message and exit + --database_path DATABASE_PATH, -d DATABASE_PATH + directory, The path to place the data + --dataset_name DATASET_NAME, -n DATASET_NAME + Dataset name, will be used to generate scenario files + --split + {v1.0-trainval,v1.0-test,train,train_val,val,test} + Which splits of VOD data should be used. If set to + ['v1.0-trainval', 'v1.0-test'], it will + convert the full log into scenarios with 20 second episode + length. If set to ['train', 'train_val', 'val', 'test'], + it will convert segments used for VOD prediction challenge + to scenarios, resulting in more converted scenarios. + Generally, you should choose this parameter from + ['v1.0-trainval', 'v1.0-test'] to get complete + scenarios for planning unless you want to use the + converted scenario files for prediction task. + --dataroot DATAROOT The path of vod data + --map_radius MAP_RADIUS The size of map + --future FUTURE 3 seconds by default. How many future seconds to + predict. Only available if split is chosen from + ['train', 'train_val', 'val', 'test'] + --past PAST 0.5 seconds by default. How many past seconds are + used for prediction. Only available if split is + chosen from ['train', 'train_val', 'val', 'test'] + --overwrite If the database_path exists, whether to overwrite it + --num_workers NUM_WORKERS number of workers to use + + +This script converts the View-of-Delft Prediction (VoD) dataset into our scenario descriptions. +You will need to install ``vod-devkit`` and download the source data from https://intelligent-vehicles.org/datasets/view-of-delft/. +Detailed guide is available at Section :ref:`vod`. + Convert PG ------------------------- @@ -519,4 +568,4 @@ The main goal of this command is to ensure that the training and test sets are i -h, --help show this help message and exit --d_1 D_1 The path of the first database --d_2 D_2 The path of the second database - --show_id whether to show the id of overlapped scenarios \ No newline at end of file + --show_id whether to show the id of overlapped scenarios diff --git a/documentation/vod.rst b/documentation/vod.rst new file mode 100644 index 0000000..9743698 --- /dev/null +++ b/documentation/vod.rst @@ -0,0 +1,109 @@ +############################# +View-of-Delft (VoD) +############################# + +| Website: https://intelligent-vehicles.org/datasets/view-of-delft/ +| Download: https://intelligent-vehicles.org/datasets/view-of-delft/ (Registration required) +| Papers: + Detection dataset: https://ieeexplore.ieee.org/document/9699098 + Prediction dataset: https://ieeexplore.ieee.org/document/10493110 + +The View-of-Delft (VoD) dataset is a novel automotive dataset recorded in Delft, +the Netherlands. It contains 8600+ frames of synchronized and calibrated +64-layer LiDAR-, (stereo) camera-, and 3+1D (range, azimuth, elevation, + +Doppler) radar-data acquired in complex, urban traffic. It consists of 123100+ +3D bounding box annotations of both moving and static objects, including 26500+ +pedestrian, 10800 cyclist and 26900+ car labels. It additionally contains +semantic map annotations and accurate ego-vehicle localization data. + +Benchmarks for detection and prediction tasks are released for the dataset. See +the sections below for details on these benchmarks. + +**Detection**: + An object detection benchmark is available for researchers to develop and + evaluate their models on the VoD dataset. At the time of publication, this + benchmark was the largest automotive multi-class object detection dataset + containing 3+1D radar data, and the only dataset containing high-end (64-layer) + LiDAR and (any kind of) radar data at the same time. + +**Prediction**: + A trajectory prediction benchmark is publicly available to enable research + on urban multi-class trajectory prediction. This benchmark contains challenging + prediction cases in the historic city center of Delft with a high proportion of + Vulnerable Road Users (VRUs), such as pedestrians and cyclists. Semantic map + annotations for road elements such as lanes, sidewalks, and crosswalks are + provided as context for prediction models. + +1. Install VoD Prediction Toolkit +================================= + +We will use the VoD Prediction toolkit to convert the data. +First of all, we have to install the ``vod-devkit``. + +.. code-block:: bash + + # install from github (Recommend) + git clone git@github.com:tudelft-iv/view-of-delft-prediction-devkit.git + cd vod-devkit + pip install -e . + + # or install from PyPI + pip install vod-devkit + +By installing from github, you can access examples and source code the toolkit. +The examples are useful to verify whether the installation and dataset setup is correct or not. + + +2. Download VoD Data +============================== + +The official instruction is available at https://intelligent-vehicles.org/datasets/view-of-delft/. +Here we provide a simplified installation procedure. + +First of all, please fill in the access form on vod website: https://intelligent-vehicles.org/datasets/view-of-delft/. +The maintainers will send the data link to your email. Download and unzip the file named ``view_of_delft_prediction_PUBLIC.zip``. + +Secondly, all files should be organized to the following structure:: + + /vod/data/path/ + ├── maps/ + | └──expansion/ + ├── v1.0-trainval/ + | ├──attribute.json + | ├──calibrated_sensor.json + | ├──map.json + | ├──log.json + | ├──ego_pose.json + | └──... + └── v1.0-test/ + +**Note**: The sensor data is currently not available in the Prediction dataset, but will be released in the near future. + +The ``/vod/data/path`` should be ``/data/sets/vod`` by default according to the official instructions, +allowing the ``vod-devkit`` to find it. +But you can still place it to any other places and: + +- build a soft link connect your data folder and ``/data/sets/vod`` +- or specify the ``dataroot`` when calling vod APIs and our convertors. + + +After this step, the examples in ``vod-devkit`` is supposed to work well. +Please try ``view-of-delft-prediction-devkit/tutorials/vod_tutorial.ipynb`` and see if the demo can successfully run. + +3. Build VoD Database +=========================== + +After setup the raw data, convertors in ScenarioNet can read the raw data, convert scenario format and build the database. +Here we take converting raw data in ``v1.0-trainval`` as an example:: + + python -m scenarionet.convert_vod -d /path/to/your/database --split v1.0-trainval --dataroot /vod/data/path + +The ``split`` is to determine which split to convert. ``dataroot`` is set to ``/data/sets/vod`` by default, +but you need to specify it if your data is stored in any other directory. +Now all converted scenarios will be placed at ``/path/to/your/database`` and are ready to be used in your work. + + +Known Issues: VoD +======================= + +N/A diff --git a/scenarionet/convert_vod.py b/scenarionet/convert_vod.py new file mode 100644 index 0000000..ec6e129 --- /dev/null +++ b/scenarionet/convert_vod.py @@ -0,0 +1,95 @@ +desc = "Build database from VOD scenarios" + +prediction_split = ["train", "train_val", "val", "test"] +scene_split = ["v1.0-trainval", "v1.0-test"] + +split_to_scene = { + "train": "v1.0-trainval", + "train_val": "v1.0-trainval", + "val": "v1.0-trainval", + "test": "v1.0-test", +} + +if __name__ == "__main__": + import pkg_resources # for suppress warning + import argparse + import os.path + from functools import partial + from scenarionet import SCENARIONET_DATASET_PATH + from scenarionet.converter.vod.utils import ( + convert_vod_scenario, + get_vod_scenarios, + get_vod_prediction_split, + ) + from scenarionet.converter.utils import write_to_directory + + parser = argparse.ArgumentParser(description=desc) + parser.add_argument( + "--database_path", + "-d", + default=os.path.join(SCENARIONET_DATASET_PATH, "vod"), + help="directory, The path to place the data", + ) + parser.add_argument( + "--dataset_name", + "-n", + default="vod", + help="Dataset name, will be used to generate scenario files", + ) + parser.add_argument( + "--split", + default="v1.0-trainval", + choices=scene_split + prediction_split, + help="Which splits of VOD data should be used. If set to {}, it will convert the full log into scenarios" + " with 20 second episode length. If set to {}, it will convert segments used for VOD prediction" + " challenge to scenarios, resulting in more converted scenarios. Generally, you should choose this " + " parameter from {} to get complete scenarios for planning unless you want to use the converted scenario " + " files for prediction task.".format(scene_split, prediction_split, scene_split), + ) + parser.add_argument("--dataroot", default="/data/sets/vod", help="The path of vod data") + parser.add_argument("--map_radius", default=500, type=float, help="The size of map") + parser.add_argument( + "--future", + default=3, + type=float, + help="3 seconds by default. How many future seconds to predict. Only " + "available if split is chosen from {}".format(prediction_split), + ) + parser.add_argument( + "--past", + default=0.5, + type=float, + help="0.5 seconds by default. How many past seconds are used for prediction." + " Only available if split is chosen from {}".format(prediction_split), + ) + parser.add_argument( + "--overwrite", + action="store_true", + help="If the database_path exists, whether to overwrite it", + ) + parser.add_argument("--num_workers", type=int, default=8, help="number of workers to use") + args = parser.parse_args() + + overwrite = args.overwrite + dataset_name = args.dataset_name + output_path = args.database_path + version = args.split + + if version in scene_split: + scenarios, vods = get_vod_scenarios(args.dataroot, version, args.num_workers) + else: + scenarios, vods = get_vod_prediction_split(args.dataroot, version, args.past, args.future, args.num_workers) + write_to_directory( + convert_func=convert_vod_scenario, + scenarios=scenarios, + output_path=output_path, + dataset_version=version, + dataset_name=dataset_name, + overwrite=overwrite, + num_workers=args.num_workers, + vodelft=vods, + past=[args.past for _ in range(args.num_workers)], + future=[args.future for _ in range(args.num_workers)], + prediction=[version in prediction_split for _ in range(args.num_workers)], + map_radius=[args.map_radius for _ in range(args.num_workers)], + ) diff --git a/scenarionet/converter/vod/__init__.py b/scenarionet/converter/vod/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scenarionet/converter/vod/type.py b/scenarionet/converter/vod/type.py new file mode 100644 index 0000000..e5031b1 --- /dev/null +++ b/scenarionet/converter/vod/type.py @@ -0,0 +1,90 @@ +ALL_TYPE = { + "noise": 'noise', + "human.pedestrian.adult": 'adult', + "human.pedestrian.child": 'child', + "human.pedestrian.wheelchair": 'wheelchair', + "human.pedestrian.stroller": 'stroller', + "human.pedestrian.personal_mobility": 'p.mobility', + "human.pedestrian.police_officer": 'police', + "human.pedestrian.construction_worker": 'worker', + "animal": 'animal', + "vehicle.car": 'car', + "vehicle.motorcycle": 'motorcycle', + "vehicle.bicycle": 'bicycle', + "vehicle.bus.bendy": 'bus.bendy', + "vehicle.bus.rigid": 'bus.rigid', + "vehicle.truck": 'truck', + "vehicle.construction": 'constr. veh', + "vehicle.emergency.ambulance": 'ambulance', + "vehicle.emergency.police": 'police car', + "vehicle.trailer": 'trailer', + "movable_object.barrier": 'barrier', + "movable_object.trafficcone": 'trafficcone', + "movable_object.pushable_pullable": 'push/pullable', + "movable_object.debris": 'debris', + "static_object.bicycle_rack": 'bicycle racks', + "flat.driveable_surface": 'driveable', + "flat.sidewalk": 'sidewalk', + "flat.terrain": 'terrain', + "flat.other": 'flat.other', + "static.manmade": 'manmade', + "static.vegetation": 'vegetation', + "static.other": 'static.other', + "vehicle.ego": "ego", + # ADDED: + "static.vehicle.bicycle": "static.other", + "static.vehicle.motorcycle": "static.other", + "vehicle.other": "vehicle.other", + "static.vehicle.other": "static.other", + "vehicle.unknown": "vehicle.unknown" +} +NOISE_TYPE = { + "noise": 'noise', + "animal": 'animal', + "static_object.bicycle_rack": 'bicycle racks', + "movable_object.pushable_pullable": 'push/pullable', + "movable_object.debris": 'debris', + "static.manmade": 'manmade', + "static.vegetation": 'vegetation', + "static.other": 'static.other', + "static.vehicle.bicycle": "static.other", + "static.vehicle.motorcycle": "static.other", + "static.vehicle.other": "static.other", +} +HUMAN_TYPE = { + "human.pedestrian.adult": 'adult', + "human.pedestrian.child": 'child', + "human.pedestrian.wheelchair": 'wheelchair', + "human.pedestrian.stroller": 'stroller', + "human.pedestrian.personal_mobility": 'p.mobility', + "human.pedestrian.police_officer": 'police', + "human.pedestrian.construction_worker": 'worker', +} +BICYCLE_TYPE = { + "vehicle.bicycle": 'bicycle', + "vehicle.motorcycle": 'motorcycle', +} +VEHICLE_TYPE = { + "vehicle.car": 'car', + "vehicle.bus.bendy": 'bus.bendy', + "vehicle.bus.rigid": 'bus.rigid', + "vehicle.truck": 'truck', + "vehicle.construction": 'constr. veh', + "vehicle.emergency.ambulance": 'ambulance', + "vehicle.emergency.police": 'police car', + "vehicle.trailer": 'trailer', + "vehicle.ego": "ego", + # ADDED: + "vehicle.other": "vehicle.other", + "vehicle.unknown": "vehicle.other" +} +OBSTACLE_TYPE = { + "movable_object.barrier": 'barrier', + "movable_object.trafficcone": 'trafficcone', +} +TERRAIN_TYPE = { + "flat.driveable_surface": 'driveable', + "flat.sidewalk": 'sidewalk', + "flat.terrain": 'terrain', + "flat.other": 'flat.other' +} diff --git a/scenarionet/converter/vod/utils.py b/scenarionet/converter/vod/utils.py new file mode 100644 index 0000000..54c5932 --- /dev/null +++ b/scenarionet/converter/vod/utils.py @@ -0,0 +1,558 @@ +import copy +import logging + +import geopandas as gpd +import numpy as np +from metadrive.scenario import ScenarioDescription as SD +from metadrive.type import MetaDriveType +from vod.eval.prediction.splits import get_prediction_challenge_split +from shapely.ops import unary_union + +from scenarionet.converter.vod.type import ( + ALL_TYPE, + HUMAN_TYPE, + BICYCLE_TYPE, + VEHICLE_TYPE, +) + +logger = logging.getLogger(__name__) +try: + import logging + + logging.getLogger("shapely.geos").setLevel(logging.CRITICAL) + from vod import VOD + from vod.can_bus.can_bus_api import VODCanBus + from vod.eval.common.utils import quaternion_yaw + from vod.map_expansion.arcline_path_utils import discretize_lane + from vod.map_expansion.map_api import VODMap + from pyquaternion import Quaternion +except ImportError as e: + logger.warning("Can not import vod-devkit: {}".format(e)) + +EGO = "ego" + + +def get_metadrive_type(obj_type): + meta_type = obj_type + md_type = None + if ALL_TYPE[obj_type] == "barrier": + md_type = MetaDriveType.TRAFFIC_BARRIER + elif ALL_TYPE[obj_type] == "trafficcone": + md_type = MetaDriveType.TRAFFIC_CONE + elif obj_type in VEHICLE_TYPE: + md_type = MetaDriveType.VEHICLE + elif obj_type in HUMAN_TYPE: + md_type = MetaDriveType.PEDESTRIAN + elif obj_type in BICYCLE_TYPE: + md_type = MetaDriveType.CYCLIST + + # assert meta_type != MetaDriveType.UNSET and meta_type != "noise" + return md_type, meta_type + + +def parse_frame(frame, vod: VOD): + ret = {} + for obj_id in frame["anns"]: + obj = vod.get("sample_annotation", obj_id) + # velocity = vod.box_velocity(obj_id)[:2] + # if np.nan in velocity: + velocity = np.array([0.0, 0.0]) + ret[obj["instance_token"]] = { + "position": obj["translation"], + "obj_id": obj["instance_token"], + "heading": quaternion_yaw(Quaternion(*obj["rotation"])), + "rotation": obj["rotation"], + "velocity": velocity, + "size": obj["size"], + "visible": obj["visibility_token"], + "attribute": [vod.get("attribute", i)["name"] for i in obj["attribute_tokens"]], + "type": obj["category_name"], + } + # print(frame["data"]["dummy"]) + ego_token = vod.get("sample_data", frame["data"]["dummy"])["ego_pose_token"] + # print(ego_token) + ego_state = vod.get("ego_pose", ego_token) + ret[EGO] = { + "position": ego_state["translation"], + "obj_id": EGO, + "heading": quaternion_yaw(Quaternion(*ego_state["rotation"])), + "rotation": ego_state["rotation"], + "type": "vehicle.car", + "velocity": np.array([0.0, 0.0]), + # size https://en.wikipedia.org/wiki/Renault_Zoe + "size": [4.08, 1.73, 1.56], + } + return ret + + +def interpolate_heading(heading_data, old_valid, new_valid, num_to_interpolate=1): + new_heading_theta = np.zeros_like(new_valid) + for k, valid in enumerate(old_valid[:-1]): + if abs(valid) > 1e-1 and abs(old_valid[k + 1]) > 1e-1: + diff = (heading_data[k + 1] - heading_data[k] + np.pi) % (2 * np.pi) - np.pi + # step = diff + interpolate_heading = np.linspace(heading_data[k], heading_data[k] + diff, 2) # not sure if 2 is correct + new_heading_theta[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (interpolate_heading[:-1]) + elif abs(valid) > 1e-1 and abs(old_valid[k + 1]) < 1e-1: + new_heading_theta[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (heading_data[k]) + new_heading_theta[-1] = heading_data[-1] + return new_heading_theta * new_valid + + +def _interpolate_one_dim(data, old_valid, new_valid, num_to_interpolate=1): + new_data = np.zeros_like(new_valid) + for k, valid in enumerate(old_valid[:-1]): + if abs(valid) > 1e-1 and abs(old_valid[k + 1]) > 1e-1: + diff = data[k + 1] - data[k] + # step = diff + interpolate_data = np.linspace(data[k], data[k] + diff, num_to_interpolate + 1) + new_data[k * num_to_interpolate:(k + 1) * num_to_interpolate] = (interpolate_data[:-1]) + elif abs(valid) > 1e-1 and abs(old_valid[k + 1]) < 1e-1: + new_data[k * num_to_interpolate:(k + 1) * num_to_interpolate] = data[k] + new_data[-1] = data[-1] + return new_data * new_valid + + +def interpolate(origin_y, valid, new_valid): + if len(origin_y.shape) == 1: + ret = _interpolate_one_dim(origin_y, valid, new_valid) + elif len(origin_y.shape) == 2: + ret = [] + for dim in range(origin_y.shape[-1]): + new_y = _interpolate_one_dim(origin_y[..., dim], valid, new_valid) + new_y = np.expand_dims(new_y, axis=-1) + ret.append(new_y) + ret = np.concatenate(ret, axis=-1) + else: + raise ValueError("Y has shape {}, Can not interpolate".format(origin_y.shape)) + return ret + + +def get_tracks_from_frames(vod: VOD, scene_info, frames, num_to_interpolate=5): + episode_len = len(frames) + # Fill tracks + all_objs = set() + for frame in frames: + all_objs.update(frame.keys()) + tracks = { + k: dict( + type=MetaDriveType.UNSET, + state=dict( + position=np.zeros(shape=(episode_len, 3)), + heading=np.zeros(shape=(episode_len, )), + velocity=np.zeros(shape=(episode_len, 2)), + valid=np.zeros(shape=(episode_len, )), + length=np.zeros(shape=(episode_len, 1)), + width=np.zeros(shape=(episode_len, 1)), + height=np.zeros(shape=(episode_len, 1)), + ), + metadata=dict( + track_length=episode_len, + type=MetaDriveType.UNSET, + object_id=k, + original_id=k, + ), + ) + for k in list(all_objs) + } + + tracks_to_remove = set() + first = True + a = 0 + for frame_idx in range(episode_len): + # Record all agents' states (position, velocity, ...) + # if frame_idx == 0: + # continue + for id, state in frames[frame_idx].items(): + # Fill type + md_type, meta_type = get_metadrive_type(state["type"]) + tracks[id]["type"] = md_type + tracks[id][SD.METADATA]["type"] = meta_type + if md_type is None or md_type == MetaDriveType.UNSET: + tracks_to_remove.add(id) + continue + elif first: + first = False + id_f = id + + if id == id_f: + a += 1 + # print("FOOUND KEY: ", a, episode_len) + # print(state["position"]) + tracks[id]["type"] = md_type + tracks[id][SD.METADATA]["type"] = meta_type + + # Introducing the state item + if ((frame_idx == 0) or (frame_idx == 1)) and (id == list(frames[frame_idx].keys())[0]): + if state["position"][0] != 0: + print(state["position"], md_type) + tracks[id]["state"]["position"][frame_idx] = state["position"] + tracks[id]["state"]["heading"][frame_idx] = state["heading"] + tracks[id]["state"]["velocity"][frame_idx] = tracks[id]["state"]["velocity"][frame_idx] + tracks[id]["state"]["valid"][frame_idx] = 1 + + tracks[id]["state"]["length"][frame_idx] = state["size"][1] + tracks[id]["state"]["width"][frame_idx] = state["size"][0] + tracks[id]["state"]["height"][frame_idx] = state["size"][2] + + tracks[id]["metadata"]["original_id"] = id + tracks[id]["metadata"]["object_id"] = id + + for track in tracks_to_remove: + track_data = tracks.pop(track) + obj_type = track_data[SD.METADATA]["type"] + print("\nWARNING: Can not map type: {} to any MetaDrive Type".format(obj_type)) + + new_episode_len = (episode_len - 1) * num_to_interpolate + 1 + + # interpolate + interpolate_tracks = {} + for ( + id, + track, + ) in tracks.items(): + interpolate_tracks[id] = copy.deepcopy(track) + interpolate_tracks[id]["metadata"]["track_length"] = new_episode_len + + # valid first + new_valid = np.zeros(shape=(new_episode_len, )) + if track["state"]["valid"][0]: + new_valid[0] = 1 + for k, valid in enumerate(track["state"]["valid"][1:], start=1): + if valid: + if abs(new_valid[(k - 1) * num_to_interpolate] - 1) < 1e-2: + start_idx = (k - 1) * num_to_interpolate + 1 + else: + start_idx = k * num_to_interpolate + new_valid[start_idx:k * num_to_interpolate + 1] = 1 + interpolate_tracks[id]["state"]["valid"] = new_valid + + # position + interpolate_tracks[id]["state"]["position"] = interpolate( + track["state"]["position"], track["state"]["valid"], new_valid + ) + # print(np.diff(track["state"]["position"], axis=0)) + # print(interpolate_tracks[id]["state"]["position"], track["state"]["position"]) + if id == "ego" and not scene_info.get("prediction", False): + assert "prediction" not in scene_info + # We can get it from canbus + try: + canbus = VODCanBus(dataroot=vod.dataroot) + imu_pos = np.asarray([state["pos"] for state in canbus.get_messages(scene_info["name"], "pose")[::5]]) + min_len = min(len(imu_pos), new_episode_len) + interpolate_tracks[id]["state"]["position"][:min_len] = imu_pos[:min_len] + except: + logger.info("Fail to get canbus data for {}".format(scene_info["name"])) + + # velocity + interpolate_tracks[id]["state"]["velocity"] = interpolate( + track["state"]["velocity"], track["state"]["valid"], new_valid + ) + vel = (interpolate_tracks[id]["state"]["position"][1:] - interpolate_tracks[id]["state"]["position"][:-1]) + interpolate_tracks[id]["state"]["velocity"][:-1] = vel[..., :2] / 0.1 + for k, valid in enumerate(new_valid[1:], start=1): + if valid == 0 or not valid or abs(valid) < 1e-2: + interpolate_tracks[id]["state"]["velocity"][k] = np.array([0.0, 0.0]) + interpolate_tracks[id]["state"]["velocity"][k - 1] = np.array([0.0, 0.0]) + # speed outlier check + max_vel = np.max(np.linalg.norm(interpolate_tracks[id]["state"]["velocity"], axis=-1)) + if max_vel > 30: + print("\nWARNING: Too large speed for {}: {}".format(id, max_vel)) + + # heading + # then update position + new_heading = interpolate_heading(track["state"]["heading"], track["state"]["valid"], new_valid) + interpolate_tracks[id]["state"]["heading"] = new_heading + if id == "ego" and not scene_info.get("prediction", False): + assert "prediction" not in scene_info + # We can get it from canbus + try: + canbus = VODCanBus(dataroot=vod.dataroot) + imu_heading = np.asarray( + [ + quaternion_yaw(Quaternion(state["orientation"])) + for state in canbus.get_messages(scene_info["name"], "pose")[::5] + ] + ) + min_len = min(len(imu_heading), new_episode_len) + interpolate_tracks[id]["state"]["heading"][:min_len] = imu_heading[:min_len] + except: + logger.info("Fail to get canbus data for {}".format(scene_info["name"])) + + for k, v in track["state"].items(): + if k in ["valid", "heading", "position", "velocity"]: + continue + else: + interpolate_tracks[id]["state"][k] = interpolate(v, track["state"]["valid"], new_valid) + # if id == "ego": + # ego is valid all time, so we can calculate the velocity in this way + return interpolate_tracks + + +def get_map_features(scene_info, vod: VOD, map_center, radius=500, points_distance=1, only_lane=False): + """ + Extract map features from vod data. The objects in specified region will be returned. Sampling rate determines + the distance between 2 points when extracting lane center line. + """ + ret = {} + map_name = vod.get("log", scene_info["log_token"])["location"] + map_api = VODMap(dataroot=vod.dataroot, map_name=map_name) + + layer_names = [ + # "line", + # "polygon", + # "node", + "drivable_area", + "road_segment", + # 'road_block', + "lane", + "ped_crossing", + "walkway", + # 'stop_line', + # 'carpark_area', + "lane_connector", + # 'road_divider', + # 'lane_divider', + # 'traffic_light' + ] + # road segment includes all roadblocks (a list of lanes in the same direction), intersection and unstructured road + + map_objs = map_api.get_records_in_radius(map_center[0], map_center[1], radius, layer_names) + + if not only_lane: + # build map boundary + polygons = [] + for id in map_objs["drivable_area"]: + seg_info = map_api.get("drivable_area", id) + assert seg_info["token"] == id + for polygon_token in seg_info["polygon_tokens"]: + polygon = map_api.extract_polygon(polygon_token) + polygons.append(polygon) + # for id in map_objs["road_segment"]: + # seg_info = map_api.get("road_segment", id) + # assert seg_info["token"] == id + # polygon = map_api.extract_polygon(seg_info["polygon_token"]) + # polygons.append(polygon) + # for id in map_objs["road_block"]: + # seg_info = map_api.get("road_block", id) + # assert seg_info["token"] == id + # polygon = map_api.extract_polygon(seg_info["polygon_token"]) + # polygons.append(polygon) + polygons = [geom if geom.is_valid else geom.buffer(0) for geom in polygons] + boundaries = gpd.GeoSeries(unary_union(polygons)).boundary.explode(index_parts=True) + for idx, boundary in enumerate(boundaries[0]): + block_points = np.array(list(i for i in zip(boundary.coords.xy[0], boundary.coords.xy[1]))) + id = "boundary_{}".format(idx) + ret[id] = { + SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_WHITE, + SD.POLYLINE: block_points, + } + + # broken line + # for id in map_objs["lane_divider"]: + # line_info = map_api.get("lane_divider", id) + # assert line_info["token"] == id + # line = map_api.extract_line(line_info["line_token"]).coords.xy + # line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))]) + # ret[id] = {SD.TYPE: MetaDriveType.LINE_BROKEN_SINGLE_WHITE, SD.POLYLINE: line} + + # # solid line + # for id in map_objs["road_divider"]: + # line_info = map_api.get("road_divider", id) + # assert line_info["token"] == id + # line = map_api.extract_line(line_info["line_token"]).coords.xy + # line = np.asarray([[line[0][i], line[1][i]] for i in range(len(line[0]))]) + # ret[id] = {SD.TYPE: MetaDriveType.LINE_SOLID_SINGLE_YELLOW, SD.POLYLINE: line} + + # crosswalk + for id in map_objs["ped_crossing"]: + info = map_api.get("ped_crossing", id) + assert info["token"] == id + boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy + boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]) + ret[id] = { + SD.TYPE: MetaDriveType.CROSSWALK, + SD.POLYGON: boundary_polygon, + } + + # walkway + for id in map_objs["walkway"]: + info = map_api.get("walkway", id) + assert info["token"] == id + boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy + boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]) + ret[id] = { + SD.TYPE: MetaDriveType.BOUNDARY_SIDEWALK, + SD.POLYGON: boundary_polygon, + } + + # normal lane + for id in map_objs["lane"]: + lane_info = map_api.get("lane", id) + assert lane_info["token"] == id + boundary = map_api.extract_polygon(lane_info["polygon_token"]).boundary.xy + boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]) + # boundary_polygon += [[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))] + ret[id] = { + SD.TYPE: MetaDriveType.LANE_SURFACE_STREET, + SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)), + SD.POLYGON: boundary_polygon, + SD.ENTRY: map_api.get_incoming_lane_ids(id), + SD.EXIT: map_api.get_outgoing_lane_ids(id), + SD.LEFT_NEIGHBORS: [], + SD.RIGHT_NEIGHBORS: [], + } + + # intersection lane + for id in map_objs["lane_connector"]: + lane_info = map_api.get("lane_connector", id) + assert lane_info["token"] == id + # boundary = map_api.extract_polygon(lane_info["polygon_token"]).boundary.xy + # boundary_polygon = [[boundary[0][i], boundary[1][i], 0.1] for i in range(len(boundary[0]))] + # boundary_polygon += [[boundary[0][i], boundary[1][i], 0.] for i in range(len(boundary[0]))] + ret[id] = { + SD.TYPE: MetaDriveType.LANE_SURFACE_UNSTRUCTURE, + SD.POLYLINE: np.asarray(discretize_lane(map_api.arcline_path_3[id], resolution_meters=points_distance)), + # SD.POLYGON: boundary_polygon, + "speed_limit_kmh": 100, + SD.ENTRY: map_api.get_incoming_lane_ids(id), + SD.EXIT: map_api.get_outgoing_lane_ids(id), + } + + # # stop_line + # for id in map_objs["stop_line"]: + # info = map_api.get("stop_line", id) + # assert info["token"] == id + # boundary = map_api.extract_polygon(info["polygon_token"]).exterior.xy + # boundary_polygon = np.asarray([[boundary[0][i], boundary[1][i]] for i in range(len(boundary[0]))]) + # ret[id] = { + # SD.TYPE: MetaDriveType.STOP_LINE, + # SD.POLYGON: boundary_polygon , + # } + + # 'stop_line', + # 'carpark_area', + + return ret + + +def convert_vod_scenario( + token, + version, + vodelft: VOD, + map_radius=500, + prediction=False, + past=2, + future=6, + only_lane=False, +): + """ + Data will be interpolated to 0.1s time interval, while the time interval of original key frames are 0.5s. + """ + if prediction: + past_num = int(float(past) / 0.1) + future_num = int(float(future) / 0.1) + vode = vodelft + instance_token, sample_token = token.split("_") + current_sample = last_sample = next_sample = vode.get("sample", sample_token) + past_samples = [] + future_samples = [] + for _ in range(past_num): + if last_sample["prev"] == "": + break + last_sample = vode.get("sample", last_sample["prev"]) + past_samples.append(parse_frame(last_sample, vode)) + + for _ in range(future_num): + if next_sample["next"] == "": + break + next_sample = vode.get("sample", next_sample["next"]) + future_samples.append(parse_frame(next_sample, vode)) + frames = (past_samples[::-1] + [parse_frame(current_sample, vode)] + future_samples) + scene_info = copy.copy(vode.get("scene", current_sample["scene_token"])) + scene_info["name"] = scene_info["name"] + "_" + token + scene_info["prediction"] = True + frames_scene_info = [frames, scene_info] + else: + frames_scene_info = extract_frames_scene_info(token, vodelft) + + scenario_log_interval = 0.1 + frames, scene_info = frames_scene_info + result = SD() + result[SD.ID] = scene_info["name"] + result[SD.VERSION] = "vod" + version + result[SD.LENGTH] = len(frames) + result[SD.METADATA] = {} + result[SD.METADATA]["dataset"] = "vod" + result[SD.METADATA][SD.METADRIVE_PROCESSED] = False + result[SD.METADATA]["map"] = vodelft.get("log", scene_info["log_token"])["location"] + result[SD.METADATA]["date"] = vodelft.get("log", scene_info["log_token"])["date_captured"] + result[SD.METADATA]["coordinate"] = "right-handed" + # result[SD.METADATA]["dscenario_token"] = scene_token + result[SD.METADATA][SD.ID] = scene_info["name"] + result[SD.METADATA]["scenario_id"] = scene_info["name"] + result[SD.METADATA]["sample_rate"] = scenario_log_interval + result[SD.METADATA][SD.TIMESTEP] = np.arange(0.0, len(frames), 1) * 0.1 + # interpolating to 0.1s interval + result[SD.TRACKS] = get_tracks_from_frames(vodelft, scene_info, frames, num_to_interpolate=1) + result[SD.METADATA][SD.SDC_ID] = "ego" + + # No traffic light in vod at this stage + result[SD.DYNAMIC_MAP_STATES] = {} + if prediction: + track_to_predict = result[SD.TRACKS][instance_token] + result[SD.METADATA]["tracks_to_predict"] = { + instance_token: { + "track_index": list(result[SD.TRACKS].keys()).index(instance_token), + "track_id": instance_token, + "difficulty": 0, + "object_type": track_to_predict["type"], + } + } + + # map + print(result[SD.LENGTH], len(result[SD.METADATA][SD.TIMESTEP])) + map_center = np.array(result[SD.TRACKS]["ego"]["state"]["position"][0]) + result[SD.MAP_FEATURES] = get_map_features(scene_info, vodelft, map_center, map_radius, only_lane=only_lane) + del frames_scene_info + del frames + del scene_info + return result + + +def extract_frames_scene_info(scene, vod): + scene_token = scene["token"] + scene_info = vod.get("scene", scene_token) + scene_info["nbr_samples"] -= 1 + frames = [] + current_frame = vod.get("sample", scene_info["first_sample_token"]) + while current_frame["token"] != scene_info["last_sample_token"]: + frames.append(parse_frame(current_frame, vod)) + current_frame = vod.get("sample", current_frame["next"]) + frames.append(parse_frame(current_frame, vod)) + frames = frames[1:] + assert current_frame["next"] == "" + assert len(frames) == scene_info["nbr_samples"], "Number of sample mismatches! " + return frames, scene_info + + +def get_vod_scenarios(dataroot, version, num_workers=2): + vode = VOD(version=version, dataroot=dataroot) + + return vode.scene, [vode for _ in range(num_workers)] + + +def get_vod_prediction_split(dataroot, version, past, future, num_workers=2): + # TODO do properly + split_to_scene = { + "mini_train": "v1.0-mini", + "mini_val": "v1.0-mini", + "train": "v1.0-trainval", + "train_val": "v1.0-trainval", + "val": "v1.0-trainval", + "test": "v1.0-test", + } + + vode = VOD(version=split_to_scene[version], dataroot=dataroot) + + return get_prediction_challenge_split(version, dataroot=dataroot), [vode for _ in range(num_workers)]