├── train ├── __init__.py ├── configs │ ├── mix_net │ │ ├── net_params.json │ │ ├── trainer_params_train.json │ │ └── trainer_params_test.json │ └── indy_net │ │ └── default.json ├── indy_net_bayes.py ├── mix_net_dataset.py ├── indy_net_dataset.py ├── neural_network.py ├── indy_net_evaluation.py ├── data_set_helper.py └── indy_net_train.py ├── mix_net ├── __init__.py ├── mix_net │ ├── __init__.py │ ├── src │ │ ├── __init__.py │ │ ├── mix_net.py │ │ ├── indy_net.py │ │ ├── indy_net_handler.py │ │ └── rulebased_interaction.py │ ├── utils │ │ ├── __init__.py │ │ ├── cuda.py │ │ ├── handler_interface.py │ │ ├── helper.py │ │ ├── sort.py │ │ ├── setup_helpers.py │ │ ├── fuzzy.py │ │ ├── line_helper.py │ │ ├── overtake_fuzzy.py │ │ ├── geometry.py │ │ └── map_utils.py │ ├── data │ │ └── inference_model │ │ │ ├── mix_net │ │ │ ├── model.pth │ │ │ ├── net_params.json │ │ │ └── trainer_params_test.json │ │ │ └── indy_net │ │ │ ├── lstm_mse_noise.tar │ │ │ └── default.json │ └── config │ │ ├── main_params.ini │ │ └── README.md ├── resource │ └── mix_net ├── setup.cfg ├── test │ ├── test_copyright.py │ ├── test_pep257.py │ └── test_flake8.py ├── package.xml ├── setup.py └── launch │ └── mix_net.launch.py ├── assets └── mix_net_model.png ├── data ├── indy_net_sample.txt ├── mix_net_sample.pkl └── evaluation_data │ └── README.md ├── requirements.txt ├── .dockerignore ├── .gitignore ├── Dockerfile ├── tools ├── file_utils.py ├── track_helper.py ├── visualize_smoothness.py └── visualization.py └── README.md /train/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /mix_net/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /mix_net/mix_net/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /mix_net/resource/mix_net: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /mix_net/mix_net/src/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /assets/mix_net_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/MixNet/HEAD/assets/mix_net_model.png -------------------------------------------------------------------------------- /data/indy_net_sample.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/MixNet/HEAD/data/indy_net_sample.txt -------------------------------------------------------------------------------- /data/mix_net_sample.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/MixNet/HEAD/data/mix_net_sample.pkl -------------------------------------------------------------------------------- /mix_net/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/mix_net 3 | [install] 4 | install_scripts=$base/lib/mix_net 5 | -------------------------------------------------------------------------------- /mix_net/mix_net/data/inference_model/mix_net/model.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/MixNet/HEAD/mix_net/mix_net/data/inference_model/mix_net/model.pth -------------------------------------------------------------------------------- /mix_net/mix_net/data/inference_model/indy_net/lstm_mse_noise.tar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TUMFTM/MixNet/HEAD/mix_net/mix_net/data/inference_model/indy_net/lstm_mse_noise.tar -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | wheel==0.37.1 2 | bayesian_optimization==1.2.0 3 | GitPython==3.1.27 4 | matplotlib==3.4.2 5 | names==0.3.0 6 | numpy==1.20.3 7 | optuna==2.10.1 8 | pkbar==0.5 9 | pytest==7.1.2 10 | scikit_learn==1.1.1 11 | scipy==1.3.3 12 | setuptools==56.0.0 13 | Shapely==1.7.0 14 | torch==2.6.0 15 | tqdm==4.61.1 16 | tensorboard==2.9.1 17 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/cuda.py: -------------------------------------------------------------------------------- 1 | """This module provides a function to cudanize an artbitrary number of tensors.""" 2 | 3 | 4 | def cudanize(*args): 5 | """Transform an arbitrary amount of input tensors to cuda tensors. 6 | 7 | Returns: 8 | (tuple): cuda tensors in the same order as the given inputs 9 | """ 10 | arglist = [] 11 | for arg in list(args): 12 | try: 13 | arglist.append(arg.cuda()) 14 | except Exception: 15 | arglist.append(None) 16 | output = arglist[0] if len(arglist) <= 1 else tuple(arglist) 17 | return output 18 | -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | **/__pycache__ 2 | *.pyc 3 | .idea 4 | *.pth 5 | *.svg 6 | *.pdf 7 | **/*.pckl 8 | **/.classpath 9 | **/.dockerignore 10 | **/.env 11 | **/.venv 12 | **/.git 13 | **/.gitignore 14 | **/.project 15 | **/.settings 16 | **/.toolstarget 17 | **/.vs 18 | **/.vscode 19 | **/*.*proj.user 20 | **/*.dbmdl 21 | **/*.jfm 22 | **/azds.yaml 23 | **/bin 24 | **/charts 25 | **/docker-compose* 26 | **/Dockerfile* 27 | **/node_modules 28 | **/npm-debug.log 29 | **/obj 30 | **/secrets.dev.yaml 31 | **/values.dev.yaml 32 | README. 33 | **/install/ 34 | install/ 35 | **/build 36 | build/ 37 | **/log/ 38 | log/ 39 | data/ 40 | logs/ 41 | venv/ 42 | venv* 43 | assets/ -------------------------------------------------------------------------------- /train/configs/mix_net/net_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "use_cuda": true, 3 | "encoder": { 4 | "in_size": 16, 5 | "hidden_size": 64 6 | }, 7 | "mixer_linear_stack": { 8 | "hidden_sizes": [ 9 | 256, 10 | 256, 11 | 32 12 | ], 13 | "out_size": 4 14 | }, 15 | "init_vel_linear_stack": { 16 | "hidden_sizes": [ 17 | 32 18 | ], 19 | "out_size": 1, 20 | "max_vel": 90.0 21 | }, 22 | "acc_decoder": { 23 | "in_size": 16, 24 | "hidden_size": 32, 25 | "num_acc_sections": 5, 26 | "max_acc": 30 27 | } 28 | } -------------------------------------------------------------------------------- /mix_net/mix_net/data/inference_model/mix_net/net_params.json: -------------------------------------------------------------------------------- 1 | { 2 | "use_cuda": true, 3 | "encoder": { 4 | "in_size": 16, 5 | "hidden_size": 64 6 | }, 7 | "mixer_linear_stack": { 8 | "hidden_sizes": [ 9 | 256, 10 | 256, 11 | 32 12 | ], 13 | "out_size": 4 14 | }, 15 | "init_vel_linear_stack": { 16 | "hidden_sizes": [ 17 | 32 18 | ], 19 | "out_size": 1, 20 | "max_vel": 90.0 21 | }, 22 | "acc_decoder": { 23 | "in_size": 16, 24 | "hidden_size": 32, 25 | "num_acc_sections": 5, 26 | "max_acc": 30 27 | } 28 | } -------------------------------------------------------------------------------- /mix_net/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found errors" 24 | -------------------------------------------------------------------------------- /mix_net/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=[".", "test"]) 23 | assert rc == 0, "Found code style errors / warnings" 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .devcontainer 3 | *.csv 4 | *.xlsx 5 | *.png 6 | *.svg 7 | *.log 8 | *.saa 9 | *.txt 10 | *.eps 11 | *.pdf 12 | *.pkl 13 | *.tar 14 | *.mp4 15 | *.egg-info 16 | *.onnx 17 | venv* 18 | misc 19 | __pycache__ 20 | 21 | # ROS2 22 | /log 23 | /install 24 | /build 25 | .idea 26 | 27 | # Keep requirements 28 | !requirements.txt 29 | 30 | # Allow data-generation-configs 31 | train/tb_logs 32 | train/trained_models 33 | 34 | # don't ignore some parameter files. 35 | !train/configs 36 | 37 | # training data 38 | data/mix_net_dataloader** 39 | !data/mix_net_dataloaders/sample/** 40 | !data/indy_net_sample.txt 41 | !data/mix_net_sample.pkl 42 | 43 | # Allow map data to be commited 44 | !mix_net/mix_net/data/map/* 45 | 46 | # Logs 47 | logs/ 48 | 49 | # Allow assets folder to be commited 50 | !*assets/* 51 | 52 | # Allow to keep best model 53 | !mix_net/mix_net/data/params/** 54 | !mix_net/mix_net/data/inference_model/** 55 | -------------------------------------------------------------------------------- /train/configs/indy_net/default.json: -------------------------------------------------------------------------------- 1 | { 2 | "gpu": "", 3 | "worker": 0, 4 | "data_path": "data/indy_net_sample.txt", 5 | "use_every_nth": 1.0, 6 | "splitted_data": false, 7 | "train_size": 0.8, 8 | "val_size": 0.1, 9 | "test_size": 0.1, 10 | "cut_hist_probability": 0.5, 11 | "hist_min_len": 5, 12 | "enc_dec_layer": "lstm", 13 | "decoder_type": "original", 14 | "encoder_size": 64, 15 | "decoder_size": 128, 16 | "in_length": 30, 17 | "out_length": 50, 18 | "dyn_embedding_size": 32, 19 | "input_embedding_size": 32, 20 | "train_flag": true, 21 | "save_path": "train/trained_models/indy_net", 22 | "save_best": true, 23 | "loss_fn": "WMSE", 24 | "lr_rmse": 1e-3, 25 | "lr_nll": 1e-3, 26 | "lr_rmse_decay_rate": 0.96, 27 | "tb_logs": "train/tb_logs", 28 | "pretrainEpochs": 4, 29 | "trainEpochs": 2, 30 | "batch_size": 64, 31 | "online_layer": 0, 32 | "ego_awareness": false 33 | } -------------------------------------------------------------------------------- /mix_net/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, "Found %d code style errors / warnings:\n" % len( 24 | errors 25 | ) + "\n".join(errors) 26 | -------------------------------------------------------------------------------- /mix_net/mix_net/data/inference_model/indy_net/default.json: -------------------------------------------------------------------------------- 1 | { 2 | "gpu": "", 3 | "worker": 0, 4 | "data_path": "data/indy_net_sample.txt", 5 | "use_every_nth": 1.0, 6 | "splitted_data": false, 7 | "train_size": 0.8, 8 | "val_size": 0.1, 9 | "test_size": 0.1, 10 | "cut_hist_probability": 0.5, 11 | "hist_min_len": 5, 12 | "enc_dec_layer": "lstm", 13 | "decoder_type": "original", 14 | "encoder_size": 64, 15 | "decoder_size": 128, 16 | "in_length": 30, 17 | "out_length": 50, 18 | "dyn_embedding_size": 32, 19 | "input_embedding_size": 32, 20 | "train_flag": true, 21 | "save_path": "train/trained_models/indy_net", 22 | "save_best": true, 23 | "loss_fn": "WMSE", 24 | "lr_rmse": 1e-3, 25 | "lr_nll": 1e-3, 26 | "lr_rmse_decay_rate": 0.96, 27 | "tb_logs": "train/tb_logs", 28 | "pretrainEpochs": 50, 29 | "trainEpochs": 30, 30 | "batch_size": 64, 31 | "online_layer": 0, 32 | "ego_awareness": false 33 | } -------------------------------------------------------------------------------- /mix_net/mix_net/utils/handler_interface.py: -------------------------------------------------------------------------------- 1 | from abc import ABCMeta, abstractmethod 2 | from typing import Tuple 3 | 4 | 5 | class HandlerInterface(metaclass=ABCMeta): 6 | """Implements the interface that every handler used for prediction 7 | needs to implement. 8 | """ 9 | 10 | @classmethod 11 | def __subclasshook__(cls, subclass): 12 | return ( 13 | hasattr(subclass, "predict") 14 | and callable(subclass.predict) 15 | or NotImplemented 16 | ) 17 | 18 | @abstractmethod 19 | def predict(self, **kwargs) -> Tuple[dict, dict, dict, dict, int]: 20 | """Prediction method that has to be implemented in the child classes. 21 | 22 | returns: 23 | pred_dict: (dict), the dictionary which then directly can be added to the prediction 24 | dict in the main, 25 | log_hist: (dict), the dict of history logs, 26 | log_obj: (dict), the dict of object logs, 27 | log_boundaries: (dict), the log of boundary logs, 28 | prediction_id: (int), The next prediction ID. 29 | """ 30 | raise NotImplementedError 31 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/helper.py: -------------------------------------------------------------------------------- 1 | import json 2 | import cProfile 3 | import io 4 | import pstats 5 | 6 | import numpy as np 7 | 8 | 9 | def get_param_dict(config_parser): 10 | """Write a config parser as a dict of dicts. 11 | 12 | Args: 13 | config_parser ([type]): [description] 14 | 15 | Returns: 16 | [type]: [description] 17 | """ 18 | param_dict = {} 19 | for sec in config_parser.sections(): 20 | for key, value in config_parser.items(sec): 21 | param_dict[sec] = json.loads(config_parser.get(sec, key)) 22 | 23 | return param_dict 24 | 25 | 26 | def fill_with_nans(array, desired_length): 27 | 28 | nan_mask = np.empty(desired_length) * np.nan 29 | nan_mask[: len(array)] = array 30 | 31 | return nan_mask 32 | 33 | 34 | def profile(func): 35 | def inner(*args, **kwargs): 36 | pr = cProfile.Profile() 37 | pr.enable() 38 | retval = func(*args, **kwargs) 39 | pr.disable() 40 | s = io.StringIO() 41 | sortby = "cumulative" 42 | ps = pstats.Stats(pr, stream=s).sort_stats(sortby) 43 | ps.print_stats() 44 | print(s.getvalue()) 45 | return retval 46 | 47 | return inner 48 | -------------------------------------------------------------------------------- /mix_net/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | mix_net 7 | 0.0.0 8 | Structured Deep Neural Motion Prediction of Opposing Vehicles for an Autonomous Racecar 9 | Phillip Karle 10 | Apache License 2.0 11 | 12 | rclcpp 13 | std_msgs 14 | sensor_msgs 15 | delphi_esr_msgs 16 | geometry_msgs 17 | tum_msgs 18 | 19 | rclcpp 20 | std_msgs 21 | sensor_msgs 22 | delphi_esr_msgs 23 | geometry_msgs 24 | tum_msgs 25 | 26 | ament_copyright 27 | ament_flake8 28 | ament_pep257 29 | python3-pytest 30 | 31 | 32 | ament_python 33 | 34 | 35 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # For more information, please refer to https://aka.ms/vscode-docker-python 2 | ARG TAG_BASE_IMAGE=0.0.0 3 | FROM tumiac/base:$TAG_BASE_IMAGE 4 | 5 | # first copying only the requirements file 6 | WORKDIR /dev_ws 7 | 8 | # Update apt 9 | RUN apt-get update 10 | 11 | # Python requirements 12 | COPY requirements.txt ./requirements.txt 13 | 14 | # Install python packages 15 | RUN apt-get update && pip3 install --upgrade pip \ 16 | && pip3 install --no-cache-dir -r ./requirements.txt --ignore-installed PyYAML \ 17 | && rm -rf ~/.cache/pip/* 18 | 19 | # Copy repo into the container 20 | COPY . /dev_ws/src/mod_prediction 21 | 22 | # Resolve dependencies 23 | RUN bash -c ". install/setup.bash && \ 24 | rosdep init && \ 25 | rosdep update && \ 26 | rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -r -y" 27 | 28 | # Set entrypoint by sourcing overlay workspace 29 | RUN echo '#!/bin/bash\nset -e\n\n# setup ros environment\nsource "/opt/ros/$ROS_DISTRO/setup.bash"\n. \ 30 | /dev_ws/install/setup.bash\nexec "$@"' > /ros_entrypoint.sh && \ 31 | chmod +x /ros_entrypoint.sh 32 | 33 | # Build and test workspace 34 | RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && \ 35 | colcon build --paths /dev_ws/src/mod_prediction/mix_net" 36 | 37 | ENTRYPOINT ["/ros_entrypoint.sh"] 38 | CMD ["bash"] 39 | -------------------------------------------------------------------------------- /train/configs/mix_net/trainer_params_train.json: -------------------------------------------------------------------------------- 1 | { 2 | "train": true, 3 | "test": false, 4 | "model": 5 | { 6 | "load_model": false, 7 | "model_load_path": "" 8 | }, 9 | "data": 10 | { 11 | "frequency": 10.0, 12 | "map_file_path": "mix_net/mix_net/data/map/traj_ltpl_cl_IMS_GPS.csv", 13 | "path": "data/mix_net_sample.pkl", 14 | "use_every_nth": 1.0, 15 | "from_saved_dataloader": false, 16 | "dataloader_path": "", 17 | "train_size": 0.8, 18 | "val_size": 0.1, 19 | "test_size": 0.1, 20 | "cut_hist_probability": 0.5, 21 | "min_hist_len": 5, 22 | "shuffle": true, 23 | "random_state": 0 24 | }, 25 | "training": 26 | { 27 | "pred_len": 51, 28 | "batch_size": 128, 29 | "num_epochs": 50, 30 | "loss_fn": "WMSE", 31 | "max_weight": 1.5, 32 | "weighting_horizon": 10, 33 | "lr": 5e-5, 34 | "lr_decay_rate": 0.997, 35 | "weight_decay": 1e-7, 36 | "model_save_path": "train/trained_models/mix_net" 37 | }, 38 | "logging": 39 | { 40 | "train_loss_log_interval": 10, 41 | "log_path": "train/logs/mix_net_training", 42 | "save_dataloaders": true, 43 | "dataloader_path": "data/mix_net_dataloaders", 44 | "save_train_dataloader": true, 45 | "save_val_dataloader": true, 46 | "save_test_dataloader": true 47 | } 48 | } -------------------------------------------------------------------------------- /train/configs/mix_net/trainer_params_test.json: -------------------------------------------------------------------------------- 1 | { 2 | "train": false, 3 | "test": true, 4 | "model": 5 | { 6 | "load_model": true, 7 | "model_load_path": "mix_net/mix_net/data/inference_model/mix_net/model.pth" 8 | }, 9 | "data": 10 | { 11 | "frequency": 10.0, 12 | "map_file_path": "mix_net/mix_net/data/map/traj_ltpl_cl_IMS_GPS.csv", 13 | "path": "", 14 | "use_every_nth": 1.0, 15 | "from_saved_dataloader": true, 16 | "dataloader_path": "data/mix_net_dataloaders/sample", 17 | "train_size": 0.8, 18 | "val_size": 0.1, 19 | "test_size": 0.1, 20 | "cut_hist_probability": 0.5, 21 | "min_hist_len": 5, 22 | "shuffle": true, 23 | "random_state": 0 24 | }, 25 | "training": 26 | { 27 | "pred_len": 51, 28 | "batch_size": 32, 29 | "num_epochs": 10, 30 | "loss_fn": "WMSE", 31 | "max_weight": 1.5, 32 | "weighting_horizon": 10, 33 | "lr": 1e-4, 34 | "lr_decay_rate": 0.997, 35 | "weight_decay": 1e-7, 36 | "model_save_path": "train/trained_models/mix_net" 37 | }, 38 | "logging": 39 | { 40 | "train_loss_log_interval": 10, 41 | "log_path": "train/logs/mix_net_training", 42 | "save_dataloaders": false, 43 | "dataloader_path": "data/mix_net_dataloaders", 44 | "save_train_dataloader": false, 45 | "save_val_dataloader": false, 46 | "save_test_dataloader": false 47 | } 48 | } -------------------------------------------------------------------------------- /mix_net/mix_net/data/inference_model/mix_net/trainer_params_test.json: -------------------------------------------------------------------------------- 1 | { 2 | "train": false, 3 | "test": true, 4 | "model": 5 | { 6 | "load_model": true, 7 | "model_load_path": "mix_net/mix_net/data/inference_model/mix_net/model.pth" 8 | }, 9 | "data": 10 | { 11 | "frequency": 10.0, 12 | "map_file_path": "data/map_data/traj_ltpl_cl_LO_GPS.csv", 13 | "path": "", 14 | "use_every_nth": 1.0, 15 | "from_saved_dataloader": true, 16 | "dataloader_path": "data/mix_net_dataloaders/2022_07_15/09_29_24", 17 | "train_size": 0.8, 18 | "val_size": 0.1, 19 | "test_size": 0.1, 20 | "cut_hist_probability": 0.5, 21 | "min_hist_len": 5, 22 | "shuffle": true, 23 | "random_state": 0 24 | }, 25 | "training": 26 | { 27 | "pred_len": 51, 28 | "batch_size": 32, 29 | "num_epochs": 10, 30 | "loss_fn": "WMSE", 31 | "max_weight": 6.0, 32 | "weighting_horizon": 25, 33 | "lr": 1e-4, 34 | "lr_decay_rate": 0.997, 35 | "weight_decay": 1e-7, 36 | "model_save_path": "mod_prediction/trained_models/mix_net" 37 | }, 38 | "logging": 39 | { 40 | "train_loss_log_interval": 10, 41 | "log_path": "logs/mix_net_training", 42 | "save_dataloaders": false, 43 | "dataloader_path": "data/mix_net_dataloaders", 44 | "save_train_dataloader": false, 45 | "save_val_dataloader": false, 46 | "save_test_dataloader": false 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /mix_net/mix_net/config/main_params.ini: -------------------------------------------------------------------------------- 1 | # Parameters for Handling of observation storage and elemination 2 | [OBJ_HANDLING_PARAMS] 3 | vals={"max_obs_length": 30} 4 | 5 | [MODEL_PARAMS] 6 | vals={"model_type": "MixNet", 7 | "sampling_frequency": 10, 8 | "data_min_obs_length": 1.1, 9 | "data_min_velocity": 0.0, 10 | "data_max_acceleration": 10.0, 11 | "stat_vel_threshhold": 5.0, 12 | "stat_prediction_horizon": 5.0, 13 | "view": 400, 14 | "dist": 20 15 | } 16 | 17 | [FILE_NAMES] 18 | vals = {"indy_net_params": "default.json", 19 | "indy_net_weights": "lstm_mse_noise.tar", 20 | "mix_net_params": "net_params.json", 21 | "mix_net_weights": "model.pth", 22 | "map_file": "traj_ltpl_cl_IMS_GPS.csv" 23 | } 24 | 25 | [MIX_NET_PARAMS] 26 | vals = {"physics_based_const_vel": false, 27 | "physics_based_init_vel": true, 28 | "safety_physics_override": true, 29 | "override_error": 2.0, 30 | "pred_len": 50, 31 | "dt": 0.1, 32 | "data_min_obs_length": 5 33 | } 34 | 35 | [INTERACTION_PARAMS] 36 | vals = {"rule_based": true, 37 | "no_iterations": 1, 38 | "priority_on_ego": false, 39 | "delta_v_overtake": 5.0, 40 | "lanechange_time": 2.0, 41 | "lat_overtake_dist": 4.0, 42 | "lat_veh_half_m": 0.943, 43 | "long_veh_half_m": 2.4605, 44 | "collision_check": "euclidean", 45 | "approx_radius": 2.0, 46 | "lat_safety_m": 1.5, 47 | "long_safety_m": 1.5 48 | } 49 | 50 | [LOGGING_PARAMS] 51 | vals = {"history": true, 52 | "boundaries": false, 53 | "time_array": false, 54 | "num_covs": 1, 55 | "heading": false 56 | } 57 | -------------------------------------------------------------------------------- /mix_net/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | from setuptools import setup 4 | 5 | package_name = "mix_net" 6 | 7 | setup( 8 | name=package_name, 9 | version="0.0.0", 10 | packages=[package_name], 11 | data_files=[ 12 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 13 | ("share/" + package_name, ["package.xml"]), 14 | (os.path.join("share", package_name), glob("launch/*.launch.py")), 15 | # data 16 | (os.path.join("lib", package_name, "config"), glob("mix_net/config/*.ini")), 17 | ( 18 | os.path.join("lib", package_name, "data", "map"), 19 | glob("mix_net/data/map/*"), 20 | ), 21 | ( 22 | os.path.join("lib", package_name, "data", "inference_model", "indy_net"), 23 | glob("mix_net/data/inference_model/indy_net/*"), 24 | ), 25 | ( 26 | os.path.join("lib", package_name, "data", "inference_model", "mix_net"), 27 | glob("mix_net/data/inference_model/mix_net/*"), 28 | ), 29 | # src 30 | (os.path.join("lib", package_name, "src"), glob("mix_net/src/*.py")), 31 | # utils 32 | (os.path.join("lib", package_name, "utils"), glob("mix_net/utils/*.py")), 33 | ], 34 | install_requires=["setuptools"], 35 | zip_safe=True, 36 | author="Phillip Karle", 37 | author_email="phillip.karle@tum.de", 38 | maintainer="Phillip Karle", 39 | maintainer_email="phillip.karle@tum.de", 40 | description="MixNet: Structured Deep Neural Motion Prediction of Opposing Vehicles for Autonomous Racing", 41 | license="Apache License 2.0", 42 | tests_require=["pytest"], 43 | entry_points={ 44 | "console_scripts": ["mix_net_node = mix_net.mix_net_node:main"], 45 | }, 46 | ) 47 | -------------------------------------------------------------------------------- /tools/file_utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | 4 | def list_dirs_with_file(root_dirs, file_name): 5 | """Returns a list of all dirs under any of the dirs in root_dirs which contain 6 | a file which's name contains the the string give in file_name. 7 | 8 | args: 9 | root_dirs: (list), list of directories to search under. 10 | file_name: (str), the phrase to search for in the file names. 11 | 12 | returns: 13 | A list with all the dirs which contain a file which includes the file_name string. 14 | """ 15 | 16 | assert isinstance(root_dirs, list), "The given root_dirs is not a list." 17 | 18 | dir_list = [] 19 | 20 | for root_dir in root_dirs: 21 | if not os.path.exists(root_dir): 22 | continue 23 | 24 | for path, _, files in os.walk(root_dir): 25 | for name in files: 26 | if file_name in name: 27 | dir_list.append(os.path.dirname(os.path.join(path, name))) 28 | 29 | return list(set(dir_list)) 30 | 31 | 32 | def list_files_with_name(roots, file_name=""): 33 | """Lists every file under the given roots which contain the file_name. 34 | Some of the paths in roots can also be a file name it self, these are just 35 | simply going to be added to the list. 36 | 37 | args: 38 | roots: (list), the list of dirs and files to search under. 39 | file_name: (str), the string that every returned file name must contain. 40 | 41 | returns: 42 | the list of files. 43 | """ 44 | 45 | assert isinstance(roots, list), "The given root_dirs is not a list." 46 | 47 | file_list = [] 48 | root_dirs = [] 49 | 50 | for path in roots: 51 | if os.path.exists(path): 52 | if os.path.isfile(path) and (file_name in path): 53 | file_list.append(path) 54 | elif os.path.isdir(path): 55 | root_dirs.append(path) 56 | 57 | dir_list = list_dirs_with_file(root_dirs, file_name) 58 | 59 | for dir in dir_list: 60 | for path in os.listdir(dir): 61 | total_path = os.path.join(dir, path) 62 | if os.path.isfile(total_path) and (file_name in total_path): 63 | file_list.append(total_path) 64 | 65 | return list(set(file_list)) 66 | -------------------------------------------------------------------------------- /mix_net/launch/mix_net.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description(): 8 | # Initialize launch parameters 9 | frequency = LaunchConfiguration("frequency", default=20.0) 10 | track = LaunchConfiguration("track", default="IMS") 11 | use_sim_time = LaunchConfiguration("use_sim_time", default=False) 12 | ci = LaunchConfiguration("ci", default=False) 13 | use_cuda = LaunchConfiguration("use_cuda", default=False) 14 | 15 | return LaunchDescription( 16 | [ 17 | # Declare Arguments 18 | DeclareLaunchArgument( 19 | "frequency", 20 | default_value=frequency, 21 | description="Specify node frequency in Hz", 22 | ), 23 | DeclareLaunchArgument( 24 | "track", 25 | default_value=track, 26 | description="Specify track to use correct map, currently only 'IMS' supported", 27 | ), 28 | DeclareLaunchArgument( 29 | "use_sim_time", 30 | default_value=use_sim_time, 31 | description="Set node to use sim time to replay ros2-bags", 32 | ), 33 | DeclareLaunchArgument( 34 | "ci", 35 | default_value=ci, 36 | description="trigger ci shut down", 37 | ), 38 | DeclareLaunchArgument( 39 | "use_cuda", 40 | default_value=use_cuda, 41 | description="Set boolean to use cuda", 42 | ), 43 | # Create Node 44 | Node( 45 | package="mix_net", 46 | executable="mix_net_node", 47 | name="MIX_NET", 48 | namespace="", 49 | parameters=[ 50 | { 51 | "frequency": frequency, 52 | "track": track, 53 | "use_sim_time": use_sim_time, 54 | "ci": ci, 55 | "use_cuda": use_cuda, 56 | } 57 | ], 58 | arguments=["--ros-args"], 59 | ), 60 | ] 61 | ) 62 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/sort.py: -------------------------------------------------------------------------------- 1 | from random import randint 2 | 3 | 4 | def quicksort(array, compare_fn): 5 | 6 | # If the input array contains fewer than two elements, 7 | # then return it as the result of the function 8 | 9 | if len(array) < 2: 10 | 11 | return array 12 | 13 | low, same, high = [], [], [] 14 | 15 | # Select your `pivot` element randomly 16 | pivot = array[randint(0, len(array) - 1)] 17 | 18 | for item in array: 19 | # Elements that are smaller than the `pivot` go to 20 | # the `low` list. Elements that are larger than 21 | # `pivot` go to the `high` list. Elements that are 22 | # equal to `pivot` go to the `same` list. 23 | if item == pivot: 24 | same.append(item) 25 | elif compare_fn(item, pivot): 26 | low.append(item) 27 | else: 28 | high.append(item) 29 | 30 | # The final result combines the sorted `low` list 31 | # with the `same` list and the sorted `high` list 32 | 33 | return quicksort(low, compare_fn) + same + quicksort(high, compare_fn) 34 | 35 | 36 | def sort_collisions(collision_list, priority_list): 37 | """Sort a collision list with a given priority list. 38 | 39 | Args: 40 | collision_list ([list]): [list of collision dicts] 41 | priority_list ([list]): [sorted list of prediction IDs according to their priority] 42 | 43 | Returns: 44 | [list]: [sorted list of collision dicts] 45 | """ 46 | for collision in collision_list: 47 | collision["priority"] = priority_list.index( 48 | collision["pred_ids"][0] 49 | ) + priority_list.index(collision["pred_ids"][1]) 50 | 51 | # Setting who is leading and who is following based on 52 | # the vehicle indices in the priority list: 53 | if priority_list.index(collision["pred_ids"][0]) > priority_list.index( 54 | collision["pred_ids"][1] 55 | ): 56 | collision["following_pred_id"] = collision["pred_ids"][0] 57 | collision["leading_pred_id"] = collision["pred_ids"][1] 58 | else: 59 | collision["following_pred_id"] = collision["pred_ids"][1] 60 | collision["leading_pred_id"] = collision["pred_ids"][0] 61 | 62 | collision_list_sorted = sorted(collision_list, key=lambda k: k["priority"]) 63 | 64 | return collision_list_sorted 65 | -------------------------------------------------------------------------------- /data/evaluation_data/README.md: -------------------------------------------------------------------------------- 1 | # Evaluation data 2 | 3 | * 0_scenarios: Contains the original scenarios that can be replayed to the models. 4 | * 1_mixnet: Logs generated by replaying the scenarios to the module using MixNet. 5 | * 2_indynet: Logs generated by replaying the scenarios to the module using IndyNet. 6 | 7 | ## Overall prediction performance 8 | Evaluates the prediction error and sensitivity against semantics (number of object, speed, history length) of the models. Results are stored to [data/evaluation_data/results](results). 9 | 1. MixNet 10 | ``` 11 | python tools/evaluation.py --logdir data/evaluation_data/1_mixnet --save-path data/evaluation_data/results/mix_net --RMSE_tot 12 | ``` 13 | ``` 14 | ---------- Total RMSE (L2-Norm) ---------- 15 | Number of datapoints used for evaluation: 38493 16 | Overall RMSE (L2-Norm) = 4.096 m 17 | Overall average velocity in the logs: 71.322 m/s 18 | ``` 19 | With the flag `--rail`, the rail-based prediction is also evaluated in addition. 20 | 21 | 2. IndyNet: 22 | ``` 23 | python tools/evaluation.py --logdir data/evaluation_data/2_indynet --save-path data/evaluation_data/results/indy_net --RMSE_tot 24 | ``` 25 | Output: 26 | ``` 27 | ---------- Total RMSE (L2-Norm) ---------- 28 | Number of datapoints used for evaluation: 38342 29 | Overall RMSE (L2-Norm) = 4.481 m 30 | Overall average velocity in the logs: 71.532 m/s 31 | ``` 32 | 33 | ## Line plots 34 | Creates line plots of IndyNet, rail-based model and MixNet, stored to [data/evaluation_data/line_plots](line_plots). 35 | ``` 36 | python tools/evaluation_line_plot.py --save-path data/evaluation_data/line_plots 37 | ``` 38 | 39 | ## Input-Output correlation 40 | Analysis of MixNet's superposition behaior based on synthetic input curves. Created plots are stored to [data/evaluation_data/input_output](input_output). 41 | ``` 42 | python tools/input_output_analysis.py 43 | ``` 44 | Output with `seed = 42` is: 45 | ``` 46 | Overall RMSE: 0.8044372797012329 47 | weights in: [0.4 0.1 0.3 0.2] 48 | weights out: [0.22620717 0.17092304 0.10150512 0.50136465] 49 | ``` 50 | 51 | ## Smoothness plot 52 | Plot of an exemplary scenario to compare the prediction behavior of IndyNet, rail-based prediction and MixNet. Figure is saved to [data/evaluation_data/smoothness](smoothness). Recommended scenario to evaluate is `scenario_02` at predictionID = 760. To compare the MixNet with the benchmark model both replayed logs have to be inputted, which are: 53 | - logdir_benchmark: "data/evaluation_data/2_indynet/12_13_21/" 54 | - logdir_mixnet: "data/evaluation_data/1_mixnet/14_23_40/" 55 | 56 | Run the following command to visualize the exemplary sample: 57 | ``` 58 | python tools/visualize_smoothness.py 59 | ``` -------------------------------------------------------------------------------- /train/indy_net_bayes.py: -------------------------------------------------------------------------------- 1 | """Bayesian optimization of IndyNet.""" 2 | import os 3 | import argparse 4 | import json 5 | from bayes_opt import BayesianOptimization 6 | from bayes_opt.logger import JSONLogger 7 | from bayes_opt.event import Events 8 | import names 9 | 10 | from indy_net_train import main 11 | 12 | 13 | def bayes(lr_rmse_log, lr_nll_log, lr_rmse_decay_rate): 14 | """Optimize inputted net.""" 15 | print( 16 | "lr_rmse_log: {:2f} \t lr_nll_log: {:2f} \t lr_rmse_decay_rate: {:4f}".format( 17 | lr_rmse_log, lr_nll_log, lr_rmse_decay_rate 18 | ) 19 | ) 20 | common_args["lr_rmse"] = 10**lr_rmse_log 21 | common_args["lr_nll"] = 10**lr_nll_log 22 | common_args["lr_rmse_decay_rate"] = lr_rmse_decay_rate 23 | 24 | common_args["model_name"] = names.get_first_name() 25 | 26 | gain = main(common_args, verbose=True) 27 | 28 | return gain 29 | 30 | 31 | if __name__ == "__main__": 32 | # Parse arguments 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument( 35 | "--config", type=str, default="train/configs/indy_net/default.json" 36 | ) 37 | parser.add_argument("--debug", action="store_true", default=False) 38 | parser.add_argument("--init_points", type=int, default=20) 39 | parser.add_argument("--n_iter", type=int, default=30) 40 | args = parser.parse_args() 41 | 42 | if args.debug: 43 | args.init_points = 2 44 | args.n_iter = 3 45 | 46 | # Load config 47 | with open(args.config, "r") as f: 48 | common_args = json.load(f) 49 | 50 | # Network Arguments 51 | common_args["use_cuda"] = bool(common_args["gpu"]) 52 | 53 | logger_path = "train/logs/indy_net_bayes" 54 | if not os.path.exists(logger_path): 55 | os.makedirs(logger_path) 56 | 57 | try: 58 | # Linux 59 | common_args["model_name"] = args.config.split("/")[2].split(".")[0] 60 | except IndexError: 61 | # Windows 62 | common_args["model_name"] = args.config.split("\\")[2].split(".")[0] 63 | 64 | common_args["debug"] = args.debug 65 | common_args["tb_logs"] = "train/tb_logs/bayes" 66 | 67 | # Bounded region of parameter space 68 | pbounds = { 69 | "lr_rmse_log": (-2.7, -3.5), 70 | "lr_nll_log": (-2.7, -3.5), 71 | "lr_rmse_decay_rate": (0.95, 0.999), 72 | } 73 | 74 | optimizer = BayesianOptimization( 75 | f=bayes, 76 | pbounds=pbounds, 77 | random_state=1, 78 | ) 79 | 80 | logger = JSONLogger(path=os.path.join(logger_path, "bayes_logs.json")) 81 | optimizer.subscribe(Events.OPTIMIZATION_STEP, logger) 82 | 83 | optimizer.maximize( 84 | init_points=args.init_points, 85 | n_iter=args.n_iter, 86 | ) 87 | -------------------------------------------------------------------------------- /mix_net/mix_net/config/README.md: -------------------------------------------------------------------------------- 1 | ## Parameter description 2 | All parameters can be set in [main_params](main_params.ini). 3 | 4 | ### Section : `MODEL_PARAMS` 5 | 6 | | Parameter | Type | Default | Description 7 | | ------------- | ------------- | ------ | ----- | 8 | model_type | str | MixNet | Choose prediction model, options: MixNet, IndyNet| 9 | sampling_frequency | float | 10.0 | Frequency in Hz to sample object history (network input) | 10 | data_min_obs_length | float | 1.1 | Minimal observation length to output a prediction | 11 | data_min_velocity | float | 0.0 | Velocity threshold in m/s, above data-based prediction is used | 12 | data_max_acceleration | float | 0.0 | Acceleration threshold in m/s², below data-based prediction is used | 13 | stat_vel_threshold | float | 5.0 | Distance of minimal movement in m, below object is predicted static | 14 | stat_prediction_horizon | float | 5.0 | Static prediction horizon in s | 15 | view | int | 400 | Length in m to sample boundaries in driving direction (network input) | 16 | dist | int | 20 | Sample distance in m of boundaries in driving direction (network input) | 17 | 18 | ### Section : `FILE_NAMES` 19 | Specify the file names for MixNet, IndyNet and map, which should be used by the module. All files have to be stored in the respective subfolders in `mix_net/mix_net/data`. 20 | 21 | 22 | ### Section : `MIX_NET_PARAMS` 23 | | Parameter | Type | Default | Description 24 | | ------------- | ------------- | ------ | ----- | 25 | physics_based_const_vel | boolean | false | If true, constant velocity assumption is applied | 26 | physics_based_init_vel | boolean| true | If true, initial velocity from observation storage is applied | 27 | safety_physics_override | boolean | true | If true, lateral offset at the beginning of the prediction horizon is correct | 28 | override_error | float | 2.0 | Distance threshold in m to apply override function | 29 | pred_len | int | 50 | Number of prediction steps 30 | dt | float | 0.1 | Time step size in s between prediction steps | 31 | data_min_obs_length | int | 5 | Minimal number of observation to apply data-based prediction 32 | 33 | 34 | ### Section : `INTERACTION_PARAMS` 35 | | Parameter | Type | Default | Description 36 | | ------------- | ------------- | ------ | ----- | 37 | rule_based | boolean | true | If true, interaction between the objects by fuzzy logic is considered | 38 | no_iterations | int | 1 | Number of iterations the fuzzy logic is applied to the objects to resolve conflicts | 39 | priority_on_ego | boolean | true | If true, collision free ego prediction is secured 40 | delta_v_overtake | float | 5.0 | Velocity difference in m/s, above overtaking is modeled | 41 | lanechange_time | float | 2.0 | Time in s of modeled overtaking maneuver | 42 | lat_overtake_dist | float | 4.0 | Lateral distance in m of modeled overtaking maneuver | 43 | lat_veh_half_m | float | 0.943 | Half vehicle width in m | 44 | long_veh_half_m | float | 2.4605| Half vehicle length in m | 45 | collision_check | str | euclidean | Type of collision check (if not 'euclidean' rectangle check is applied) | 46 | approx_radius | float | 2.0 | Radius in m used for euclidean collision check | 47 | lat_safety_m | float | 1.5 | Lateral safety distance in m for collision check | 48 | long_safety_m | float | 1.5 | Longitudinal safety distance in m for collision check | 49 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/setup_helpers.py: -------------------------------------------------------------------------------- 1 | """Helper functions to setup Objects class.""" 2 | import os 3 | import sys 4 | import configparser 5 | from shutil import copyfile 6 | import datetime 7 | 8 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 9 | sys.path.append(repo_path) 10 | 11 | from utils.logging_helper import ( 12 | DataLogging, 13 | MessageLogging, 14 | ) 15 | from utils.helper import get_param_dict 16 | 17 | 18 | def stamp2time(seconds: int, nanoseconds: int) -> int: 19 | return seconds * 1000000000 + nanoseconds 20 | 21 | 22 | def setup_logger(path_dict): 23 | """Setup main, detection and tracking logger.""" 24 | # Set up Prediction Logger 25 | if not os.path.exists(path_dict["abs_log_path"]): 26 | os.makedirs(path_dict["abs_log_path"]) 27 | 28 | # copy version.txt file to log folder: 29 | version_path = os.path.join(path_dict["src_path"], "version.txt") 30 | if os.path.exists(version_path): 31 | logdir = os.path.join(path_dict["abs_log_path"], "version.txt") 32 | copyfile(version_path, logdir) 33 | 34 | return ( 35 | MessageLogging(path_dict["main_log_path"]), 36 | DataLogging(path_dict["data_log_path"]), 37 | ) 38 | 39 | 40 | def get_params(path_dict): 41 | """Get all params.""" 42 | # read prediction parameter files 43 | main_param = configparser.ConfigParser() 44 | if not main_param.read(path_dict["main_param_path"]): 45 | raise ValueError( 46 | "Broken path to main_params: {}".format(path_dict["main_param_path"]) 47 | ) 48 | 49 | # get prediction params as dict 50 | param_dict = get_param_dict(main_param) 51 | param_dict["track_path"] = os.path.join( 52 | path_dict["map_path"], param_dict["FILE_NAMES"]["map_file"] 53 | ) 54 | 55 | return param_dict 56 | 57 | 58 | def get_raceline_csv( 59 | params: dict, 60 | raceline: str, 61 | track_key: str, 62 | ) -> str: 63 | """Get csv-file name for chosen raceline. 64 | 65 | Possible raceline: default, inner, outer, center (LVMS only) 66 | 67 | Args: 68 | tracking_params (dict): Contains tracking parameter 69 | raceline (str): String specifying raceline 70 | track_key (str): Abbreviation for track ('LVMS', 'IMS', 'LO') 71 | 72 | Returns: 73 | str: csv-file name for chosen raceline. 74 | """ 75 | track_file_str = "track_file_" + track_key 76 | if raceline == "default": 77 | return params[track_file_str] 78 | elif raceline == "center": 79 | return params[track_file_str + "_center"] 80 | elif raceline == "inner": 81 | return params[track_file_str + "_inner"] 82 | elif raceline == "outer": 83 | return params[track_file_str + "_outer"] 84 | 85 | return params[track_file_str] 86 | 87 | 88 | def create_path_dict(): 89 | """Create a dict of all required paths. 90 | 91 | Args: 92 | node_path (str): absolute path to module 93 | 94 | Returns: 95 | dict: dict with paths 96 | """ 97 | 98 | node_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 99 | 100 | # Global variables 101 | abs_log_path = os.path.join( 102 | node_path, 103 | "logs", 104 | datetime.date.today().strftime("%Y_%m_%d"), 105 | datetime.datetime.now().strftime("%H_%M_%S"), 106 | ) 107 | 108 | path_dict = { 109 | "node_path": node_path, 110 | "src_path": os.path.abspath(os.path.join("src", "mod_prediction")), 111 | "main_param_path": os.path.join( 112 | node_path, 113 | "config", 114 | "main_params.ini", 115 | ), 116 | "abs_log_path": abs_log_path, 117 | "main_log_path": os.path.join( 118 | abs_log_path, 119 | "prediction_main.csv", 120 | ), 121 | "data_log_path": os.path.join( 122 | abs_log_path, 123 | "prediction_data.csv", 124 | ), 125 | "mix_net_path": os.path.join(node_path, "data", "inference_model", "mix_net"), 126 | "indy_net_path": os.path.join(node_path, "data", "inference_model", "indy_net"), 127 | "map_path": os.path.join(node_path, "data", "map"), 128 | } 129 | 130 | return path_dict 131 | -------------------------------------------------------------------------------- /train/mix_net_dataset.py: -------------------------------------------------------------------------------- 1 | """Create dataset for MixNet.""" 2 | # Thrid party imports 3 | import torch 4 | from torch.utils.data import Dataset 5 | import numpy as np 6 | 7 | 8 | class MixNetDataset(Dataset): 9 | """Dataset for containing data that is needed for training the MixNet.""" 10 | 11 | def __init__(self, data, cut_probability, min_len, random_seed=0): 12 | """Initializes an MixNetDataset from the given data. 13 | 14 | args: 15 | data: (dict), already loaded data with the correct keys: 16 | "hist": (list of 2D lists) the history trajectories. 17 | "fut": (list of 2D lists) the groundtruth future trajectories. 18 | "fut_inds": (list of lists) the indices of the nearest centerline points 19 | corresponding to the ground truth prediction. 20 | "left_bd": (list of 2D lists) left track boundary snippet. 21 | "right_bd": (list of 2D lists) right track boundary snippet. 22 | """ 23 | 24 | self._cut_probability = cut_probability 25 | self._min_len = min_len 26 | self._rng = np.random.default_rng( 27 | random_seed 28 | ) # random number generator with seed 29 | 30 | # checking the data: 31 | keys_gotten = list(data.keys()) 32 | keys_needed = ["hist", "fut", "fut_inds", "left_bd", "right_bd"] 33 | 34 | for key in keys_needed: 35 | assert ( 36 | key in keys_gotten 37 | ), "Key {} is not found in the given data. Keys found: {}".format( 38 | key, keys_gotten 39 | ) 40 | 41 | self.D = data 42 | 43 | def __len__(self): 44 | """Must be defined for a Dataset""" 45 | 46 | return self.D["hist"].shape[0] 47 | 48 | def __getitem__(self, idx): 49 | """must be defined for a Dataset""" 50 | 51 | hist = self.D["hist"][ 52 | idx, ::-1, : 53 | ].copy() # Changing the order of history back to normal 54 | fut = self.D["fut"][idx, :, :] 55 | fut_inds = self.D["fut_inds"][idx] 56 | left_boundary = self.D["left_bd"][idx, :, :] 57 | right_boundary = self.D["right_bd"][idx, :, :] 58 | 59 | return hist, fut, fut_inds, left_boundary, right_boundary 60 | 61 | def collate_fn(self, samples): 62 | """Function that defines how the samples are collated when using mini-batch training. 63 | 64 | This is needed, because although every datasample has the same length originally, we would 65 | like to train with different history lengths. For every history, we decide: 66 | * With a probability it can keep its original length (30 usually) 67 | * If the sample does not keep its original lenght, its length is chosen randomly 68 | between a min and max bound. 69 | The collate function is needed in order to collate these different length histories. 70 | The remaining place is filled up with zeros. 71 | """ 72 | 73 | batch_size = len(samples) 74 | len_in = samples[0][0].shape[0] 75 | len_out = samples[0][1].shape[0] 76 | len_bound = samples[0][3].shape[0] 77 | 78 | # Initialize history, history lengths, future, output mask, lateral maneuver and longitudinal maneuver batches: 79 | hist_batch = torch.zeros((batch_size, len_in, 2), dtype=torch.float32) 80 | fut_batch = torch.zeros((batch_size, len_out, 2), dtype=torch.float32) 81 | fut_inds_batch = torch.zeros((batch_size, len_out), dtype=torch.int16) 82 | left_bd_batch = torch.zeros((batch_size, len_bound, 2), dtype=torch.float32) 83 | right_bd_batch = torch.zeros((batch_size, len_bound, 2), dtype=torch.float32) 84 | 85 | for i, (hist, fut, fut_inds, left_bd, right_bd) in enumerate(samples): 86 | # changing the length with a given probability: 87 | if self._rng.binomial(size=1, n=1, p=self._cut_probability): 88 | hist_len = int(self._rng.uniform(self._min_len, hist.shape[0])) 89 | else: 90 | hist_len = hist.shape[0] 91 | 92 | # Filling up the tensors, and also CHANGING THE ORDER OF HISTORY!!!: 93 | hist_batch[i, :hist_len, :] = torch.from_numpy(hist[-hist_len:, :]) 94 | fut_batch[i, :, :] = torch.from_numpy(fut) 95 | fut_inds_batch[i, :] = torch.from_numpy(fut_inds) 96 | left_bd_batch[i, :, :] = torch.from_numpy(left_bd) 97 | right_bd_batch[i, :, :] = torch.from_numpy(right_bd) 98 | 99 | return hist_batch, fut_batch, fut_inds_batch, left_bd_batch, right_bd_batch 100 | -------------------------------------------------------------------------------- /train/indy_net_dataset.py: -------------------------------------------------------------------------------- 1 | """Create IndyNet Dataset.""" 2 | # Thrid party imports 3 | import torch 4 | from torch.utils.data import Dataset 5 | import numpy as np 6 | 7 | 8 | # Dataset class for Indy 9 | class IndyDataset(Dataset): 10 | """Dataset for containing data that is needed for training the IndyNet.""" 11 | 12 | def __init__(self, data, cut_probability, min_len, random_seed=0): 13 | """Initializes an IndyDataset from the given data. 14 | 15 | args: 16 | data: (dict), already loaded data with the correct keys: 17 | "id": (list) the IDs 18 | "hist": (list of 2D lists) the history trajectories. 19 | "fut": (list of 2D lists) the groundtruth future trajectories. 20 | "left_bd": (list of 2D lists) left track boundary snippet. 21 | "right_bd": (list of 2D lists) right track boundary snippet. 22 | cut_probability: (float in [0, 1]), the probability that a datasample is going 23 | to be cut in the collate function to be able to train with different history lengths. 24 | min_len: (float), the minimum length of a history to which it can be cut to. 25 | random_seed: (float, default=0), the random seed with which the random lengths are generated. 26 | """ 27 | 28 | self._cut_probability = cut_probability 29 | self._min_len = min_len 30 | self._rng = np.random.default_rng( 31 | random_seed 32 | ) # random number generator with seed 33 | 34 | # checking the data: 35 | keys_gotten = list(data.keys()) 36 | keys_needed = ["id", "hist", "fut", "left_bd", "right_bd"] 37 | 38 | for key in keys_needed: 39 | assert ( 40 | key in keys_gotten 41 | ), "Key {} is not found in the given data. Keys found: {}".format( 42 | key, keys_gotten 43 | ) 44 | 45 | self.D = data 46 | 47 | def __len__(self): 48 | """Must be defined for a Dataset""" 49 | 50 | return self.D["hist"].shape[0] 51 | 52 | def __getitem__(self, idx): 53 | """must be defined for a Dataset""" 54 | 55 | # Get track history 'hist' = ndarray, and future track 'fut' = ndarray 56 | smpl_id = self.D["id"][idx] 57 | hist = self.D["hist"][ 58 | idx, ::-1, : 59 | ].copy() # Changing the order of the history back to normal!!!! 60 | fut = self.D["fut"][idx, :, :] 61 | left_boundary = self.D["left_bd"][idx, :, :] 62 | right_boundary = self.D["right_bd"][idx, :, :] 63 | try: 64 | ego = self.D["ego"][idx] 65 | except Exception: 66 | ego = None 67 | 68 | return smpl_id, hist, fut, left_boundary, right_boundary, ego 69 | 70 | def collate_fn(self, samples): 71 | """Function that defines how the samples are collated when using mini-batch training. 72 | 73 | This is needed, because although every datasample has the same length originally, we would 74 | like to train with different history lengths. For every history, we decide: 75 | * With a probability it can keep its original length (30 usually) 76 | * If the sample does not keep its original lenght, its length is chosen randomly 77 | between a min and max bound. 78 | The collate function is needed in order to collate these different length histories. 79 | The remaining place is filled up with zeros. 80 | """ 81 | 82 | batch_size = len(samples) 83 | len_in = samples[0][1].shape[0] 84 | len_out = samples[0][2].shape[0] 85 | len_bound = samples[0][3].shape[0] 86 | 87 | # Initialize history, history lengths, future, output mask, lateral maneuver and longitudinal maneuver batches: 88 | hist_batch = torch.zeros((len_in, batch_size, 2)) 89 | ego_batch = torch.zeros((len_in, batch_size, 2)) 90 | fut_batch = torch.zeros((len_out, batch_size, 2)) 91 | left_bd_batch = torch.zeros((len_bound, batch_size, 2)) 92 | right_bd_batch = torch.zeros((len_bound, batch_size, 2)) 93 | 94 | smpl_ids = [] 95 | for sampleId, (smpl_id, hist, fut, left_bd, right_bd, ego) in enumerate( 96 | samples 97 | ): 98 | # changing the length with a given probability: 99 | if self._rng.binomial(size=1, n=1, p=self._cut_probability): 100 | hist_len = int(self._rng.uniform(self._min_len, hist.shape[0])) 101 | else: 102 | hist_len = hist.shape[0] 103 | 104 | # Filling up the tensors, and also CHANGING THE ORDER OF HISTORY!!!: 105 | # After chaning the order hist[0] is the oldest history and hist[-1] ist the latest position. 106 | hist_batch[:hist_len, sampleId, :] = torch.from_numpy(hist[-hist_len:, :]) 107 | fut_batch[:, sampleId, :] = torch.from_numpy(fut) 108 | left_bd_batch[:, sampleId, :] = torch.from_numpy(left_bd) 109 | right_bd_batch[:, sampleId, :] = torch.from_numpy(right_bd) 110 | 111 | try: 112 | ego_batch[0 : len(ego), sampleId, 0] = torch.from_numpy(ego[:, 0]) 113 | ego_batch[0 : len(ego), sampleId, 1] = torch.from_numpy(ego[:, 1]) 114 | except Exception: 115 | ego_batch = None 116 | 117 | smpl_ids.append(smpl_id) 118 | 119 | return smpl_ids, hist_batch, fut_batch, left_bd_batch, right_bd_batch, ego_batch 120 | -------------------------------------------------------------------------------- /tools/track_helper.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import numpy as np 4 | 5 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 6 | sys.path.append(repo_path) 7 | 8 | from mix_net.mix_net.utils.map_utils import get_track_paths 9 | 10 | 11 | class TrackHelper: 12 | """Class to implement some helper functionalities regarding the track data.""" 13 | 14 | def __init__(self, track_file, repo_path): 15 | """Initializes the Track helper by loading the map data and carrying out 16 | some calculations on it. 17 | """ 18 | track_path = os.path.join(repo_path, "data", "map", track_file) 19 | if not os.path.exists(track_path): 20 | print( 21 | "Track Data is missing, try to run with default csv-file from data/map" 22 | ) 23 | track_path = None 24 | ( 25 | self.arc_center, 26 | self._center_line, 27 | self._bound_right_xy, 28 | self._bound_left_xy, 29 | self._track_width, 30 | ) = get_track_paths(track_path, bool_track_width=True) 31 | 32 | self._max_ind = self._bound_left_xy.shape[0] 33 | 34 | def point_is_in_track(self, point, hint=None): 35 | """Returns true if the point lies within the track. If first 36 | searches for the nearest point of the left track boundary, and then 37 | calculates the points distance from it. Then, it is determined, 38 | whether the point lies within the track. 39 | 40 | Depending on whether a hint was given or not for where to search 41 | for the nearest point, 2 different search methods are used: 42 | - If no hint is provided, simply the distance to all of the points 43 | is calculated. then the point with the smallest distance is taken. 44 | - If a hint is provided, the search starts from this index 45 | and then iteratively the next boundary point is taken until 46 | the distance is reducing. If the distance starts to rise, the 47 | previous point was the closest one. 48 | !!! WARNING !!! 49 | The hint should be "near" the real nearest index, or else 50 | algorithm converges to a wrong value. (By near we meen, it should be on 51 | the correct half of the track at least.) Else the assumption, that the 52 | distance towards the nearest point decreases monotonally does not 53 | necessarily hold. (So don't give hint if it is not a "good" hint.) 54 | !!! WARNING !!! 55 | 56 | args: 57 | point: (np.array), shape=(2,), the point to check. 58 | hint: (int), an index, around which the nearest point of 59 | the left boundary should be searched for. 60 | returns: 61 | tuple: 62 | - bool, that is true, if the point is within the track boundaries. 63 | - the index of the nearest point. 64 | """ 65 | 66 | if hint is not None: 67 | ind = self._get_nearest_ind_iterative(point, hint) 68 | else: 69 | ind = self._get_nearest_ind_naive(point) 70 | 71 | dist_left = np.linalg.norm((self._bound_left_xy[ind, :2] - point)) 72 | dist_right = np.linalg.norm((self._bound_right_xy[ind, :2] - point)) 73 | dist_max = self._track_width[ind] 74 | 75 | in_track = (dist_left <= dist_max) and (dist_right <= dist_max) 76 | return in_track, ind 77 | 78 | def _get_nearest_ind_iterative(self, point, hint=0): 79 | """Searches for the index of the nearest point of the 80 | left track boundary to the given point. It is an iteratice 81 | search that starts from the given hint index. The distance 82 | to the given point is calculated iteratively for the next point 83 | of the boundary until the distance sinks, hence the nearest point 84 | is found. 85 | 86 | !!! WARNING !!! 87 | The hint should be "near" the real nearest index, or else 88 | algorithm converges to a wrong value. (By near we meen, it should be on 89 | the correct half of the track at least.) Else the assumption, that the 90 | distance decreases monotonously towards the nearest point does not 91 | necessarily hold. 92 | """ 93 | 94 | ind = hint 95 | # just making sure, that ind is in the range [0, self._max_ind) 96 | while ind < 0: 97 | ind += self._max_ind 98 | while ind >= self._max_ind: 99 | ind -= self._max_ind 100 | 101 | ind_prev = ind - 1 if ind > 0 else self._max_ind - 1 102 | 103 | dist = np.linalg.norm(self._bound_left_xy[ind, :2] - point) 104 | dist_prev = np.linalg.norm(self._bound_left_xy[ind_prev, :2] - point) 105 | 106 | add_on = 1 if dist < dist_prev else -1 107 | 108 | counter = 0 109 | smallest = dist 110 | smallest_ind = ind 111 | 112 | # there are some small local minimas around the global minima, this is 113 | # why the 0.01 threshold is needed. 114 | while (smallest + 0.01) > dist: 115 | ind += add_on 116 | if ind >= self._max_ind: 117 | ind -= self._max_ind 118 | if ind < 0: 119 | ind += self._max_ind 120 | 121 | dist = np.linalg.norm(self._bound_left_xy[ind, :2] - point) 122 | 123 | if smallest > dist: 124 | smallest = dist 125 | smallest_ind = ind 126 | 127 | if counter > self._max_ind: 128 | print("Did not find solution for closest index in a whole round.") 129 | break 130 | 131 | return smallest_ind 132 | 133 | def _get_nearest_ind_naive(self, point): 134 | """Finds the nearest point of the left track boundary to 135 | the given point by the naive method: calculates the distance 136 | to all of the points and choses the index with the smallest distance. 137 | """ 138 | dists = np.linalg.norm((self._bound_left_xy[:, :2] - point), axis=1) 139 | return np.argmin(dists) 140 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/fuzzy.py: -------------------------------------------------------------------------------- 1 | """This script implements some (base) classes for fuzzy inference. 2 | The hierarhy from bottom up is as follows: 3 | - FuzzyMembershipFunction 4 | - FuzzyImplication 5 | - FuzzyInference 6 | 7 | A fuzzy membership function takes in some values and produces a 8 | membership value for a given class. 9 | Example: Is x a big value? A fuzzy membership function is defined 10 | That answers this question with a value between 0 and 1 for 11 | any given x. 12 | 13 | A fuzzy implication contains several membership functions, which 14 | are the so called evidences for an implication. 15 | Example: The color is red, the price is high, is it a Ferrary? 16 | The implication takes in all the data and evaluates each 17 | membership function (Is the color red? Is the price high?) 18 | Based on the resulting values and a combination rule, it 19 | draws the implication, what is the chance, that the car was 20 | a Ferrary. 21 | 22 | A fuzzy inference takes several fuzzy implications and draws the overall 23 | conclusion. Each implication has its own confidence, whether the car 24 | was a Ferrary. The inference takes these values and determines the overall 25 | conclusion with the help of a defined operation. (e.g. max operation) 26 | """ 27 | 28 | 29 | class FuzzyMembershipFunction: 30 | """Base class for implementing a fuzzy membership function 31 | class for a specific task. 32 | 33 | A Fuzzy Memmbership function has got a form: 34 | X: A(x) --> [0, 1] 35 | That is, it is a function that maps the input variable from 36 | the domain X to the [0, 1] interval, hence saying, up to which 37 | degree does a point x belong to a class A. 38 | 39 | Hence, what has to be implemented in a child class is a function 40 | that takes in a value (or tuple of values) x and produces a 41 | membership value as an output. 42 | """ 43 | 44 | def __call__(self, x): 45 | """This is the function that has to be implemented. 46 | It gives back the membership value corresponding to the 47 | value x. 48 | 49 | args: 50 | x: dict, that contains the values, based on which 51 | the membership value is determined. 52 | 53 | returns: 54 | membership value. 55 | """ 56 | raise NotImplementedError 57 | 58 | 59 | class FuzzyImplication: 60 | """Base class for implementing a fuzzy implication class 61 | for a specific task. 62 | 63 | A Fuzzy implication has got the form: 64 | IF U1 is A1 and U2 is A2 ... and Un is An THEN V is B 65 | 66 | therefore, a FuzzyImplication class has got some MembershipFunctions, 67 | which represent the Ui is Ai evidences and a relation operator, which 68 | determines the membership value of V in B. (up to which extent does V 69 | belong to the class B.) 70 | 71 | Hence, the function, that has to be implemented, is the one that takes 72 | the variable x, which is given to A1 ... An for evaluation and then based 73 | on the membership functions determines the value of B. 74 | In the easyest default version, this done by taking the minimum among the 75 | evidences, that is B = min{A1 is U1, ... , An is Un}. 76 | """ 77 | 78 | def __init__(self, evidences): 79 | """Initializes a FuzzyImplication object. 80 | 81 | args: 82 | evidences: list of FuzzyMembershipFunction objects 83 | """ 84 | 85 | self._evidences = evidences 86 | 87 | def __call__(self, x): 88 | """This is the function that can be overriden on demand. 89 | It generates the implication based on the observation x. 90 | In this default version the x observation is given to each 91 | evidence membership function and then the implication is 92 | determined by minimum operation. 93 | 94 | args: 95 | x: dict, that contains all the necessary values for each 96 | of the membership functions. Every membership function 97 | gets this x dictionary. 98 | """ 99 | 100 | membership_values = [ 101 | membership_function(x) for membership_function in self._evidences 102 | ] 103 | 104 | return min(membership_values) 105 | 106 | 107 | class FuzzyInference: 108 | """Base class for implementing a Fuzzy Inference class 109 | for a specific task. 110 | 111 | It takes several Fuzzy Implications and determines the 112 | overall conclusion. Formally, given: 113 | IF U11 is A11 and ... and U1n is A1n THEN V is B1 114 | IF U21 is A21 and ... and U2k is A2k THEN V is B2 115 | ... 116 | IF UM1 is AM1 and ... and UMp is AMp THEN V is BM 117 | and we know the results of all these implications, that is 118 | we know the Bi values, what is the overall conclusion that 119 | can be derived from these: 120 | F(B1, B2, ..., BM) = B 121 | 122 | In the default implementation, the inference is carried out 123 | by the max operator, that is: 124 | B = max{B1, B2, ..., BM} 125 | This can however be overriden in child classes. 126 | """ 127 | 128 | def __init__(self, implications): 129 | """Initializes a FuzzyInference object. 130 | 131 | args: 132 | implications: The list of FuzzyImplication objects. 133 | """ 134 | 135 | self._implications = implications 136 | 137 | def __call__(self, x): 138 | """Carries ot the inferenc based on the observation x. 139 | 140 | In the default implementation it amounts to getting the 141 | implications from all of the FuzzyImplication objects 142 | and carrying out a max operation on the results. 143 | 144 | args: 145 | x: dict, that contains all the necessary values for each 146 | of the membership functions. Every implication and hence 147 | every membership function gets this x dictionary. 148 | """ 149 | 150 | implications_results = [implication(x) for implication in self._implications] 151 | 152 | return max(implications_results) 153 | -------------------------------------------------------------------------------- /train/neural_network.py: -------------------------------------------------------------------------------- 1 | """Helper function for neural network train and inference.""" 2 | import torch 3 | 4 | 5 | def weighted_MSE(target, out, weights): 6 | """Weighted MSE loss to be able to cost the errors at 7 | different parts of the trajectories differently. 8 | 9 | args: 10 | target: (tensor with shape=(batch_size, pred_len, pose_size)) 11 | out: (tensor with shape=(batch_size, pred_len, pose_size)) 12 | weights: (tensor with shape=(pred_len)) 13 | 14 | returns: 15 | The scalar weighted MSE loss. 16 | """ 17 | 18 | loss = (target - out) ** 2 19 | 20 | if len(loss.shape) == 3: 21 | loss = torch.sum(loss, dim=(0, 2)) 22 | else: 23 | loss = torch.sum(loss, dim=(0)) 24 | 25 | return loss @ weights[: len(loss)] / (out.shape[0] * out.shape[1]) 26 | 27 | 28 | def MSE(y_pred, y_gt): 29 | """MSE Loss for single outputs. 30 | 31 | Arguments: 32 | y_pred {[type]} -- [description] 33 | y_gt {[type]} -- [description] 34 | 35 | Returns: 36 | [type] -- [description] 37 | """ 38 | 39 | # If GT has not enough timesteps, shrink y_pred 40 | if y_gt.shape[0] < y_pred.shape[0]: 41 | y_pred = y_pred[: y_gt.shape[0], :, :] 42 | 43 | muX = y_pred[:, :, 0] 44 | muY = y_pred[:, :, 1] 45 | x = y_gt[:, :, 0] 46 | y = y_gt[:, :, 1] 47 | mse_det = torch.pow(x - muX, 2) + torch.pow(y - muY, 2) 48 | count = torch.sum(torch.ones(mse_det.shape)) 49 | mse = torch.sum(mse_det) / count 50 | 51 | return mse, mse_det 52 | 53 | 54 | def MSE2(y_pred1, y_pred2, probs, y_gt): 55 | """MSE loss for multiple outputs 56 | 57 | Arguments: 58 | y_pred1 {[type]} -- [description] 59 | y_pred2 {[type]} -- [description] 60 | probs {[type]} -- [description] 61 | y_gt {[type]} -- [description] 62 | 63 | Returns: 64 | [type] -- [description] 65 | """ 66 | 67 | _, mse_det1 = MSE(y_pred1, y_gt) 68 | _, mse_det2 = MSE(y_pred2, y_gt) 69 | mse_det = probs[:, 0] * mse_det1 + probs[:, 1] * mse_det2 70 | count = torch.sum(torch.ones(mse_det.shape)) 71 | mse = torch.sum(mse_det) / count 72 | return mse, mse_det 73 | 74 | 75 | def NLL(y_pred, y_gt): 76 | """NLL loss for single output 77 | 78 | Arguments: 79 | y_pred {[type]} -- [description] 80 | y_gt {[type]} -- [description] 81 | 82 | Returns: 83 | [type] -- [description] 84 | """ 85 | 86 | # If GT has not enough timesteps, shrink y_pred 87 | if y_gt.shape[0] < y_pred.shape[0]: 88 | y_pred = y_pred[: y_gt.shape[0], :, :] 89 | 90 | muX = y_pred[:, :, 0] 91 | muY = y_pred[:, :, 1] 92 | sigX = y_pred[:, :, 2] # actually 1/ sigma_x 93 | sigY = y_pred[:, :, 3] # actually 1/sigma_y 94 | rho = y_pred[:, :, 4] 95 | ohr = torch.pow(1 - torch.pow(rho, 2), -0.5) 96 | x = y_gt[:, :, 0] 97 | y = y_gt[:, :, 1] 98 | out_det = torch.pow(ohr, 2) * ( 99 | torch.pow(sigX, 2) * torch.pow(x - muX, 2) 100 | + torch.pow(sigY, 2) * torch.pow(y - muY, 2) 101 | - 2 * rho * torch.pow(sigX, 1) * torch.pow(sigY, 1) * (x - muX) * (y - muY) 102 | ) - torch.log(sigX * sigY * ohr) 103 | count = torch.sum(torch.ones(out_det.shape)) 104 | out = torch.sum(out_det) / count 105 | return out, out_det 106 | 107 | 108 | def NLL2(y_pred1, y_pred2, probs, y_gt): 109 | """NLL loss for multiple trajectory outputs 110 | 111 | Arguments: 112 | y_pred1 {[type]} -- [description] 113 | y_pred2 {[type]} -- [description] 114 | probs {[type]} -- [description] 115 | y_gt {[type]} -- [description] 116 | 117 | Returns: 118 | [type] -- [description] 119 | """ 120 | nll1, nll_det1 = NLL(y_pred1, y_gt) 121 | nll2, nll_det2 = NLL(y_pred2, y_gt) 122 | nll_det = probs[:, 0] * nll_det1 + probs[:, 1] * nll_det2 123 | count = torch.sum(torch.ones(nll_det2.shape)) 124 | nll = torch.sum(nll_det) / count 125 | return nll, nll_det 126 | 127 | 128 | def multi_loss(y_pred1, y_pred2, probs): 129 | """This loss should encourage the network to propose two different predictions without a probibility being close to 0. 130 | 131 | Arguments: 132 | y_pred1 {[torch tensor]} -- [description] 133 | y_pred2 {[torcj tensor]} -- [description] 134 | probs {[torch tensor]} -- [description] 135 | """ 136 | 137 | # predicted trajectories should not match 138 | mse, _ = MSE(y_pred1, y_pred2) 139 | loss = 1 / mse * (-torch.log(probs[:, 0] * probs[:, 1])) 140 | loss = torch.sum(loss) / torch.sum(torch.ones(loss.shape)) 141 | return loss 142 | 143 | 144 | def logsumexp(inputs, dim=None, keepdim=False): 145 | """Helper function for log sum exp calculation 146 | 147 | Arguments: 148 | inputs {[type]} -- [description] 149 | 150 | Keyword Arguments: 151 | dim {[type]} -- [description] (default: {None}) 152 | keepdim {bool} -- [description] (default: {False}) 153 | 154 | Returns: 155 | [type] -- [description] 156 | """ 157 | 158 | if dim is None: 159 | inputs = inputs.view(-1) 160 | dim = 0 161 | s, _ = torch.max(inputs, dim=dim, keepdim=True) 162 | outputs = s + (inputs - s).exp().sum(dim=dim, keepdim=True).log() 163 | if not keepdim: 164 | outputs = outputs.squeeze(dim) 165 | return outputs 166 | 167 | 168 | def add_histograms(writer, net, global_step=0): 169 | """Add histograms of weights to tensorboard 170 | 171 | Arguments: 172 | writer {[tensorboard writer]} -- [Tensorboard writer] 173 | net {[torch network]} -- [Prediction network] 174 | 175 | Keyword Arguments: 176 | global_step {int} -- [Training epoch] (default: {0}) 177 | 178 | Returns: 179 | writer {[tensorboard writer]} -- [Tensorboard writer] 180 | """ 181 | writer.add_histogram("conv_3x1", net.conv_3x1.weight, global_step=global_step) 182 | writer.add_histogram( 183 | "dec_lstm_0", net.dec_lstm.all_weights[0][0], global_step=global_step 184 | ) 185 | writer.add_histogram( 186 | "dec_lstm_1", net.dec_lstm.all_weights[0][1], global_step=global_step 187 | ) 188 | writer.add_histogram( 189 | "dec_lstm_2", net.dec_lstm.all_weights[0][2], global_step=global_step 190 | ) 191 | writer.add_histogram( 192 | "dec_lstm_3", net.dec_lstm.all_weights[0][3], global_step=global_step 193 | ) 194 | writer.add_histogram("dyn_emb", net.dyn_emb.weight, global_step=global_step) 195 | writer.add_histogram( 196 | "enc_lstm_0", net.enc_lstm.all_weights[0][0], global_step=global_step 197 | ) 198 | writer.add_histogram( 199 | "enc_lstm_1", net.enc_lstm.all_weights[0][1], global_step=global_step 200 | ) 201 | writer.add_histogram( 202 | "enc_lstm_2", net.enc_lstm.all_weights[0][2], global_step=global_step 203 | ) 204 | writer.add_histogram( 205 | "enc_lstm_3", net.enc_lstm.all_weights[0][3], global_step=global_step 206 | ) 207 | writer.add_histogram("ip_emb", net.ip_emb.weight, global_step=global_step) 208 | writer.add_histogram("soc_conv", net.soc_conv.weight, global_step=global_step) 209 | 210 | return writer 211 | -------------------------------------------------------------------------------- /train/indy_net_evaluation.py: -------------------------------------------------------------------------------- 1 | """This script keeps all evaluation functions and as a main script executes 2 | evaluation with NLL and RMSE metrics on a defined model. 3 | Arguments: 4 | -- config 5 | -- model 6 | -- debug Set for debug mode (only one step training/validation/evaluation) 7 | """ 8 | 9 | # Standard imports 10 | from __future__ import print_function 11 | import os 12 | import json 13 | import sys 14 | 15 | # Third party imports 16 | import torch 17 | import argparse 18 | import tqdm 19 | import numpy as np 20 | from torch.utils.data import DataLoader 21 | 22 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 23 | sys.path.append(repo_path) 24 | 25 | # Custom imports 26 | from train.indy_net_dataset import IndyDataset 27 | from mix_net.mix_net.src.indy_net import IndyNet 28 | from train.neural_network import NLL, MSE 29 | from train.data_set_helper import load_indy_net_data 30 | from mix_net.mix_net.utils.cuda import cudanize 31 | 32 | 33 | def evaluate(dataloader, net, common_args, verbose=True): 34 | """Calculate evaluation metrics on a given dataset and net. 35 | 36 | Arguments: 37 | dataloader {[torch Dataloader]} -- [pytorch Dataloader] 38 | net {[torch nn.module]} -- [pytorch neural network] 39 | common_args {[dict]} -- [network arguments] 40 | 41 | Returns: 42 | [rmse, nll, prob_list, img_list] -- [RMSE, NLL, Probabilites, Scene images for visualization] 43 | """ 44 | 45 | # Initialize torch variables 46 | if common_args["use_cuda"]: 47 | lossVals_mse = torch.zeros(common_args["out_length"]).cuda() 48 | counts_mse = torch.zeros(common_args["out_length"]).cuda() 49 | lossVals_nll = torch.zeros(common_args["out_length"]).cuda() 50 | counts_nll = torch.zeros(common_args["out_length"]).cuda() 51 | else: 52 | lossVals_mse = torch.zeros(common_args["out_length"]) 53 | counts_mse = torch.zeros(common_args["out_length"]) 54 | lossVals_nll = torch.zeros(common_args["out_length"]) 55 | counts_nll = torch.zeros(common_args["out_length"]) 56 | 57 | for i, data in enumerate(tqdm.tqdm(dataloader)): 58 | # Unpack data 59 | smpl_id, hist, fut, left_bound, right_bound, ego = data 60 | 61 | # Initialize Variables 62 | if common_args["use_cuda"]: 63 | hsmpl_id, hist, fut, left_bound, right_bound, ego = cudanize( 64 | smpl_id, hist, fut, left_bound, right_bound, ego 65 | ) 66 | 67 | # Predict 68 | fut_pred = net(hist, left_bound, right_bound, ego=ego) 69 | 70 | _, l_nll = NLL(fut_pred, fut) 71 | _, l_mse = MSE(fut_pred, fut) 72 | 73 | if not torch.all(l_nll.eq(l_nll)): 74 | print("We might have some nans here. Please check!") 75 | continue 76 | 77 | # Get average over batch 78 | counts_mse += l_mse.shape[1] 79 | lossVals_mse += l_mse.sum(axis=1).detach() 80 | counts_nll += l_nll.shape[1] 81 | lossVals_nll += l_nll.sum(axis=1).detach() 82 | 83 | # Get standard deviation 84 | if i == 0: 85 | # initialize array 86 | ae_array = l_mse.detach().cpu().numpy() ** 0.5 87 | nll_array = l_nll.detach().cpu().numpy() 88 | else: 89 | ae_array = np.append(ae_array, l_mse.detach().cpu().numpy() ** 0.5, axis=1) 90 | nll_array = np.append(nll_array, l_nll.detach().cpu().numpy(), axis=1) 91 | 92 | if common_args["debug"]: 93 | break 94 | 95 | # Get average over batch 96 | nll = lossVals_nll / counts_nll 97 | nll = nll.cpu().detach().numpy() 98 | 99 | mae = np.mean(ae_array, axis=1) 100 | std_ae = np.std(ae_array, axis=1) 101 | std_nll = np.std(nll_array, axis=1) 102 | 103 | if common_args["use_cuda"]: 104 | rmse = torch.pow(lossVals_mse / counts_mse, 0.5) 105 | rmse = np.array(rmse.cpu()) 106 | else: 107 | rmse = np.array(torch.pow(lossVals_mse / counts_mse, 0.5)) 108 | 109 | if verbose: 110 | print("=" * 30) 111 | print("NLL 1s: {0:.2f} +/- {1:.2f}".format(nll[9], std_nll[9])) 112 | print("NLL 2s: {0:.2f} +/- {1:.2f}".format(nll[19], std_nll[19])) 113 | print("NLL 3s: {0:.2f} +/- {1:.2f}".format(nll[29], std_nll[29])) 114 | print("NLL 4s: {0:.2f} +/- {1:.2f}".format(nll[39], std_nll[39])) 115 | print("NLL 5s: {0:.2f} +/- {1:.2f}".format(nll[49], std_nll[49])) 116 | print("=" * 30) 117 | 118 | print("=" * 30) 119 | print("RMSE 1s: {0:.2f}".format(rmse[9])) 120 | print("RMSE 2s: {0:.2f}".format(rmse[19])) 121 | print("RMSE 3s: {0:.2f}".format(rmse[29])) 122 | print("RMSE 4s: {0:.2f}".format(rmse[39])) 123 | print("RMSE 5s: {0:.2f}".format(rmse[49])) 124 | print("=" * 30) 125 | 126 | print("=" * 30) 127 | print("MAE 1s: {0:.2f} +/- {1:.2f}".format(mae[9], std_ae[9])) 128 | print("MAE 2s: {0:.2f} +/- {1:.2f}".format(mae[19], std_ae[19])) 129 | print("MAE 3s: {0:.2f} +/- {1:.2f}".format(mae[29], std_ae[29])) 130 | print("MAE 4s: {0:.2f} +/- {1:.2f}".format(mae[39], std_ae[39])) 131 | print("MAE 5s: {0:.2f} +/- {1:.2f}".format(mae[49], std_ae[49])) 132 | print("=" * 30) 133 | 134 | metric_dict = { 135 | "rmse": rmse, 136 | "nll": nll, 137 | "nll_std": std_nll, 138 | "mae": mae, 139 | "mae_std": std_ae, 140 | } 141 | 142 | return metric_dict 143 | 144 | 145 | if __name__ == "__main__": 146 | # Parse arguments 147 | parser = argparse.ArgumentParser() 148 | parser.add_argument( 149 | "--config", 150 | type=str, 151 | default="mix_net/mix_net/data/inference_model/indy_net/default.json", 152 | ) 153 | parser.add_argument( 154 | "--model", 155 | type=str, 156 | default="mix_net/mix_net/data/inference_model/indy_net/lstm_mse_noise.tar", 157 | ) 158 | parser.add_argument("--debug", action="store_true", default=False) 159 | args = parser.parse_args() 160 | 161 | # Read config file 162 | with open(args.config, "r") as f: 163 | common_args = json.load(f) 164 | 165 | # Network Arguments 166 | common_args["use_cuda"] = bool(common_args["gpu"]) 167 | common_args["model_name"] = args.config.split("/")[1].split(".")[0] 168 | common_args["debug"] = args.debug 169 | common_args["online_layer"] = 0 170 | 171 | # Initialize network 172 | net = IndyNet(common_args) 173 | if common_args["use_cuda"]: 174 | net.load_model_weights(weights_path=args.model) 175 | net = net.cuda() 176 | else: 177 | net.load_model_weights(weights_path=args.model) 178 | 179 | sample_data = load_indy_net_data( 180 | os.path.join( 181 | os.path.dirname(os.path.dirname(__file__)), "data/indy_net_sample.txt" 182 | ), 183 | use_every_nth=1.0, 184 | ) 185 | # Initialize data loaders 186 | tsSet = IndyDataset( 187 | data=sample_data, 188 | cut_probability=common_args["cut_hist_probability"], 189 | min_len=common_args["hist_min_len"], 190 | ) 191 | tsDataloader = DataLoader( 192 | tsSet, 193 | batch_size=128, 194 | shuffle=True, 195 | num_workers=common_args["worker"], 196 | collate_fn=tsSet.collate_fn, 197 | ) 198 | 199 | # Call main evaulation function 200 | metric_dict = evaluate(tsDataloader, net, common_args) 201 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.6953977.svg)](https://doi.org/10.5281/zenodo.6953977) 2 | [![Linux](https://img.shields.io/badge/os-linux-blue.svg)](https://www.linux.org/) 3 | [![Docker](https://badgen.net/badge/icon/docker?icon=docker&label)](https://www.docker.com/) 4 | [![ROS2galactic](https://img.shields.io/badge/ros2-galactic-blue.svg)](https://docs.ros.org/en/galactic/index.html) 5 | [![Python 3.8](https://img.shields.io/badge/python-3.8-blue.svg)](https://www.python.org/downloads/release/python-380/) 6 | # MixNet 7 | 8 | ### Structured Deep Neural Motion Prediction for Autonomous Racing 9 | 10 | mix_net_model 11 | 12 | This repository provides a deep neural network approach to prediction that incorporates semantic knowledge to guarantee the quality of the output prediction. In this way, high robustness can be achieved without compromising prediction performance. The code is embedded into a ROS2-Node, which is configured in [mix_net](mix_net). The algorithm was developed by the team TUM Autonomous Motorsport for the [Indy Autonomous Challenge 2021](https://www.indyautonomouschallenge.com/) and the [Autonomous Challenge at CES 2022](https://www.indyautonomouschallenge.com/stream). 13 | 14 | ## Requirements 15 | 16 | - Linux Ubuntu >= 18.04 17 | - Docker >= 18.04 18 | - Python >= 3.8 19 | 20 | ## Installation 21 | 22 | Clone repository: 23 | ``` 24 | git clone https://github.com/TUMFTM/MixNet.git 25 | ``` 26 | 27 | Install requirements: 28 | ``` 29 | pip install -r requirements.txt 30 | ``` 31 | 32 | Install tex extensions, necessary to plot in with desired font: 33 | ``` 34 | sudo apt-get install texlive-latex-extra texlive-fonts-recommended dvipng cm-super 35 | ``` 36 | 37 | 38 | ## MixNet training and evaluation 39 | A MixNet training can be started with the following command: 40 | ``` 41 | python train/mix_net_trainer.py 42 | ``` 43 | All parameters used for the training and evaluation are specified in the MixNet-config-files ([train config](train/configs/mix_net/trainer_params_train.json), [test config](train/configs/mix_net/trainer_params_test.json)). Per default `10` exemplary scenarios are plotted to show the prediction behavior. The number of plotted scenarios can be changed by the flag `--max_plots`. Add `--plt_vel` to evaluate the velocity profile additionally. To save the plots instead of showing them, use the flag `--save_figs`, which stores the plots to [train/figs](train/figs). To run an evaluation of a trained net, add the argument `--test` and run the script again. 44 | 45 | ## Data and Evaluation 46 | * A sample of the raw data ([data/mix_net_sample.pkl](data/mix_net_sample.pkl)) and the processed data (torch.Dataloader, [data/mix_net_dataloaders/sample](data/mix_net_dataloaders/sample)) are included in the repository. Per default, the samples are used in the MixNet trainer. 47 | 48 | * The full dataset, either raw data or the processed in torch.Dataloader-format can be downloaded [here](https://doi.org/10.5281/zenodo.6954020). To use the raw data, extract them and change the key `path` in the section `data` in the [mix_net-config-file](train/configs/mix_net/trainer_params_test.json). Note that every parent directory containing .pkl-files can be specified. To use the dataloaders, extract them, set `"from_saved_dataloader": true` and specify the `dataloader_path` in the section `data` in the [mix_net-config-file](train/configs/mix_net/trainer_params_test.json). 49 | 50 | * The evaluation data is also available to [download](https://doi.org/10.5281/zenodo.6954020). Copy the folder `evaluation_data` into this repository folder `data`. The details how to evaluate the data are given in the related [readme](data/evaluation_data/README.md). 51 | 52 | ## ROS2-Node 53 | It is recommended to run the ROS2-node of the module in a Docker container. To build the related image, execute: 54 | ``` 55 | docker build --pull --tag : . 56 | # e.g. docker build --pull --tag mix_net:0.0.1 . 57 | ``` 58 | To run the container and launch the ROS2-node, run: 59 | ``` 60 | docker run : ros2 launch mix_net mix_net.launch.py 61 | # e.g. docker run mix_net:0.0.1 ros2 launch mix_net mix_net.launch.py 62 | ``` 63 | Add additional parameters to the ros2 launch command if desired, see section `Parameter and Files` below. For further details about Docker and ROS2, we refer to the official documentations. 64 | 65 | 66 | ## Parameter and Files 67 | ### Directory: `mix_net` 68 | The directory [mix_net](mix_net) contains the ROS2-node to apply the MixNet in a full software stack. 69 | 70 | | Files | Description | 71 | | ------------------ | ----------------------- | 72 | mix_net/mix_net/mix_net_node.py | ROS2 main file to apply the MixNet 73 | mix_net/launch/mix_net.launch.py | ROS2 launch file with parameter definition 74 | 75 | The [launch description](mix_net/launch/mix_net.launch.py) contains the following parameters: 76 | 77 | | Parameter | Type | Default | Description 78 | | ------------- | ------------- | ------ | ----- | 79 | frequency | dynamic_typing | 20.0 | Cycle frequency in Hz of the node (int, float)| 80 | track | string | IMS | Name of used race track, currently only IMS supported | 81 | use_sim_time | boolean | False | Flag to use sim time instead of system time to replay ROS-bags 82 | use_cuda | boolean | False | If true the GPU and cuda is used 83 | 84 | 85 | ### Directory: `tools` 86 | The directory [tools](tools) contains the script to visualize logged data of the applied ROS2-Node. To visualize logged data of the mix_net-node run: 87 | ``` 88 | python tools/visualize_logfiles.py 89 | ``` 90 | 91 | ### Directory: `train` 92 | The directory [train](train) contains the scripts to train the MixNet and the benchmark model (IndyNet). 93 | 94 | | File | Description | 95 | | ------------------ | ----------------------- | 96 | mix_net_trainer.py | Training script of the MixNet 97 | mix_net_dataset.py | Dataset class, used in mix_net_trainer.py 98 | indy_net_trainer.py | Training script of the benchmark model (IndyNet) 99 | indy_net_bayes.py | Bayes optimizer of the benchmark model 100 | indy_net_evaluation.py | Evaluation script of the benchmark model 101 | 102 | ## Module configuration 103 | The configuration of the prediction module is set in [main_params.ini](mix_net/mix_net/config/main_params.ini). There are several options to specify the prediction behavior when applying the ROS2-node. A parameter description is given in the [README](mix_net/mix_net/config/README.md). 104 | 105 | 106 | ## Qualitative Example 107 | Below is an exemplary visualization of the prediction on a scenario on the Indianapolis Motor Speedway. The input features of history, left and right boundary as well as predicted and ground truth trajectory are shown. MixNet combines the best out of two worlds: the superposition of base curves and the comprehensive, learned scenario understanding. 108 | smoothness 109 | 110 | ## Inference time 111 | The average computation times for predicting four vehicles on a single core of an Intel i7-4720HQ 2.6 GHz CPU for MixNet is 9 ms. 112 | 113 | ## References 114 | P. Karle, F. Török, M. Geisslinger and M. Lienkamp, "MixNet: Physics Constrained Deep Neural Motion Prediction for Autonomous Racing," in *IEEE Access*, vol. 11, pp. 85914-85926, 2023, doi: 10.1109/ACCESS.2023.3303841. 115 | 116 | BibTex: 117 | ``` 118 | @ARTICLE{Karle2023_2, 119 | author={Karle, Phillip and Török, Ferenc and Geisslinger, Maximilian and Lienkamp, Markus}, 120 | journal={IEEE Access}, 121 | title={MixNet: Physics Constrained Deep Neural Motion Prediction for Autonomous Racing}, 122 | year={2023}, 123 | volume={11}, 124 | number={}, 125 | pages={85914-85926}, 126 | doi={10.1109/ACCESS.2023.3303841} 127 | } 128 | ``` 129 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/line_helper.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def cross2D(vect1, vect2): 5 | """Calculates the cross product between 2 2D vectors. 6 | IMPORTANT: The order of the vectors matter!!! 7 | 8 | args: 9 | vect1: (np.array, shape=(2,) or shape=(N, 2)), the thumb in the right hand rule 10 | vect2: (np.array, shape=(2,) or shape=(N, 2)), the pointing finger in the right hand rule 11 | 12 | returns: 13 | The scalar value of the cross product. 14 | """ 15 | if len(vect1.shape) == 1: 16 | return vect1[0] * vect2[1] - vect1[1] * vect2[0] 17 | else: 18 | return vect1[:, 0] * vect2[:, 1] - vect1[:, 1] * vect2[:, 0] 19 | 20 | 21 | def is_static_traj(obs, threshold=1.0): 22 | """Checks whether a trajectory can be considered as static. 23 | 24 | args: 25 | obs: [np.array with shape=(N, 2)]: the trajectoriy 26 | threshold: [float, default=1.0]: If the max difference in both x and y 27 | directions is smaller than this threshold, it is considered static. 28 | 29 | returns: 30 | true, if the obstacle should be considered static. 31 | """ 32 | 33 | x_min = np.min(obs[:, 0]) 34 | x_max = np.max(obs[:, 0]) 35 | 36 | y_min = np.min(obs[:, 1]) 37 | y_max = np.max(obs[:, 1]) 38 | 39 | return (x_max - x_min < threshold) and (y_max - y_min < threshold) 40 | 41 | 42 | class LineHelper: 43 | """Class for handling a line (boundaries, center line, race line).""" 44 | 45 | def __init__(self, line): 46 | """Initializes a LineHelper object.""" 47 | 48 | self.line = line 49 | self.tangents = self._get_tangents() 50 | self.curvs = self._get_curvatures() 51 | self.arc_lens = self._get_arc_lens() 52 | 53 | self.max_arc_len = self.arc_lens[-1] 54 | self.max_ind = line.shape[0] 55 | 56 | def _get_tangents(self): 57 | """Calculates the normalized tangent vectors for the line. The tangentials 58 | are calculated from the 2 neighbouring points. 59 | """ 60 | 61 | tangs = np.zeros_like(self.line) 62 | 63 | # very first point: 64 | tangs[0, :] = self.line[1, :] - self.line[-1, :] 65 | 66 | # very last point: 67 | tangs[-1, :] = self.line[0, :] - self.line[-2, :] 68 | 69 | # every other point: 70 | tangs[1:-1, :] = self.line[2:, :] - self.line[:-2, :] 71 | 72 | # normalizing: 73 | tangs /= np.linalg.norm(tangs, axis=1)[ 74 | :, None 75 | ] # None creates a new axis for the division. 76 | 77 | return tangs 78 | 79 | def _get_curvatures(self): 80 | """Calculates the curvatures for the line.""" 81 | 82 | kappa = np.zeros(self.line.shape[0]) 83 | 84 | # point 0: 85 | d1 = self.line[0, :] - self.line[-1, :] 86 | ds1 = np.linalg.norm(d1) + 1e-12 87 | d1 /= ds1 88 | 89 | d2 = self.line[1, :] - self.line[0, :] 90 | ds2 = np.linalg.norm(d2) + 1e-12 91 | d2 /= ds2 92 | 93 | # the np.sign() part sets the sign of the curvature value. 94 | kappa[0] = np.linalg.norm((d2 - d1) / ds1) * np.sign(cross2D(d1, d2)) 95 | 96 | # points [1: -1]: 97 | d1 = self.line[1:-1, :] - self.line[:-2, :] 98 | ds1 = np.linalg.norm(d1, axis=1) + 1e-12 99 | d1 /= ds1[:, None] 100 | 101 | d2 = self.line[2:, :] - self.line[1:-1, :] 102 | ds2 = np.linalg.norm(d2, axis=1) + 1e-12 103 | d2 /= ds2[:, None] 104 | 105 | # the np.sign() part sets the sign of the curvature value. 106 | kappa[1:-1] = np.linalg.norm((d2 - d1) / ds1[:, None], axis=1) * np.sign( 107 | cross2D(d1, d2) 108 | ) 109 | 110 | # point -1: 111 | d1 = self.line[-1, :] - self.line[-2, :] 112 | ds1 = np.linalg.norm(d1) + 1e-12 113 | d1 /= ds1 114 | 115 | d2 = self.line[0, :] - self.line[-1, :] 116 | ds2 = np.linalg.norm(d2) + 1e-12 117 | d2 /= ds2 118 | 119 | # the np.sign() part sets the sign of the curvature value. 120 | kappa[-1] = np.linalg.norm((d2 - d1) / ds1) * np.sign(cross2D(d1, d2)) 121 | 122 | return kappa 123 | 124 | def _get_arc_lens(self): 125 | """Calculates the arclen for every point in the line. 126 | 127 | If line has shape=(N, 2), arc_lens will have shape=((N+1),): 128 | The first and last arc distances both correspond to the very first point. The first 129 | value is 0 and the last one is the length of the line. 130 | """ 131 | 132 | arc_lens = np.zeros((self.line.shape[0] + 1)) 133 | dists = np.linalg.norm((self.line[1:, :] - self.line[:-1, :]), axis=1) 134 | arc_lens[1:-1] = np.cumsum(dists) 135 | 136 | arc_lens[-1] = arc_lens[-2] + np.linalg.norm(self.line[0, :] - self.line[-1, :]) 137 | 138 | return arc_lens 139 | 140 | def get_nearest_ind(self, point, hint=None): 141 | """Gets the nearest index of the line to the point given. 142 | If a hint index is given to search around, iterative search is carried out. 143 | """ 144 | 145 | if hint is not None: 146 | return self._get_nearest_ind_iterative(point, hint=hint) 147 | else: 148 | return self._get_nearest_ind_naive(point) 149 | 150 | def _get_nearest_ind_iterative(self, point, hint=0): 151 | """Searches for the index of the nearest point of the 152 | left track boundary to the given point. It is an iteratice 153 | search that starts from the given hint index. The distance 154 | to the given point is calculated iteratively for the next point 155 | of the boundary until the distance sinks, hence the nearest point 156 | is found. 157 | 158 | !!! WARNING !!! 159 | The hint should be "near" the real nearest index, or else 160 | algorithm converges to a wrong value. (By near we meen, it should be on 161 | the correct half of the track at least.) Else the assumption, that the 162 | distance decreases monotonously towards the nearest point does not 163 | necessarily hold. 164 | """ 165 | 166 | ind = hint 167 | # just making sure, that ind is in the range [0, self._max_ind) 168 | while ind < 0: 169 | ind += self.max_ind 170 | while ind >= self.max_ind: 171 | ind -= self.max_ind 172 | 173 | ind_prev = ind - 1 if ind > 0 else self.max_ind - 1 174 | 175 | dist = np.linalg.norm(self.line[ind, :] - point) 176 | dist_prev = np.linalg.norm(self.line[ind_prev, :] - point) 177 | 178 | add_on = 1 if dist < dist_prev else -1 179 | 180 | counter = 0 181 | smallest = dist 182 | smallest_ind = ind 183 | 184 | # there are some small local minimas around the global minima, this is 185 | # why the 0.01 threshold is needed. 186 | while (smallest + 0.01) > dist: 187 | ind += add_on 188 | if ind >= self.max_ind: 189 | ind -= self.max_ind 190 | if ind < 0: 191 | ind += self.max_ind 192 | 193 | dist = np.linalg.norm(self.line[ind, :] - point) 194 | 195 | if smallest > dist: 196 | smallest = dist 197 | smallest_ind = ind 198 | 199 | if counter > self.max_ind: 200 | print("Did not find solution for closest index in a whole round.") 201 | break 202 | 203 | return smallest_ind 204 | 205 | def _get_nearest_ind_naive(self, point): 206 | """Finds the nearest point of the left track boundary to 207 | the given point by the naive method: calculates the distance 208 | to all of the points and choses the index with the smallest distance. 209 | """ 210 | 211 | dists = np.linalg.norm((self.line - point), axis=1) 212 | 213 | return np.argmin(dists) 214 | -------------------------------------------------------------------------------- /mix_net/mix_net/src/mix_net.py: -------------------------------------------------------------------------------- 1 | import json 2 | import torch 3 | import torch.nn as nn 4 | from collections import OrderedDict 5 | 6 | 7 | class MixNet(nn.Module): 8 | """Neural Network to predict the future trajectory of a vehicle based on its history. 9 | It predicts mixing weights for mixing the boundaries, the centerline and the raceline. 10 | Also, it predicts section wise constant accelerations and an initial velocity to 11 | compute the velocity profile from. 12 | """ 13 | 14 | def __init__(self, params): 15 | """Initializes a MixNet object.""" 16 | super(MixNet, self).__init__() 17 | 18 | self._params = params 19 | 20 | # Input embedding layer: 21 | self._ip_emb = torch.nn.Linear(2, params["encoder"]["in_size"]) 22 | 23 | # History encoder LSTM: 24 | self._enc_hist = torch.nn.LSTM( 25 | params["encoder"]["in_size"], 26 | params["encoder"]["hidden_size"], 27 | 1, 28 | batch_first=True, 29 | ) 30 | 31 | # Boundary encoders: 32 | self._enc_left_bound = torch.nn.LSTM( 33 | params["encoder"]["in_size"], 34 | params["encoder"]["hidden_size"], 35 | 1, 36 | batch_first=True, 37 | ) 38 | 39 | self._enc_right_bound = torch.nn.LSTM( 40 | params["encoder"]["in_size"], 41 | params["encoder"]["hidden_size"], 42 | 1, 43 | batch_first=True, 44 | ) 45 | 46 | # Linear stack that outputs the path mixture ratios: 47 | self._mix_out_layers = self._get_linear_stack( 48 | in_size=params["encoder"]["hidden_size"] * 3, 49 | hidden_sizes=params["mixer_linear_stack"]["hidden_sizes"], 50 | out_size=params["mixer_linear_stack"]["out_size"], 51 | name="mix", 52 | ) 53 | 54 | # Linear stack for outputting the initial velocity: 55 | self._vel_out_layers = self._get_linear_stack( 56 | in_size=params["encoder"]["hidden_size"], 57 | hidden_sizes=params["init_vel_linear_stack"]["hidden_sizes"], 58 | out_size=params["init_vel_linear_stack"]["out_size"], 59 | name="vel", 60 | ) 61 | 62 | # dynamic embedder between the encoder and the decoder: 63 | self._dyn_embedder = nn.Linear( 64 | params["encoder"]["hidden_size"] * 3, params["acc_decoder"]["in_size"] 65 | ) 66 | 67 | # acceleration decoder: 68 | self._acc_decoder = nn.LSTM( 69 | params["acc_decoder"]["in_size"], 70 | params["acc_decoder"]["hidden_size"], 71 | 1, 72 | batch_first=True, 73 | ) 74 | 75 | # output linear layer of the acceleration decoder: 76 | self._acc_out_layer = nn.Linear(params["acc_decoder"]["hidden_size"], 1) 77 | 78 | # migrating the model parameters to the chosen device: 79 | if params["use_cuda"] and torch.cuda.is_available(): 80 | self.device = torch.device("cuda:0") 81 | print("Using CUDA as device for MixNet") 82 | else: 83 | self.device = torch.device("cpu") 84 | print("Using CPU as device for MixNet") 85 | 86 | self.to(self.device) 87 | 88 | def forward(self, hist, left_bound, right_bound): 89 | """Implements the forward pass of the model. 90 | 91 | args: 92 | hist: [tensor with shape=(batch_size, hist_len, 2)] 93 | left_bound: [tensor with shape=(batch_size, boundary_len, 2)] 94 | right_bound: [tensor with shape=(batch_size, boundary_len, 2)] 95 | 96 | returns: 97 | mix_out: [tensor with shape=(batch_size, out_size)]: The path mixing ratios in the order: 98 | left_ratio, right_ratio, center_ratio, race_ratio 99 | vel_out: [tensor with shape=(batch_size, 1)]: The initial velocity of the velocity profile 100 | acc_out: [tensor with shape=(batch_size, num_acc_sections)]: The accelerations in the sections 101 | """ 102 | 103 | # encoders: 104 | _, (hist_h, _) = self._enc_hist(self._ip_emb(hist.to(self.device))) 105 | _, (left_h, _) = self._enc_left_bound(self._ip_emb(left_bound.to(self.device))) 106 | _, (right_h, _) = self._enc_right_bound( 107 | self._ip_emb(right_bound.to(self.device)) 108 | ) 109 | 110 | # concatenate and squeeze encodings: 111 | enc = torch.squeeze(torch.cat((hist_h, left_h, right_h), 2), dim=0) 112 | 113 | # path mixture through softmax: 114 | mix_out = torch.softmax(self._mix_out_layers(enc), dim=1) 115 | 116 | # initial velocity: 117 | vel_out = self._vel_out_layers(torch.squeeze(hist_h, dim=0)) 118 | vel_out = torch.sigmoid(vel_out) 119 | vel_out = vel_out * self._params["init_vel_linear_stack"]["max_vel"] 120 | 121 | # acceleration decoding: 122 | dec_input = torch.relu(self._dyn_embedder(enc)).unsqueeze(dim=1) 123 | dec_input = dec_input.repeat( 124 | 1, self._params["acc_decoder"]["num_acc_sections"], 1 125 | ) 126 | acc_out, _ = self._acc_decoder(dec_input) 127 | acc_out = torch.squeeze(self._acc_out_layer(torch.relu(acc_out)), dim=2) 128 | acc_out = torch.tanh(acc_out) * self._params["acc_decoder"]["max_acc"] 129 | 130 | return mix_out, vel_out, acc_out 131 | 132 | def load_model_weights(self, weights_path): 133 | self.load_state_dict(torch.load(weights_path, map_location=self.device)) 134 | print("Successfully loaded model weights from {}".format(weights_path)) 135 | 136 | def _get_linear_stack( 137 | self, in_size: int, hidden_sizes: list, out_size: int, name: str 138 | ): 139 | """Creates a stack of linear layers with the given sizes and with relu activation.""" 140 | 141 | layer_sizes = [] 142 | layer_sizes.append(in_size) # The input size of the linear stack 143 | layer_sizes += hidden_sizes # The hidden layer sizes specified in params 144 | layer_sizes.append(out_size) # The output size specified in the params 145 | 146 | layer_list = [] 147 | for i in range(len(layer_sizes) - 1): 148 | layer_name = name + "linear" + str(i + 1) 149 | act_name = name + "relu" + str(i + 1) 150 | layer_list.append( 151 | (layer_name, nn.Linear(layer_sizes[i], layer_sizes[i + 1])) 152 | ) 153 | layer_list.append((act_name, nn.ReLU())) 154 | 155 | # removing the last ReLU layer: 156 | layer_list = layer_list[:-1] 157 | 158 | return nn.Sequential(OrderedDict(layer_list)) 159 | 160 | def get_params(self): 161 | """Accessor for the params of the network.""" 162 | return self._params 163 | 164 | 165 | if __name__ == "__main__": 166 | param_file = "mod_prediction/utils/mix_net/params/net_params.json" 167 | with open(param_file, "r") as fp: 168 | params = json.load(fp) 169 | 170 | batch_size = 32 171 | hist_len = 30 172 | bound_len = 50 173 | 174 | # random inputs: 175 | hist = torch.rand((batch_size, hist_len, 2)) 176 | left_bound = torch.rand((batch_size, bound_len, 2)) 177 | right_bound = torch.rand((batch_size, bound_len, 2)) 178 | 179 | net = MixNet(params) 180 | 181 | mix_out, vel_out, acc_out = net(hist, left_bound, right_bound) 182 | 183 | # printing the output shapes as a sanity check: 184 | print( 185 | "output shape: {}, should be: {}".format( 186 | mix_out.shape, (batch_size, params["mixer_linear_stack"]["out_size"]) 187 | ) 188 | ) 189 | 190 | print("output shape: {}, should be: {}".format(vel_out.shape, (batch_size, 1))) 191 | 192 | print( 193 | "output shape: {}, should be: {}".format( 194 | acc_out.shape, (batch_size, params["acc_decoder"]["num_acc_sections"]) 195 | ) 196 | ) 197 | 198 | print(vel_out) 199 | -------------------------------------------------------------------------------- /train/data_set_helper.py: -------------------------------------------------------------------------------- 1 | """helper functions to create dataset.""" 2 | import os 3 | import pickle 4 | import numpy as np 5 | from sklearn.model_selection import train_test_split 6 | 7 | 8 | def load_indy_net_data(path, use_every_nth=1.0): 9 | """Loads all the data from the provided path. 10 | 11 | !!! 12 | It loads everything under a given directory except for files which's name 13 | contains "info" 14 | !!! 15 | 16 | args: 17 | path: (str), A single datafile or a directory under which the data can be found. 18 | If its a dir, every subdir will be searched through for datafiles. If its a dir, 19 | it can only contain the data files or an "info.txt" file which is going to be ignored. 20 | use_every_nth: (float, default=1.0), only every n^th datapoint is going to be used, if provided 21 | and is bigger then 1.0. 22 | 23 | returns: 24 | data: (dict), all of the data with the keys: 25 | "id": (list) the IDs 26 | "hist": (np.array of shape=(num_data, hist_len, 2)) the history trajectories. 27 | "fut": (np.array of shape=(num_data, fut_len, 2)) the groundtruth future trajectories. 28 | "left_bd": (np.array of shape=(num_data, bound_len, 2)) left track boundary snippet. 29 | "right_bd": (np.array of shape=(num_data, bound_len, 2)) right track boundary snippet. 30 | """ 31 | data = {} 32 | 33 | if not os.path.isabs(path): 34 | os.path.join(os.path.dirname(os.path.dirname(__file__)), path) 35 | 36 | if os.path.isdir(path): 37 | for root, _, files in os.walk(path): 38 | for file in files: 39 | ff = os.path.join(root, file) 40 | 41 | if "info" in file: 42 | continue 43 | 44 | with open(ff, "rb") as fp: 45 | temp_data = pickle.load(fp) 46 | for key, items in temp_data.items(): 47 | if key not in data.keys(): 48 | data[key] = [] 49 | data[key] += items 50 | 51 | else: # single file input 52 | with open(path, "rb") as fp: 53 | data = pickle.load(fp) 54 | 55 | data["id"] = list(range(len(data["hist"]))) 56 | 57 | # numpify the data except for "id": 58 | for key in ["hist", "fut", "left_bd", "right_bd"]: 59 | data[key] = np.array(data[key]) 60 | 61 | # taking only every n^th datapoint: 62 | if use_every_nth > 1.0: 63 | N = int(len(data["id"]) / use_every_nth) 64 | 65 | # using random generator with seed for reproducible results: 66 | rng = np.random.default_rng(0) 67 | idxs = rng.choice(data["id"], size=(N,), replace=False) 68 | 69 | for key in ["hist", "fut", "left_bd", "right_bd"]: 70 | data[key] = data[key][idxs] 71 | 72 | data["id"] = list(range(data["hist"].shape[0])) 73 | 74 | return data 75 | 76 | 77 | def split_indy_net_data(data, train_size=0.8, val_size=0.1, test_size=0.1): 78 | """Splits the given data into train, validation and test sets. 79 | 80 | args: 81 | data: (dict), all of the data with the keys: 82 | "id": (list) the IDs 83 | "hist": (np.array of shape=(num_data, hist_len, 2)) the history trajectories. 84 | "fut": (np.array of shape=(num_data, fut_len, 2)) the groundtruth future trajectories. 85 | "left_bd": (np.array of shape=(num_data, bound_len, 2)) left track boundary snippet. 86 | "right_bd": (np.array of shape=(num_data, bound_len, 2)) right track boundary snippet. 87 | train_size: (float ]0.0, 1.0[), the ratio of the train data split 88 | val_size: (float ]0.0, 1.0[), the ratio of the validation data split 89 | test_size: (float ]0.0, 1.0[), the ratio of the test data split 90 | 91 | returns: 92 | train_data: (dict) the splitted training set with the same keys as data 93 | val_data: (dict) the splitted training set with the same keys as data 94 | test_data: (dict) the splitted training set with the same keys as data 95 | """ 96 | 97 | # train - (val + test) split: 98 | ( 99 | train_id, 100 | val_id, 101 | train_hist, 102 | val_hist, 103 | train_fut, 104 | val_fut, 105 | train_left_bd, 106 | val_left_bd, 107 | train_right_bd, 108 | val_right_bd, 109 | ) = train_test_split( 110 | data["id"], 111 | data["hist"], 112 | data["fut"], 113 | data["left_bd"], 114 | data["right_bd"], 115 | train_size=train_size, 116 | ) 117 | 118 | # val - test split: 119 | ( 120 | val_id, 121 | test_id, 122 | val_hist, 123 | test_hist, 124 | val_fut, 125 | test_fut, 126 | val_left_bd, 127 | test_left_bd, 128 | val_right_bd, 129 | test_right_bd, 130 | ) = train_test_split( 131 | val_id, 132 | val_hist, 133 | val_fut, 134 | val_left_bd, 135 | val_right_bd, 136 | train_size=(val_size / (val_size + test_size)), 137 | ) 138 | 139 | # constructing the dicts: 140 | train_data = { 141 | "id": train_id, 142 | "hist": train_hist, 143 | "fut": train_fut, 144 | "left_bd": train_left_bd, 145 | "right_bd": train_right_bd, 146 | } 147 | 148 | val_data = { 149 | "id": val_id, 150 | "hist": val_hist, 151 | "fut": val_fut, 152 | "left_bd": val_left_bd, 153 | "right_bd": val_right_bd, 154 | } 155 | 156 | test_data = { 157 | "id": test_id, 158 | "hist": test_hist, 159 | "fut": test_fut, 160 | "left_bd": test_left_bd, 161 | "right_bd": test_right_bd, 162 | } 163 | 164 | return train_data, val_data, test_data 165 | 166 | 167 | def load_mix_net_data(path, use_every_nth=1.0): 168 | """Loads all the data from the provided path. 169 | 170 | args: 171 | path: (str), A single datafile or a directory under which the data can be found. 172 | If its a dir, every subdir will be searched through for datafiles. If its a dir, 173 | it can only contain the data files or an "info.txt" file which is going to be ignored. 174 | use_every_nth: (float, default=1.0), only every n^th datapoint is going to be used, if provided 175 | and is bigger then 1.0. 176 | 177 | 178 | returns: 179 | data: (dict), all of the data with the keys: 180 | "hist": (list of 2D lists) the history trajectories. 181 | "fut": (list of 2D lists) the groundtruth future trajectories. 182 | "fut_inds": (list of lists) the indices of the nearest centerline points 183 | corresponding to the ground truth prediction. 184 | "left_bd": (list of 2D lists) left track boundary snippet. 185 | "right_bd": (list of 2D lists) right track boundary snippet. 186 | """ 187 | data = {} 188 | 189 | if not os.path.isabs(path): 190 | os.path.join(os.path.dirname(os.path.dirname(__file__)), path) 191 | 192 | if os.path.isdir(path): 193 | for root, _, files in os.walk(path): 194 | for file in files: 195 | ff = os.path.join(root, file) 196 | 197 | if "info" in file: 198 | continue 199 | 200 | with open(ff, "rb") as fp: 201 | temp_data = pickle.load(fp) 202 | for key, items in temp_data.items(): 203 | if key not in data.keys(): 204 | data[key] = [] 205 | data[key] += items 206 | 207 | else: # single file input 208 | with open(path, "rb") as fp: 209 | data = pickle.load(fp) 210 | 211 | if "fut_inds" not in data: 212 | raise IndexError("invalid data set for MixNet-Trainer") 213 | 214 | # numpify the data: 215 | for key, value in data.items(): 216 | if key == "fut_inds": 217 | data[key] = np.array(value) 218 | else: 219 | data[key] = np.array(value, dtype=(np.float32)) 220 | 221 | # taking only every n^th datapoint: 222 | if use_every_nth > 1.0: 223 | N = int(data["hist"].shape[0] / use_every_nth) 224 | 225 | # using random generator with seed for reproducible results: 226 | rng = np.random.default_rng(0) 227 | idxs = rng.choice(list(range(data["hist"].shape[0])), size=(N,), replace=False) 228 | 229 | for key in ["hist", "fut", "fut_inds", "left_bd", "right_bd"]: 230 | data[key] = data[key][idxs] 231 | 232 | data["id"] = list(range(data["hist"].shape[0])) 233 | 234 | return data 235 | -------------------------------------------------------------------------------- /mix_net/mix_net/src/indy_net.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | 4 | 5 | def outputActivation(x): 6 | """Custom activation for output layer (Graves, 2015) 7 | 8 | Arguments: 9 | x {[type]} -- [description] 10 | 11 | Returns: 12 | [type] -- [description] 13 | """ 14 | muX = x[:, :, 0:1] 15 | muY = x[:, :, 1:2] 16 | sigX = x[:, :, 2:3] 17 | sigY = x[:, :, 3:4] 18 | rho = x[:, :, 4:5] 19 | sigX = torch.exp(sigX) 20 | sigY = torch.exp(sigY) 21 | rho = torch.tanh(rho) 22 | out = torch.cat([muX, muY, sigX, sigY, rho], dim=2) 23 | return out 24 | 25 | 26 | class IndyNet(nn.Module): 27 | # Initialization 28 | def __init__(self, args): 29 | super(IndyNet, self).__init__() 30 | 31 | # Unpack arguments 32 | self.args = args 33 | 34 | # Use gpu flag 35 | self.use_cuda = args["use_cuda"] 36 | 37 | # Sizes of network layers 38 | self.encoder_size = args["encoder_size"] 39 | self.decoder_size = args["decoder_size"] 40 | self.out_length = args["out_length"] 41 | self.dyn_embedding_size = args["dyn_embedding_size"] 42 | self.input_embedding_size = args["input_embedding_size"] 43 | self.enc_dec_layer = args["enc_dec_layer"] 44 | self.ego_awareness = args["ego_awareness"] 45 | 46 | # Define network weights 47 | 48 | # Input embedding layer 49 | self.ip_emb = torch.nn.Linear(2, self.input_embedding_size) 50 | 51 | if "lstm" in self.enc_dec_layer: 52 | rnn_encoding = torch.nn.LSTM 53 | elif "gru" in self.enc_dec_layer: 54 | rnn_encoding = torch.nn.GRU 55 | 56 | # Encoder LSTM 57 | # Hist 58 | self.enc_lstm_hist = rnn_encoding( 59 | self.input_embedding_size, self.encoder_size, 1 60 | ) 61 | 62 | # Boundaries 63 | self.enc_lstm_left = rnn_encoding( 64 | self.input_embedding_size, self.encoder_size, 1 65 | ) 66 | self.enc_lstm_right = rnn_encoding( 67 | self.input_embedding_size, self.encoder_size, 1 68 | ) 69 | 70 | if self.ego_awareness: 71 | # Ego 72 | self.enc_lstm_ego = rnn_encoding( 73 | self.input_embedding_size, self.encoder_size, 1 74 | ) 75 | no_encoders = 4 76 | else: 77 | no_encoders = 3 78 | 79 | # decoder: 80 | decoder_types = ["original", "iterative_hidden", "iterative"] 81 | assert ( 82 | args["decoder_type"] in decoder_types 83 | ), "expected decoder_type to be one of {}. Received: {}".format( 84 | decoder_types, args["decoder_type"] 85 | ) 86 | 87 | if args["decoder_type"] == "original": 88 | # Encoder hidden state embedder: 89 | self.dyn_emb = torch.nn.Linear(self.encoder_size, self.dyn_embedding_size) 90 | 91 | # Decoder LSTM 92 | self.dec_lstm = rnn_encoding( 93 | no_encoders * self.dyn_embedding_size, self.decoder_size 94 | ) 95 | elif args["decoder_type"] == "iterative_hidden": 96 | # Embedder to create the cell and hidden state of the decoder from the ones of the encoder: 97 | self._c_embedder = torch.nn.Linear(3 * self.encoder_size, self.decoder_size) 98 | self._h_embedder = torch.nn.Linear(3 * self.encoder_size, self.decoder_size) 99 | 100 | # Decoder LSTM: 101 | self.dec_lstm = rnn_encoding(2, self.decoder_size) 102 | else: 103 | pass 104 | 105 | # Output layers: 106 | self.op = torch.nn.Linear(self.decoder_size, 5) 107 | 108 | # Activations: 109 | self.leaky_relu = torch.nn.LeakyReLU(0.1) 110 | self.relu = torch.nn.ReLU() 111 | self.softmax = torch.nn.Softmax(dim=1) 112 | 113 | # migrating the model parameters to the chosen device: 114 | if args["use_cuda"] and torch.cuda.is_available(): 115 | self.device = torch.device("cuda:0") 116 | print("Using CUDA as device for IndyNet") 117 | else: 118 | self.device = torch.device("cpu") 119 | print("Using CPU as device for IndyNet") 120 | 121 | self.to(self.device) 122 | 123 | # Forward Pass 124 | def forward(self, hist, left_bound, right_bound, ego=None): 125 | if self.args["decoder_type"] == "original": 126 | # Forward pass hist: 127 | hist_enc = self.get_hidden_state( 128 | self.enc_lstm_hist(self.leaky_relu(self.ip_emb(hist))), 129 | self.enc_dec_layer, 130 | ) 131 | # Forward pass left_bound: 132 | left_enc = self.get_hidden_state( 133 | self.enc_lstm_left(self.leaky_relu(self.ip_emb(left_bound))), 134 | self.enc_dec_layer, 135 | ) 136 | 137 | # Forward pass left_bound: 138 | right_enc = self.get_hidden_state( 139 | self.enc_lstm_right(self.leaky_relu(self.ip_emb(right_bound))), 140 | self.enc_dec_layer, 141 | ) 142 | 143 | if self.ego_awareness: 144 | # Forward pass ego: 145 | ego_enc = self.get_hidden_state( 146 | self.enc_lstm_ego(self.leaky_relu(self.ip_emb(ego))), 147 | self.enc_dec_layer, 148 | ) 149 | 150 | hist_enc = self.leaky_relu( 151 | self.dyn_emb(hist_enc.view(hist_enc.shape[1], hist_enc.shape[2])) 152 | ) 153 | left_enc = self.leaky_relu( 154 | self.dyn_emb(left_enc.view(left_enc.shape[1], left_enc.shape[2])) 155 | ) 156 | right_enc = self.leaky_relu( 157 | self.dyn_emb(right_enc.view(right_enc.shape[1], right_enc.shape[2])) 158 | ) 159 | 160 | if self.ego_awareness: 161 | ego_enc = self.leaky_relu( 162 | self.dyn_emb(ego_enc.view(ego_enc.shape[1], ego_enc.shape[2])) 163 | ) 164 | 165 | # Concatenate encodings: 166 | if self.ego_awareness: 167 | enc = torch.cat((left_enc, hist_enc, ego_enc, right_enc), 1) 168 | else: 169 | enc = torch.cat((left_enc, hist_enc, right_enc), 1) 170 | else: 171 | _, (hist_h, hist_c) = self.enc_lstm_hist(self.leaky_relu(self.ip_emb(hist))) 172 | _, (left_h, left_c) = self.enc_lstm_left( 173 | self.leaky_relu(self.ip_emb(left_bound)) 174 | ) 175 | _, (right_h, right_c) = self.enc_lstm_right( 176 | self.leaky_relu(self.ip_emb(right_bound)) 177 | ) 178 | 179 | enc = {} 180 | 181 | # the initial cell and hidden state of the decoder: 182 | enc["c"] = torch.tanh( 183 | self._c_embedder(torch.cat((hist_c, left_c, right_c), -1)) 184 | ) 185 | enc["h"] = torch.tanh( 186 | self._h_embedder(torch.cat((hist_h, left_h, right_h), -1)) 187 | ) 188 | 189 | fut_pred = self.decode(enc) 190 | return fut_pred 191 | 192 | def decode(self, enc): 193 | if self.args["decoder_type"] == "original": 194 | enc = enc.repeat(self.out_length, 1, 1) 195 | h_dec, _ = self.dec_lstm(enc) 196 | h_dec = h_dec.permute(1, 0, 2) # (batch_size, pred_len, decoder_size) 197 | fut_pred = self.op(h_dec) # (batch_size, pred_len, out_size) 198 | fut_pred = fut_pred.permute(1, 0, 2) # (pred_len, batch_size, out_size) 199 | 200 | else: 201 | batch_size = enc["h"].shape[1] 202 | device = enc["h"].device 203 | 204 | _, (h, _) = self.dec_lstm( 205 | torch.zeros((1, batch_size, 2), device=device), (enc["h"], enc["c"]) 206 | ) 207 | fut_pred = self.op(h) 208 | 209 | for _ in range(1, self.out_length): 210 | _, (h, _) = self.dec_lstm(torch.unsqueeze(fut_pred[-1, :, :2], dim=0)) 211 | fut_pred = torch.cat((fut_pred, self.op(h)), dim=0) 212 | 213 | fut_pred = outputActivation(fut_pred) 214 | 215 | return fut_pred 216 | 217 | def get_hidden_state(self, layer_function, enc_dec_layer="lstm"): 218 | if "lstm" in enc_dec_layer: 219 | _, (hidden_state, _) = layer_function 220 | elif "gru" in enc_dec_layer: 221 | _, hidden_state = layer_function 222 | 223 | return hidden_state 224 | 225 | def load_model_weights(self, weights_path): 226 | self.load_state_dict(torch.load(weights_path, map_location=self.device)) 227 | print("Successfully loaded model weights from {}".format(weights_path)) 228 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/overtake_fuzzy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from utils.fuzzy import FuzzyMembershipFunction, FuzzyImplication, FuzzyInference 4 | 5 | 6 | class IsFollowerNear(FuzzyMembershipFunction): 7 | """Fuzzy membership function that answers the question: 8 | "Is the follower car near the leading car?" 9 | """ 10 | 11 | def __call__(self, x): 12 | """implements the membership function. 13 | If the distance is smaller than bound_min, the value is 1 14 | If the distance is bigger than bound_max, the value is 0. 15 | Between them it varies linearly. 16 | 17 | args: 18 | x: dict, that must contain the following keys: 19 | pos_leading: the position of the leading vehicle. 20 | pos_following: the position of the following vehicle. 21 | 22 | returns: 23 | The membership value. 24 | """ 25 | 26 | # main variables that have an effect on the membership value: 27 | bound_min = 10 28 | bound_max = 70 29 | 30 | # extracting the variables: 31 | pos_leading = x["pos_leading"] 32 | pos_following = x["pos_following"] 33 | 34 | dist = np.linalg.norm(pos_leading - pos_following) 35 | 36 | # membership function definition: 37 | if dist < bound_min: 38 | # print("Is follower near: {}".format(1)) 39 | return 1 40 | elif dist < bound_max: 41 | # print("Is follower near: {}".format(1 - (dist - bound_min) / (bound_max - bound_min))) 42 | return 1 - (dist - bound_min) / (bound_max - bound_min) 43 | else: 44 | # print("Is follower near: {}".format(0)) 45 | return 0 46 | 47 | 48 | class IsFollowerOnTheRight(FuzzyMembershipFunction): 49 | """Fuzzy membership function that answers the question: 50 | "Is the follower car on the right of the leading car?" 51 | """ 52 | 53 | def __call__(self, x): 54 | """implements the membership function. 55 | It calculates the ratio between the distance of the 2 vehicles and 56 | the differenc between their distance to the left wall. The ratio is hence 57 | positive, if the follower car is on the right of the leader. 58 | If the ratio is bigger than +bound, the value is 1 59 | If the distance is smaller than -bound, the value is 0. 60 | Between them it varies linearly. 61 | 62 | args: 63 | x: dict, that must contain the following keys: 64 | pos_leading: the position of the leading vehicle. 65 | pos_following: the position of the following vehicle. 66 | side_dist_dict: Dictionary that contains the distances 67 | of the vehicles to the sides of the track. 68 | 69 | returns: 70 | The membership value. 71 | """ 72 | 73 | # main variables that have an effect on the membership value: 74 | bound = 0.1 75 | 76 | # extracting the variables: 77 | pos_leading = x["pos_leading"] 78 | pos_following = x["pos_following"] 79 | side_dist_dict = x["side_dist_dict"] 80 | 81 | dist = np.linalg.norm(pos_leading - pos_following) 82 | dist_diff_left = ( 83 | side_dist_dict["left_following"] - side_dist_dict["left_leading"] 84 | ) 85 | ratio = dist_diff_left / (dist + 1e-5) 86 | 87 | # membership function definition: 88 | if ratio > bound: 89 | # print("Is follower on the right: {}".format(1)) 90 | return 1 91 | elif ratio > -bound: 92 | # print("Is follower on the right: {}".format(1 - (bound - ratio) / (2 * bound))) 93 | return 1 - (bound - ratio) / (2 * bound) 94 | else: 95 | # print("Is follower on the right: {}".format(0)) 96 | return 0 97 | 98 | 99 | class BasedOnLeaderPosition(FuzzyMembershipFunction): 100 | """Fuzzy membership function that answers the question: 101 | "Should the leader car be overtaken from the right if 102 | considering only its position on the track?" 103 | """ 104 | 105 | def __call__(self, x): 106 | """implements the membership function. 107 | It is based on the distance of the leader car from the left 108 | track boundary. The function is linear and has got the 109 | following breakpoints (breakpoint - breakpoint value): 110 | Totally on the left - 1 111 | breakpoint1 - 0.8 112 | breakpoint2 - 0.2 113 | totally on the right - 0 114 | 115 | args: 116 | x: dict, that must contain the following keys: 117 | side_dist_dict: Dictionary that contains the distances 118 | of the vehicles to the sides of the track. 119 | overtake_margin: The minimum distance that is needed 120 | for overtaking a vehicle. 121 | 122 | returns: 123 | The membership value. 124 | """ 125 | 126 | # main variables that have an effect on the membership value: 127 | bp1 = 0.3 128 | bp2 = 1 - bp1 129 | 130 | # extracting the variables: 131 | side_dist_dict = x["side_dist_dict"] 132 | overtake_margin = x["overtake_margin"] 133 | 134 | dist_left = side_dist_dict["left_leading"] - overtake_margin 135 | track_width = ( 136 | side_dist_dict["left_leading"] 137 | + side_dist_dict["right_leading"] 138 | - 2 * overtake_margin 139 | ) 140 | 141 | ratio = dist_left / (track_width + 1e-5) 142 | 143 | # membership function definition: 144 | if ratio < bp1: 145 | # print("pure based on leader position: {}".format(1 - (ratio) / bp1 * 0.2)) 146 | return 1 - (ratio) / bp1 * 0.2 147 | elif ratio < bp2: 148 | # print("pure based on leader position: {}".format(0.8 - (ratio - bp1) / (bp2 - bp1) * 0.6)) 149 | return 0.8 - (ratio - bp1) / (bp2 - bp1) * 0.6 150 | else: 151 | # print("pure based on leader position: {}".format(0.2 - (ratio - bp2) / (1 - bp2) * 0.2)) 152 | return 0.2 - (ratio - bp2) / (1 - bp2) * 0.2 153 | 154 | 155 | class BasedOnRelativePosition(FuzzyMembershipFunction): 156 | """Fuzzy membership function that answers the question: 157 | "Should the leader car be overtaken from the right if 158 | only considering 2 cars lateral position?" 159 | """ 160 | 161 | def __call__(self, x): 162 | """implements the membership function. 163 | It is simply a linear membership function that connects 164 | the 2 extremum. 165 | 166 | args: 167 | x: dict, that must contain the following keys: 168 | side_dist_dict: Dictionary that contains the distances 169 | of the vehicles to the sides of the track. 170 | overtake_margin: The minimum distance that is needed 171 | for overtaking a vehicle. 172 | 173 | returns: 174 | The membership value. 175 | """ 176 | 177 | # extracting the variables: 178 | side_dist_dict = x["side_dist_dict"] 179 | overtake_margin = x["overtake_margin"] 180 | 181 | dist_left_leading = side_dist_dict["left_leading"] - overtake_margin 182 | dist_left_following = side_dist_dict["left_following"] - overtake_margin 183 | track_width = ( 184 | side_dist_dict["left_leading"] 185 | + side_dist_dict["right_leading"] 186 | - 2 * overtake_margin 187 | ) 188 | 189 | ratio = (dist_left_following - dist_left_leading) / (track_width + 1e-5) 190 | 191 | # membership function definition: 192 | # print("Based on relative position: {}".format(1 - (1 - ratio) / 2)) 193 | return 1 - (1 - ratio) / 2 194 | 195 | 196 | class CloseRightImplication(FuzzyImplication): 197 | """This Implication has to be reimplemented, the 198 | default min operator does not work here. The implication 199 | we want to take, is that if we are close to the vehicle, 200 | then we should overtake it from the side, on which we are on. 201 | So: 202 | "If the follower is near the leader and is on the right 203 | sid of the leader, we should overtake it from the right." 204 | 205 | The reason, why it should be reimplemented, is that if we are far 206 | away, than that does not imply anything, in that case the overtake 207 | can happen on both sides. So if we are far away, that does not simply 208 | negate the above implication (and hence leads to left overtake), it only 209 | sais, that based on this argument one can not decide (return value 0.5) 210 | """ 211 | 212 | def __call__(self, x): 213 | """Implements the implication based on the argument in 214 | the class description. 215 | """ 216 | 217 | membership_values = [ 218 | membership_function(x) for membership_function in self._evidences 219 | ] 220 | 221 | if membership_values[0] < 0.5: 222 | return 0.5 223 | else: 224 | return min(membership_values) 225 | 226 | 227 | class FuzzyOvertakeInference(FuzzyInference): 228 | """Class for the fuzzy inference, wether the 229 | car should be overtaken from the right or no. 230 | """ 231 | 232 | def __call__(self, x): 233 | """Reimplementing the default (max) inference 234 | operator to normalized summation. 235 | """ 236 | 237 | implication_values = [implication(x) for implication in self._implications] 238 | 239 | return sum(implication_values) / len(implication_values) 240 | -------------------------------------------------------------------------------- /mix_net/mix_net/src/indy_net_handler.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import numpy as np 4 | import torch 5 | import copy 6 | from typing import Tuple 7 | 8 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 9 | sys.path.append(repo_path) 10 | 11 | from utils.handler_interface import HandlerInterface 12 | from src.indy_net import IndyNet 13 | from src.boundary_generator import BoundaryGenerator 14 | from utils.cuda import cudanize 15 | from utils.geometry import ( 16 | get_heading, 17 | angle_between, 18 | transform_trajectory, 19 | retransform_trajectory, 20 | retransform_cov, 21 | ) 22 | 23 | 24 | class IndyNetHandler(HandlerInterface): 25 | """Handler class for the IndyNet neural network for prediction.""" 26 | 27 | def __init__( 28 | self, 29 | net: IndyNet, 30 | bg: BoundaryGenerator, 31 | params: dict, 32 | main_logger=None, 33 | ): 34 | """Initializes the IndyNetHandler object.""" 35 | 36 | super().__init__() 37 | 38 | self._net = net.cuda() if params["use_cuda"] else net 39 | self._bg = bg 40 | self._main_logger = main_logger 41 | self._params = params 42 | self._use_cuda = params["use_cuda"] 43 | 44 | def predict( 45 | self, 46 | obs_storage: dict, 47 | prediction_id: int, 48 | ) -> Tuple[dict, dict, dict, dict, int]: 49 | """Carries out the predictions. 50 | 51 | args: 52 | obs_storage: (dict), the observation storage that was received from mod_objects 53 | and which already only contains the IDs which have to be predicted 54 | data based. 55 | prediction_id: (int), the prediction ID of the next prediction. 56 | 57 | returns: 58 | pred_dict: (dict), the dictionary which then directly can be added to the prediction 59 | dict in the main, 60 | log_hist: (dict), the dict of history logs, 61 | log_obj: (dict), the dict of object logs, 62 | log_boundaries: (dict), the log of boundary logs, 63 | prediction_id: (int), The next prediction ID. 64 | """ 65 | pred_dict = {} 66 | log_hist, log_obj, log_boundaries = {}, {}, {} 67 | 68 | if obs_storage == {}: 69 | return pred_dict, log_hist, log_obj, log_boundaries, prediction_id 70 | 71 | # some frequently used params: 72 | sampling_frequency = self._params["MODEL_PARAMS"]["sampling_frequency"] 73 | data_min_obs_length = self._params["MODEL_PARAMS"]["data_min_obs_length"] 74 | 75 | # Generate input for neural network 76 | ( 77 | hist, 78 | left_bound, 79 | right_bound, 80 | translations, 81 | rotations, 82 | ) = self._generate_network_input(obs_storage) 83 | 84 | # Neural Network 85 | fut_preds = self._net(hist, left_bound, right_bound).detach().cpu().numpy() 86 | 87 | # Iterate over NN predictions 88 | for idx, obj_key in enumerate(obs_storage.keys()): 89 | fut_pred = np.expand_dims(fut_preds[:, idx, :], axis=1) 90 | 91 | # Calc time_list with relative times 92 | time_array = np.linspace( 93 | 0, 94 | fut_pred.shape[0] / sampling_frequency, 95 | fut_pred.shape[0] + 1, 96 | ) 97 | 98 | # Add (0,0) according to convention starting with t=0.0s 99 | pred_out = np.concatenate((np.zeros((1, 1, 5)), fut_pred), axis=0) 100 | 101 | # Retransform positions 102 | xy_global = retransform_trajectory( 103 | pred_out[:, 0, :2], translations[idx], rotations[idx] 104 | ) 105 | 106 | # Retransform covariances 107 | cov_array = retransform_cov(pred_out, rotations[idx]) 108 | 109 | # Get heading 110 | heading_list = get_heading(xy_global) 111 | 112 | # Get t_abs_perception 113 | t_abs_perception = obs_storage[obj_key]["t_abs_perception"] 114 | 115 | # Add data prediction to pred_dict 116 | pred_dict[prediction_id] = { 117 | "valid": True, 118 | "vehicle_id": obj_key, 119 | "prediction_type": "data", 120 | "t_abs_perception": t_abs_perception, 121 | "t": time_array, 122 | "x": xy_global[:, 0], 123 | "y": xy_global[:, 1], 124 | "cov": cov_array, 125 | "heading": heading_list, 126 | } 127 | 128 | # Log 129 | log_hist[prediction_id] = retransform_trajectory( 130 | self.pre_log_hist[idx, :, :], translations[idx, :], rotations[idx] 131 | ) 132 | 133 | log_obj[prediction_id] = ( 134 | obj_key, 135 | translations[idx, :], 136 | t_abs_perception, 137 | ) 138 | 139 | log_boundaries[prediction_id] = [ 140 | retransform_trajectory( 141 | self.log_left_bounds[idx, :, :], 142 | translations[idx, :], 143 | rotations[idx], 144 | ), 145 | retransform_trajectory( 146 | self.log_right_bounds[idx, :, :], 147 | translations[idx, :], 148 | rotations[idx], 149 | ), 150 | ] 151 | 152 | if self._main_logger is not None: 153 | self._main_logger.info( 154 | "Prediction-ID {0:d}: Long observation (hist={1:.1f} > min_obs_lenght={2:.1f}) - data-prediction applied, id = {3}".format( 155 | prediction_id, 156 | len(obs_storage[obj_key]["xy_positions"]) / sampling_frequency, 157 | data_min_obs_length, 158 | obj_key, 159 | ) 160 | ) 161 | 162 | # Count predictions 163 | prediction_id += 1 164 | 165 | return pred_dict, log_hist, log_obj, log_boundaries, prediction_id 166 | 167 | def _generate_network_input(self, obs_storage: dict): 168 | """Generates the network inputs for the IndyNet. 169 | 170 | args: 171 | obs_storage: (dict), the observation storage that was received from mod_objects 172 | and which already only contains the IDs which have to be predicted 173 | data based. 174 | 175 | returns: tuple, the inputs for the IndyNet: 176 | hist: [tensor with shape=(max_hist_len, batch_size, 2)], 177 | left_bound: [tensor with shape=(bound_len, batch_size, 2)], 178 | right_bound: [tensor with shape=(bound_len, batch_size, 2)], 179 | translations: [np.array with shape=(batch_size, 2)], 180 | rotations: [np.array with shape=(batch_size,)] 181 | """ 182 | 183 | # some frequently used variables: 184 | max_obs_length = self._params["OBJ_HANDLING_PARAMS"]["max_obs_length"] 185 | 186 | hist_list = [] 187 | translations = [] 188 | rotations = [] 189 | left_bounds, right_bounds = [], [] 190 | 191 | for obj_vals in obs_storage.values(): 192 | # Fill different input lengths with zeros 193 | hist_zeros = np.zeros((max_obs_length, 2)) 194 | 195 | # collecting the last states for boundary generation and transformation: 196 | translation = copy.deepcopy(obj_vals["xy_positions"][0, :]) 197 | translations.append(translation) 198 | 199 | # Get Boundaries 200 | left_bound, right_bound = self._bg.get_boundaries_single(translation) 201 | 202 | # getting the rotation: 203 | rotation = angle_between(left_bound[1, :] - left_bound[0, :], [1, 0]) 204 | rotations.append(rotation) 205 | 206 | # Transforming the history and CHANGING HISTORY ORDER!!!!: 207 | hist_zeros[: obj_vals["xy_positions"].shape[0], :] = transform_trajectory( 208 | obj_vals["xy_positions"][::-1, :], translation, rotation 209 | ) 210 | hist_list.append(hist_zeros) 211 | 212 | # also transforming the boundaries and adding them to the list: 213 | left_bounds.append(transform_trajectory(left_bound, translation, rotation)) 214 | right_bounds.append( 215 | transform_trajectory(right_bound, translation, rotation) 216 | ) 217 | 218 | if len(hist_list) == 0: 219 | return None, None, None, None, None 220 | 221 | # numpifying: 222 | hist = np.array(hist_list) 223 | translations = np.array(translations) 224 | left_bounds = np.array(left_bounds) 225 | right_bounds = np.array(right_bounds) 226 | 227 | # Log hist 228 | self.pre_log_hist = copy.deepcopy(hist) 229 | 230 | # Transform hist 231 | hist = np.swapaxes(hist, 0, 1) 232 | 233 | # Log Boundaries 234 | self.log_left_bounds = copy.deepcopy(left_bounds) 235 | self.log_right_bounds = copy.deepcopy(right_bounds) 236 | 237 | # Swap axes 238 | left_bound = np.swapaxes(np.array(left_bounds), 0, 1) 239 | right_bound = np.swapaxes(np.array(right_bounds), 0, 1) 240 | 241 | # Cudanize 242 | if self._use_cuda: 243 | hist, left_bound, right_bound = cudanize(hist, left_bound, right_bound) 244 | else: 245 | hist = torch.as_tensor(hist).float() 246 | left_bound = torch.as_tensor(left_bound).float() 247 | right_bound = torch.as_tensor(right_bound).float() 248 | 249 | return hist, left_bound, right_bound, translations, rotations 250 | -------------------------------------------------------------------------------- /tools/visualize_smoothness.py: -------------------------------------------------------------------------------- 1 | """ 2 | This debug visualization tool takes (autoamically) the latest logfile 3 | in the logs/ folder and visualizes the predictions. 4 | 5 | Arguments: 6 | --velocity: Plot the velocity profile. 7 | --dataset: Create a dataset for training etc. out of the ground truth data 8 | """ 9 | 10 | # Settings 11 | ZOOM_VEH_ID = None # Vehicle ID that should be zoomed to 12 | import os 13 | import sys 14 | import argparse 15 | 16 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 17 | sys.path.append(repo_path) 18 | 19 | import matplotlib.pyplot as plt 20 | 21 | import numpy as np 22 | from mix_net.mix_net.src.boundary_generator import BoundaryGenerator 23 | 24 | from tools.visualize_logfiles import PredictionLogVisualizer 25 | from tools.evaluation_line_plot import update_matplotlib 26 | 27 | ERROR_ANALYSIS_TIME_STEPS = [9, 19, 29, 39, 49] 28 | TRACK_PATH = "mix_net/mix_net/data/map/traj_ltpl_cl_IMS_GPS.csv" 29 | CONFIG_PATH = "mix_net/mix_net/config/main_params.ini" 30 | SAVE_DIR = "data/evaluation_data/smoothness" 31 | if not os.path.exists(SAVE_DIR): 32 | os.makedirs(SAVE_DIR) 33 | 34 | 35 | TUM_BLUE = (0 / 255, 101 / 255, 189 / 255) 36 | TUM_LIGHT_BLUE = (100 / 255, 160 / 255, 200 / 255) 37 | TUM_LIGHTER_BLUE = (152 / 255, 198 / 255, 234 / 255) 38 | TUM_ORAN = (227 / 255, 114 / 255, 34 / 255) 39 | TUM_GREEN = (162 / 255, 173 / 255, 0 / 255) 40 | TUM_BLACK = (0, 0, 0) 41 | GRAY = (104 / 255, 104 / 255, 104 / 255) 42 | BOUND_COL = (204 / 255, 204 / 255, 204 / 255) 43 | 44 | COLUMNWIDTH = 3.5 45 | PAGEWIDTH = COLUMNWIDTH * 2 46 | LABELFONTSIZE = 8 # IEEE is 8 47 | 48 | FIGSIZE = (PAGEWIDTH, PAGEWIDTH / 2) 49 | 50 | INDY_NET_COL = GRAY 51 | RAIL_BASED_COL = TUM_ORAN 52 | MIX_NET_COL = TUM_BLUE 53 | HIST_COL = TUM_BLACK 54 | HIST_LS = "solid" 55 | GT_COL = TUM_BLACK 56 | GT_LS = "dashed" 57 | BOUND_COL = BOUND_COL 58 | 59 | update_matplotlib() 60 | 61 | 62 | def plot_all( 63 | _ax, 64 | boundaries_pid, 65 | lb_sample, 66 | rb_sample, 67 | gt, 68 | pred_rail, 69 | pred, 70 | pred_mix_net, 71 | hist=None, 72 | ): 73 | """Plot bounds, history, preictions (mixnet, indynet, rail) and ground truth.""" 74 | if hist is not None: 75 | _ax.plot( 76 | hist[0, :], 77 | hist[1, :], 78 | color=HIST_COL, 79 | linestyle="solid", 80 | label="IN", 81 | ) 82 | _ax.plot( 83 | lb_sample[:, 0], 84 | lb_sample[:, 1], 85 | "x", 86 | color=BOUND_COL, 87 | linestyle="solid", 88 | ) 89 | _ax.plot( 90 | rb_sample[:, 0], 91 | rb_sample[:, 1], 92 | "x", 93 | color=BOUND_COL, 94 | label="IN", 95 | linestyle="solid", 96 | ) 97 | _ax.plot( 98 | gt[0, :], 99 | gt[1, :], 100 | color=GT_COL, 101 | linestyle=GT_LS, 102 | label="GT", 103 | ) 104 | _ax.plot( 105 | pred_rail[0, :], 106 | pred_rail[1, :], 107 | color=RAIL_BASED_COL, 108 | linestyle="solid", 109 | label="RAIL", 110 | ) 111 | _ax.plot( 112 | pred[0, :], 113 | pred[1, :], 114 | color=INDY_NET_COL, 115 | linestyle="solid", 116 | label="BENCHMARK", 117 | ) 118 | _ax.plot( 119 | pred_mix_net[0, :], 120 | pred_mix_net[1, :], 121 | color=MIX_NET_COL, 122 | linestyle="solid", 123 | label="MIXNET", 124 | ) 125 | 126 | 127 | def visz_frame( 128 | visualizer_benchmark, visualizer_mixnet, pred_id: str, with_raceline: bool = False 129 | ): 130 | """Visualize example frame from logs. 131 | 132 | Visualized are Ground Truth, History, Bounds, MixNet, IndyNet and Rail-based Prediction. 133 | 134 | Args: 135 | visualizer (PredictionLogVisualizer): Visualizer class to process logged data. 136 | pred_id (str): ID of prediction to visualize 137 | """ 138 | 139 | _fig = plt.figure(figsize=FIGSIZE) 140 | _ax = _fig.add_subplot(111) 141 | 142 | _, gt, pred_bm, hist_pid = get_single_log(visualizer_benchmark, pred_id) 143 | boundaries_pid, _, pred_mix_net, _ = get_single_log(visualizer_mixnet, pred_id) 144 | 145 | lb_sample, rb_sample, pred_rail = get_rail_prediction( 146 | visualizer_benchmark._recovered_params, 147 | pred_bm, 148 | hist_pid, 149 | with_raceline=with_raceline, 150 | ) 151 | 152 | # Visualize benchmark 153 | plot_all( 154 | _ax, 155 | boundaries_pid, 156 | lb_sample, 157 | rb_sample, 158 | gt, 159 | pred_rail, 160 | pred_bm, 161 | pred_mix_net, 162 | hist=hist_pid, 163 | ) 164 | 165 | _ax.set_xlim(-227, 357) 166 | _ax.set_ylim(-2417, -2280) 167 | 168 | insax_1 = _ax.inset_axes([0.6, 0.6, 0.25, 0.7]) 169 | insax_1.axis("equal") 170 | 171 | plot_all( 172 | insax_1, 173 | boundaries_pid, 174 | lb_sample, 175 | rb_sample, 176 | gt, 177 | pred_rail, 178 | pred_bm, 179 | pred_mix_net, 180 | ) 181 | 182 | insax_xmin = 270 183 | insax_xmax = 315 184 | insax_ymin = -2375 185 | insax_ymax = -2340 186 | 187 | insax_1.set_xlim(insax_xmin, insax_xmax) 188 | insax_1.set_ylim(insax_ymin, insax_ymax) 189 | insax_1.set_xticklabels([]) 190 | insax_1.set_yticklabels([]) 191 | _ax.indicate_inset_zoom(insax_1, edgecolor="black") 192 | 193 | insax_2 = _ax.inset_axes([0.1, 0.6, 0.4, 0.7]) 194 | insax_2.axis("equal") 195 | 196 | plot_all( 197 | insax_2, 198 | boundaries_pid, 199 | lb_sample, 200 | rb_sample, 201 | gt, 202 | pred_rail, 203 | pred_bm, 204 | pred_mix_net, 205 | hist=hist_pid, 206 | ) 207 | 208 | insax_xmin = -47 209 | insax_xmax = 67 210 | insax_ymin = -2415 211 | insax_ymax = -2380 212 | 213 | insax_2.set_xlim(insax_xmin, insax_xmax) 214 | insax_2.set_ylim(insax_ymin, insax_ymax) 215 | insax_2.set_xticklabels([]) 216 | insax_2.set_yticklabels([]) 217 | 218 | _ax.indicate_inset_zoom(insax_2, edgecolor="black") 219 | 220 | # setting axis labels of ax1: 221 | # _ax.legend(loc="upper left") 222 | _ax.set_xlabel("$x$ in m") 223 | _ax.set_ylabel("$y$ in m") 224 | # plt.axis([-400, 360, -2425, -2310]) 225 | plt.gca().set_aspect("equal", adjustable="box") 226 | _ax.grid(True) 227 | 228 | if with_raceline: 229 | strstr = "_raceline" 230 | else: 231 | strstr = "" 232 | plt.savefig(os.path.join("assets", "scenario_sample" + strstr + ".svg")) 233 | plt.savefig(os.path.join(SAVE_DIR, "scenario_sample" + strstr + ".pdf")) 234 | plt.show() 235 | 236 | 237 | def get_single_log(visualizer, pred_id): 238 | t_abs, _, hist, _, pred_dict, _ = visualizer._all_log_data[int(pred_id)] 239 | 240 | # The prediction: 241 | pred_x = pred_dict[pred_id]["x"] 242 | pred_y = pred_dict[pred_id]["y"] 243 | pred = np.vstack((pred_x, pred_y)) 244 | vehicle_id = pred_dict[pred_id]["vehicle_id"] 245 | 246 | # division by 1e9 is needed, because mod_object sends nanosecs: 247 | t_abs = float(pred_dict[pred_id]["t_abs_perception"]) / 1e9 248 | 249 | # If there is no real history log, it is created from the gt_dict: 250 | hist_pid = np.array(hist[pred_id]).T 251 | # Remove all zeros (that were added before feeding it to the nets): 252 | hist_pid = np.ma.masked_equal(hist_pid, 0) 253 | 254 | # get bounds 255 | ( 256 | left_bound, 257 | right_bound, 258 | ) = visualizer._boundary_generator.get_bounds_between_points( 259 | pred[:, 0], pred[:, -1] 260 | ) 261 | boundaries_pid = np.array([left_bound, right_bound]) 262 | 263 | # vel = np.mean(np.linalg.norm(np.diff(np.stack([pred_dict[pred_id]["x"][:3], pred_dict[pred_id]["y"][:3]])), axis=0))/0.1 264 | # Ground Truth: 265 | gt = visualizer._get_gt_from_trajs(vehicle_id, t_abs, pred.shape[1]) 266 | 267 | return boundaries_pid, gt, pred, hist_pid 268 | 269 | 270 | def get_rail_prediction(recovered_params, pred, hist_pid, with_raceline=False): 271 | """Determine rail-based prediction.""" 272 | # get boundaries 273 | _bg = BoundaryGenerator(recovered_params) 274 | lb_sample, rb_sample = _bg.get_boundaries_single( 275 | hist_pid[:, -1], with_raceline=False 276 | ) 277 | 278 | # determine rail-based prediction 279 | pred_rail = _bg.get_rail_pred( 280 | np.array(hist_pid[:, -1]), pred=pred, with_raceline=with_raceline 281 | ) 282 | 283 | return lb_sample, rb_sample, pred_rail 284 | 285 | 286 | if __name__ == "__main__": 287 | # Parse arguments: 288 | parser = argparse.ArgumentParser() 289 | parser.add_argument( 290 | "--logdir_benchmark", 291 | type=str, 292 | default="data/evaluation_data/2_indynet/12_13_21/", 293 | help="The logdir of benchmark model to get logs from", 294 | ) 295 | parser.add_argument( 296 | "--logdir_mixnet", 297 | type=str, 298 | default="data/evaluation_data/1_mixnet/14_23_40/", 299 | help="The logdir of mixnet to get logs from", 300 | ) 301 | parser.add_argument( 302 | "--pred_id", 303 | type=str, 304 | default="760", 305 | help="prediction ID which is analyzed", 306 | ) 307 | args = parser.parse_args() 308 | 309 | args.velocity = False 310 | args.analyze = False 311 | args.yolo = True 312 | 313 | # create visualizer object: 314 | 315 | args.logdir = args.logdir_benchmark 316 | visualizer_benchmark = PredictionLogVisualizer( 317 | args=args, 318 | zoom_veh_id=None, 319 | draw_uncertainty=False, 320 | yolo=True, 321 | ) 322 | 323 | args.logdir = args.logdir_mixnet 324 | visualizer_mixnet = PredictionLogVisualizer( 325 | args=args, 326 | zoom_veh_id=None, 327 | draw_uncertainty=False, 328 | yolo=True, 329 | ) 330 | 331 | # visualize example of smooth prediction output 332 | visz_frame(visualizer_benchmark, visualizer_mixnet, pred_id=args.pred_id) 333 | visz_frame( 334 | visualizer_benchmark, 335 | visualizer_mixnet, 336 | pred_id=args.pred_id, 337 | with_raceline=True, 338 | ) 339 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/geometry.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from shapely.geometry import Polygon 4 | 5 | 6 | def pi_range(yaw): 7 | """Clip yaw to (-pi, +pi].""" 8 | if yaw <= -np.pi: 9 | yaw += 2 * np.pi 10 | return yaw 11 | elif yaw > np.pi: 12 | yaw -= 2 * np.pi 13 | return yaw 14 | return yaw 15 | 16 | 17 | def get_heading(trajectory): 18 | """Calculates heading angles from x and y coordinates. 19 | IMPORTANT: 20 | - Convention says that y-axis has a heading of 0. And 90° is at -x! 21 | - Out of n x,y coordinates we can only calculate n-1 heading angles. To keep the desired format of length n we repeat the last entry. 22 | 23 | Args: 24 | trajectory (np.array): The (x, y) points of the trajectory 25 | """ 26 | 27 | # differences between the successive trajectory points. 28 | delta = trajectory[1:, :] - trajectory[:-1, :] 29 | 30 | # initializing and filling up the heading array: 31 | heading = np.zeros(trajectory.shape[0]) 32 | heading[:-1] = np.arctan2(-delta[:, 0], delta[:, 1]) 33 | 34 | # repeating the last entry to have the sufficient length: 35 | heading[-1] = heading[-2] 36 | 37 | return heading 38 | 39 | 40 | def calc_distance(p1, p2): 41 | """Calculate the distance of two points in x,y 42 | 43 | Args: 44 | pos1 ([type]): [description] 45 | pos2 ([type]): [description] 46 | """ 47 | return np.sqrt(((p1[0] - p2[0]) ** 2) + ((p1[1] - p2[1]) ** 2)) 48 | 49 | 50 | def distance_point_line(point, line): 51 | line = np.asarray(line) 52 | deltas = line - point 53 | dist_2 = np.einsum("ij,ij->i", deltas, deltas) 54 | return np.sqrt(np.min(dist_2)) 55 | 56 | 57 | def check_collision(point_1, point_2, approx_radius=3.0): 58 | """Checks 2 x,y points for collision with approximated shape of a circle with approx_radius 59 | 60 | Args: 61 | point_1 ([type]): [description] 62 | point_2 ([type]): [description] 63 | approx_radius (float, optional): [description]. Defaults to 3.0. 64 | """ 65 | 66 | return calc_distance(point_1, point_2) <= (2 * approx_radius) 67 | 68 | 69 | def rotate_loc_glob(loc_position, rot_angle): 70 | """Rotates 2D-array from local to global coordinates. 71 | 72 | Args: 73 | loc_position (ndarray(dtype=float, shape=(2,)): Local position (x, y). 74 | rot_angle (float): Rotation angle in rad from local to global coordinates. 75 | Convention: rot_angle = 0.0 for local x is parallel to global y 76 | rot_angle = - pi / 2.0 for local x is parallel to global x (rot_mat = eye(2)) 77 | 78 | Return: 79 | ndarray(dtype=float, shape=(2,): Global position (x, y). 80 | """ 81 | rot_mat = np.array( 82 | [ 83 | [-np.sin(rot_angle), -np.cos(rot_angle)], 84 | [np.cos(rot_angle), -np.sin(rot_angle)], 85 | ] 86 | ) 87 | 88 | return np.matmul(rot_mat, loc_position) 89 | 90 | 91 | def check_collision_rect( 92 | pose_1, 93 | pose_2, 94 | lat_veh_half_m, 95 | long_veh_half_m, 96 | lat_safety_dist_m, 97 | long_safety_dist_m, 98 | ): 99 | """Checks if two equal rectangluar objects (l x w) incl. safety distance overlap. 100 | 101 | Args: 102 | pose_1 ([type]): Pose of object 1 (x, y, yaw). 103 | pose_2 ([type]): Pose of object 2 (x, y, yaw). 104 | w_vehicle_m (float): Width of rectangle in m. 105 | l_vehicle_m (float): Length of rectangle in m. 106 | lat_safety_m float: lateral safety distance, half width of reactangle. 107 | long_safety_m float: longitudinal safety distance, half length of reactangle. 108 | 109 | Return: 110 | bool: True if overlap (intersection area > 0), False otherwise 111 | """ 112 | # Object 1 with safety distance 113 | rectangle_1 = np.stack( 114 | [ 115 | [long_safety_dist_m, -lat_safety_dist_m], 116 | [long_safety_dist_m, lat_safety_dist_m], 117 | [-long_safety_dist_m, lat_safety_dist_m], 118 | [-long_safety_dist_m, -lat_safety_dist_m], 119 | ], 120 | axis=1, 121 | ) 122 | 123 | edges_glob_1 = rotate_loc_glob(rectangle_1, pose_1[2]) + np.expand_dims( 124 | pose_1[:2], axis=1 125 | ) 126 | 127 | # Object 2 without safety distance 128 | rectangle_2 = np.stack( 129 | [ 130 | [long_veh_half_m, -lat_veh_half_m], 131 | [long_veh_half_m, lat_veh_half_m], 132 | [-long_veh_half_m, lat_veh_half_m], 133 | [-long_veh_half_m, -lat_veh_half_m], 134 | ], 135 | axis=1, 136 | ) 137 | edges_glob_2 = rotate_loc_glob(rectangle_2, pose_2[2]) + np.expand_dims( 138 | pose_2[:2], axis=1 139 | ) 140 | 141 | poly_1 = Polygon(edges_glob_1.T) 142 | poly_2 = Polygon(edges_glob_2.T) 143 | intersec = poly_1.intersection(poly_2) 144 | 145 | return bool(intersec.area) 146 | 147 | 148 | def get_v_and_acc_profile(position_list, sampling_freq=10): 149 | """Return velocity and acceleration profile from a list of positions. 150 | 151 | Args: 152 | position_list ([list]): [list of positions in x,y] 153 | sampling_freq ([int]): Sampling frequency of the point list 154 | """ 155 | 156 | # Calc distance profile 157 | distances = [ 158 | calc_distance(position_list[i], position_list[i + 1]) 159 | for i in range(len(position_list) - 1) 160 | ] 161 | 162 | # Calc velocity profile 163 | velocities = [dist * sampling_freq for dist in distances] 164 | 165 | # Calc acceleration profile 166 | acceleartions = [ 167 | (velocities[i + 1] - velocities[i]) * sampling_freq 168 | for i in range(len(velocities) - 1) 169 | ] 170 | 171 | return velocities, acceleartions 172 | 173 | 174 | def angle_between(v1, v2): 175 | """Returns the angle in radians between vectors 'v1' and 'v2':: 176 | args: 177 | v1: np.array((N, 2)) or np.array(2,) 178 | v2: np.array((2,)) 179 | 180 | convention: "How much should I rotate v1 to get v2?" 181 | 182 | >>> angle_between((1, 0), (0, 1)) 183 | 1.5707963267948966 184 | >>> angle_between((1, 0), (1, 0)) 185 | 0.0 186 | >>> angle_between((1, 0), (-1, 0)) 187 | 3.141592653589793 188 | """ 189 | 190 | dot_product = v1 @ v2 # |v1| * |v2| * cos(angle) 191 | if len(v1.shape) == 2: 192 | cross_product = v1[:, 0] * v2[1] - v1[:, 1] * v2[0] # |v1| * |v2| * sin(angle) 193 | else: 194 | cross_product = v1[0] * v2[1] - v1[1] * v2[0] 195 | 196 | rotations = np.arctan2( 197 | cross_product, dot_product 198 | ) # arctan(sin(angle) / cos(angle)) 199 | return rotations 200 | 201 | 202 | def transform_trajectory(trajectory, translation, rotation): 203 | """Transforms the given trajectory by first shifting it and then rotating it. 204 | 205 | out = rotate(trajectory - translation) 206 | 207 | args: 208 | trajectory: [np.array with shape=(traj_len, 2) or shape=(batch_size, traj_len, 2)] 209 | translation: [np.array with shape=(2,) or shape=(batch_size, 2)] 210 | rotation: [scalar or np.array with shape=(batch_size,)] 211 | 212 | returns: 213 | the transformed trajectory: 214 | [np.array with shape=(traj_len, 2) or shape=(batch_size, traj_len, 2)] 215 | """ 216 | 217 | rot_mat = np.array( 218 | [[np.cos(rotation), -np.sin(rotation)], [np.sin(rotation), np.cos(rotation)]] 219 | ) 220 | 221 | translation = np.expand_dims(translation, axis=-2) 222 | 223 | # creating the transposed roation matrix: 224 | if len(trajectory.shape) == 3: 225 | # reshaping the rotation matrix if in batch mode: 226 | rot_mat = np.moveaxis(rot_mat, 2, 0) # (batch_size, 2, 2) 227 | rot_mat = np.transpose(rot_mat, axes=(0, 2, 1)) 228 | else: 229 | rot_mat = rot_mat.T 230 | 231 | trajectory -= translation 232 | 233 | # transformed_trajectory = np.matmul(trajectory, rot_mat) 234 | transformed_trajectory = trajectory @ rot_mat 235 | 236 | return transformed_trajectory 237 | 238 | 239 | def retransform_trajectory(trajectory, translation, rotation): 240 | """Retransforms a trajectory that has been transformed by shifting it by -translation and rotating it by rotation. 241 | Hence, the points are first rotated by -rotation and then shifted by translation. 242 | 243 | Args: 244 | trajectory [np.array with shape=(traj_len, 2)]: The (x, y) points of the trajectory 245 | translation [np.array with shape=(2,)]: The translation vector 246 | rotation [scalar]: The yaw angle of the rotation 247 | 248 | Returns: 249 | The transformed trajectory (np.array) 250 | """ 251 | rot_mat = np.array( 252 | [ 253 | [np.cos(-rotation), -np.sin(-rotation)], 254 | [np.sin(-rotation), np.cos(-rotation)], 255 | ] 256 | ) 257 | 258 | return trajectory @ rot_mat.T + translation 259 | 260 | 261 | def retransform_cov(pred_out, rotation): 262 | """Transforms covariance matrices to the global coordinate system (= rotates them) 263 | 264 | Args: 265 | pred_out ([np.array]): [description] 266 | rotation ([float]): [rotation angle] 267 | 268 | Returns: 269 | The transformed covariance matrix in np.array 270 | """ 271 | rot_mat = np.array( 272 | [ 273 | [np.cos(-rotation), -np.sin(-rotation)], 274 | [np.sin(-rotation), np.cos(-rotation)], 275 | ] 276 | ) 277 | 278 | cov_list = [ 279 | rot_mat 280 | @ np.array( 281 | [ 282 | [pred_out[x, 0, 2], pred_out[x, 0, 4]], 283 | [pred_out[x, 0, 4], pred_out[x, 0, 3]], 284 | ] 285 | ) 286 | @ rot_mat.T 287 | for x in range(len(pred_out)) 288 | ] 289 | 290 | return np.array(cov_list) 291 | 292 | 293 | if __name__ == "__main__": 294 | 295 | # Test with example data 296 | 297 | x_list = [ 298 | 437.56731038436345, 299 | 438.6852893959684, 300 | 439.74080209936903, 301 | 440.4698214772491, 302 | 441.14865354183496, 303 | 441.7945383546361, 304 | 442.35865104136474, 305 | 442.7998234383539, 306 | 443.12149533189904, 307 | 443.32865451485156, 308 | 443.4094288693953, 309 | 443.35920837959816, 310 | 443.16746519138655, 311 | 442.7983781427311, 312 | ] 313 | y_list = [ 314 | -2172.249342799885, 315 | -2167.336603425321, 316 | -2162.0025032218996, 317 | -2155.9708399216497, 318 | -2149.2644011594402, 319 | -2142.060639930153, 320 | -2134.432286381409, 321 | -2126.4807102410305, 322 | -2118.2234666886065, 323 | -2109.390705020447, 324 | -2099.7082952523397, 325 | -2089.7006866135544, 326 | -2080.4078697775567, 327 | -2072.1956765733526, 328 | ] 329 | 330 | trajectory = np.array([x_list, y_list]).T 331 | 332 | heading = get_heading(trajectory) 333 | 334 | print(heading) 335 | 336 | plt.plot(x_list, y_list) 337 | plt.axis("equal") 338 | plt.show() 339 | -------------------------------------------------------------------------------- /train/indy_net_train.py: -------------------------------------------------------------------------------- 1 | """ 2 | This script is the main script for the training of the prediction network. 3 | Arguments: 4 | -- config 5 | -- debug Set for debug mode (only one step training/validation/evaluation) 6 | """ 7 | # Standard imports 8 | import sys 9 | import os 10 | import json 11 | import argparse 12 | import matplotlib 13 | 14 | matplotlib.use("Agg") 15 | 16 | # Third party imports 17 | import torch 18 | from torch.nn import MSELoss 19 | import pkbar 20 | import git 21 | import numpy as np 22 | from torch.utils.data import DataLoader 23 | from torch.utils.tensorboard import SummaryWriter 24 | from torch.optim.lr_scheduler import ExponentialLR 25 | import optuna 26 | 27 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 28 | sys.path.append(repo_path) 29 | 30 | # Custom imports 31 | from train.indy_net_dataset import IndyDataset 32 | from mix_net.mix_net.src.indy_net import IndyNet 33 | from train.neural_network import NLL 34 | from mix_net.mix_net.utils.cuda import cudanize 35 | from train.indy_net_evaluation import evaluate 36 | from train.data_set_helper import split_indy_net_data, load_indy_net_data 37 | from train.neural_network import weighted_MSE 38 | 39 | 40 | def main(common_args, verbose=True, trial=None): 41 | """Trains the IndyNet.""" 42 | # Create model path 43 | if not os.path.exists(common_args["save_path"]): 44 | os.makedirs(common_args["save_path"]) 45 | model_path = os.path.join( 46 | common_args["save_path"], common_args["model_name"] + ".tar" 47 | ) 48 | 49 | # Initialize network 50 | net = IndyNet(common_args) 51 | 52 | # Get number of parameters 53 | pytorch_total_params = sum(p.numel() for p in net.parameters() if p.requires_grad) 54 | if verbose: 55 | print("Model initialized with {} parameters".format(pytorch_total_params)) 56 | 57 | # Get current git commit hash 58 | repo = git.Repo(search_parent_directories=True) 59 | sha = repo.head.object.hexsha 60 | common_args["commit"] = sha 61 | 62 | # Initialize tensorboard 63 | writer = SummaryWriter( 64 | os.path.join(common_args["tb_logs"], common_args["model_name"]) 65 | ) 66 | 67 | # Initialize optimizer 68 | optimizer_rmse = torch.optim.Adam(net.parameters(), lr=common_args["lr_rmse"]) 69 | optimizer_nll = torch.optim.Adam(net.parameters(), lr=common_args["lr_nll"]) 70 | 71 | # learning rate scheduling: 72 | scheduler = ExponentialLR( 73 | optimizer=optimizer_rmse, gamma=common_args["lr_rmse_decay_rate"] 74 | ) 75 | 76 | if common_args["loss_fn"] == "MSE": 77 | loss_fn = MSELoss() 78 | else: # WMSE 79 | loss_fn = weighted_MSE 80 | 81 | # weights of weighted MSE loss: 82 | mse_weights = torch.ones((common_args["out_length"],)).to(net.device) 83 | mse_weights[:20] = torch.linspace(2.0, 1.0, steps=20) 84 | 85 | # Initialize data loaders 86 | print("Loading data...") 87 | 88 | sample_data = load_indy_net_data(path=common_args["data_path"]) 89 | 90 | train_data, val_data, test_data = split_indy_net_data( 91 | sample_data, 92 | train_size=common_args["train_size"], 93 | val_size=common_args["val_size"], 94 | test_size=common_args["test_size"], 95 | ) 96 | 97 | # datasets: 98 | trainings_set = IndyDataset( 99 | data=train_data, 100 | cut_probability=common_args["cut_hist_probability"], 101 | min_len=common_args["hist_min_len"], 102 | random_seed=0, 103 | ) 104 | validation_set = IndyDataset( 105 | data=val_data, 106 | cut_probability=common_args["cut_hist_probability"], 107 | min_len=common_args["hist_min_len"], 108 | random_seed=1, 109 | ) 110 | test_set = IndyDataset( 111 | data=test_data, 112 | cut_probability=common_args["cut_hist_probability"], 113 | min_len=common_args["hist_min_len"], 114 | random_seed=2, 115 | ) 116 | 117 | # dataloaders from datasets: 118 | tr_dataloader = DataLoader( 119 | trainings_set, 120 | batch_size=common_args["batch_size"], 121 | shuffle=True, 122 | num_workers=common_args["worker"], 123 | collate_fn=trainings_set.collate_fn, 124 | ) 125 | val_dataloader = DataLoader( 126 | validation_set, 127 | batch_size=common_args["batch_size"], 128 | shuffle=True, 129 | num_workers=common_args["worker"], 130 | collate_fn=validation_set.collate_fn, 131 | ) 132 | ts_dataloader = DataLoader( 133 | test_set, 134 | batch_size=common_args["batch_size"], 135 | shuffle=True, 136 | num_workers=common_args["worker"], 137 | collate_fn=test_set.collate_fn, 138 | ) 139 | 140 | print("Loading data has ended.") 141 | 142 | best_val_loss = np.inf 143 | 144 | for epoch_num in range(common_args["pretrainEpochs"] + common_args["trainEpochs"]): 145 | if epoch_num == 0: 146 | if verbose: 147 | print("Pre-training with MSE loss") 148 | optimizer = optimizer_rmse 149 | elif epoch_num == common_args["pretrainEpochs"]: 150 | if verbose: 151 | print("Training with NLL loss") 152 | optimizer = optimizer_nll 153 | if common_args["save_best"]: 154 | if verbose: 155 | print("Loading best model from pre-training") 156 | if common_args["use_cuda"]: 157 | net.load_model_weights(model_path) 158 | net = net.cuda() 159 | else: 160 | net.load_model_weights(model_path) 161 | 162 | # ---------------- Training ---------------- 163 | net.train_flag = True 164 | net = net.to(net.device) 165 | 166 | # Init progbar 167 | if verbose: 168 | kbar = pkbar.Kbar( 169 | target=len(tr_dataloader), 170 | epoch=epoch_num, 171 | num_epochs=common_args["pretrainEpochs"] + common_args["trainEpochs"], 172 | ) 173 | 174 | # Track train_loss 175 | train_loss = [] 176 | 177 | for i, data in enumerate(tr_dataloader): 178 | # Unpack data 179 | smpl_id, hist, fut, left_bound, right_bound, ego = data 180 | 181 | # Optionally initialize them on GPU 182 | if common_args["use_cuda"]: 183 | smpl_id, hist, fut, left_bound, right_bound, ego = cudanize( 184 | smpl_id, hist, fut, left_bound, right_bound, ego 185 | ) 186 | 187 | # Feed forward 188 | fut_pred = net(hist, left_bound, right_bound, ego=ego) 189 | 190 | if epoch_num < common_args["pretrainEpochs"]: 191 | if common_args["loss_fn"] == "MSE": 192 | loss = torch.mean( 193 | torch.sum(loss_fn(fut_pred[:, :, :2], fut), axis=-1) 194 | ) 195 | else: # WMSE 196 | loss = loss_fn( 197 | np.swapaxes(fut, 0, 1), 198 | np.swapaxes(fut_pred[:, :, :2], 0, 1), 199 | mse_weights, 200 | ) 201 | 202 | if verbose: 203 | kbar.update(i, values=[("MSE", loss)]) 204 | else: 205 | loss, _ = NLL(fut_pred, fut) 206 | if verbose: 207 | kbar.update(i, values=[("NLL", loss)]) 208 | 209 | # Track train loss 210 | train_loss.append(loss.detach().cpu().numpy()) 211 | 212 | # Backprop and update weights 213 | optimizer.zero_grad() 214 | loss.backward() 215 | torch.nn.utils.clip_grad_norm_(net.parameters(), 10) # Gradient clipping 216 | optimizer.step() 217 | 218 | if common_args["debug"]: 219 | break 220 | 221 | writer.add_scalar("training_loss", np.mean(train_loss), epoch_num + 1) 222 | 223 | if trial is not None and epoch_num >= common_args["pretrainEpochs"]: 224 | trial.report(np.mean(train_loss), epoch_num - common_args["pretrainEpochs"]) 225 | # Handle pruning based on the intermediate value. 226 | if trial.should_prune(): 227 | raise optuna.TrialPruned() 228 | 229 | # ---------------- Validation ---------------- 230 | net.train_flag = False 231 | val_loss_list = [] 232 | 233 | for i, data in enumerate(val_dataloader): 234 | # Unpack data 235 | smpl_id, hist, fut, left_bound, right_bound, ego = data 236 | 237 | # Optionally initialize them on GPU 238 | if common_args["use_cuda"]: 239 | smpl_id, hist, fut, left_bound, right_bound, ego = cudanize( 240 | smpl_id, hist, fut, left_bound, right_bound, ego 241 | ) 242 | 243 | # Feed forward 244 | fut_pred = net(hist, left_bound, right_bound, ego=ego) 245 | 246 | if epoch_num < common_args["pretrainEpochs"]: 247 | if common_args["loss_fn"] == "MSE": 248 | loss = torch.mean( 249 | torch.sum(loss_fn(fut_pred[:, :, :2], fut), axis=-1) 250 | ) 251 | else: # WMSE 252 | loss = loss_fn( 253 | np.swapaxes(fut, 0, 1), 254 | np.swapaxes(fut_pred[:, :, :2], 0, 1), 255 | mse_weights, 256 | ) 257 | else: 258 | loss, _ = NLL(fut_pred, fut) 259 | 260 | val_loss_list.append(loss.detach().cpu().numpy()) 261 | 262 | if common_args["debug"]: 263 | break 264 | 265 | val_loss = np.mean(val_loss_list) 266 | if verbose: 267 | kbar.add(1, values=[("val_loss", val_loss)]) 268 | writer.add_scalar("validation_loss", val_loss, epoch_num) 269 | 270 | # learning rate scheduling: 271 | scheduler.step() 272 | 273 | # Save model if val_loss_improved 274 | if common_args["save_best"]: 275 | if val_loss < best_val_loss: 276 | torch.save(net.state_dict(), model_path) 277 | best_val_loss = val_loss 278 | 279 | if common_args["debug"]: 280 | break 281 | 282 | if not common_args["save_best"]: 283 | torch.save(net.state_dict(), model_path) 284 | 285 | # Evaluation 286 | if verbose: 287 | print("Evaluating on test set...") 288 | 289 | # Load best model 290 | if common_args["save_best"]: 291 | if verbose: 292 | print("Loading best model") 293 | if common_args["use_cuda"]: 294 | net.load_model_weights(weights_path=model_path) 295 | net = net.cuda() 296 | else: 297 | net.load_model_weights(weights_path=model_path) 298 | 299 | # Evaluate on test set 300 | metric_dict = evaluate(ts_dataloader, net, common_args, verbose=verbose) 301 | 302 | # # Write to tensorboard 303 | for i in range(len(metric_dict["rmse"])): 304 | writer.add_scalar("rmse_test", metric_dict["rmse"][i], i + 1) 305 | writer.add_scalar("nll_test", metric_dict["nll"][i], i + 1) 306 | writer.add_scalar("nll_std_test", metric_dict["nll_std"][i], i + 1) 307 | writer.add_scalar("mae_test", metric_dict["mae"][i], i + 1) 308 | writer.add_scalar("mae_std_test", metric_dict["mae_std"][i], i + 1) 309 | 310 | writer.add_hparams( 311 | common_args, 312 | {"rmse": np.mean(metric_dict["rmse"]), "nll": np.mean(metric_dict["nll"])}, 313 | ) 314 | 315 | writer.close() 316 | 317 | return -( 318 | np.mean(metric_dict["rmse"]) + np.mean(metric_dict["nll"]) 319 | ) # for maximization in bayes optimzation 320 | 321 | 322 | if __name__ == "__main__": 323 | # Parse arguments 324 | parser = argparse.ArgumentParser() 325 | parser.add_argument( 326 | "--config", type=str, default="train/configs/indy_net/default.json" 327 | ) 328 | parser.add_argument("--debug", action="store_true", default=False) 329 | args = parser.parse_args() 330 | 331 | # Load config 332 | with open(args.config, "r") as f: 333 | common_args = json.load(f) 334 | 335 | # Network Arguments 336 | common_args["use_cuda"] = bool(common_args["gpu"]) 337 | 338 | common_args["model_name"] = os.path.basename(args.config).split(".")[0] 339 | 340 | common_args["debug"] = args.debug 341 | 342 | # Training 343 | main(common_args) 344 | -------------------------------------------------------------------------------- /tools/visualization.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import matplotlib.pyplot as plt 3 | from matplotlib.patches import Ellipse 4 | import matplotlib.transforms as transforms 5 | import numpy as np 6 | import io 7 | 8 | 9 | def visualize( 10 | prediction, 11 | hist, 12 | fut, 13 | left_bound, 14 | right_bound, 15 | ax, 16 | show=True, 17 | save=True, 18 | rmse=None, 19 | ax_no=None, 20 | ): 21 | ax.cla() 22 | if rmse: 23 | ax.text(0, -30, "RMSE: {0:.2f} m".format(rmse), fontsize=18) 24 | if ax_no is not None: 25 | left_bound = left_bound[:, ax_no, :].detach().numpy() 26 | right_bound = right_bound[:, ax_no, :].detach().numpy() 27 | else: 28 | left_bound = left_bound[:, 0, :].detach().numpy() 29 | right_bound = right_bound[:, 0, :].detach().numpy() 30 | if len(fut.shape) > 2: 31 | fut = fut[:, 0, :].detach().numpy() 32 | hist = hist[:, 0, :].detach().numpy() 33 | 34 | if type(prediction) is not np.ndarray: 35 | prediction = prediction.detach().numpy() 36 | 37 | fut_pos_list = prediction[:, 0, :2] 38 | sigma_x = 1 / (prediction[:, 0, 2] + sys.float_info.epsilon) 39 | sigma_y = 1 / (prediction[:, 0, 3] + sys.float_info.epsilon) 40 | rho = prediction[:, 0, 4] 41 | 42 | sigma_cov = np.array( 43 | [ 44 | [sigma_x**2, rho * sigma_x * sigma_y], 45 | [rho * sigma_x * sigma_y, sigma_y**2], 46 | ] 47 | ) 48 | 49 | # Swap axes to shape (50,2,2) 50 | sigma_cov = sigma_cov.swapaxes(0, 2) 51 | sigma_cov = sigma_cov.swapaxes(1, 2) 52 | 53 | draw_with_uncertainty([fut_pos_list], [sigma_cov], ax) 54 | 55 | # ax.plot(fut_pos_list[:,0], fut_pos_list[:,1], 'g-') 56 | ax.plot(hist[:, 0], hist[:, 1], "r--", label="Input - Past coordinates ego vehicle") 57 | ax.plot(fut[:, 0], fut[:, 1], "b--", label="Output - Ground Truth") 58 | ax.plot(left_bound[:, 0], left_bound[:, 1], "k-", label="Input - Track boundaries") 59 | ax.plot(right_bound[:, 0], right_bound[:, 1], "k-") 60 | ax.axis("equal") 61 | ax.set_xlabel("x in m", fontsize=16) 62 | ax.set_ylabel("y in m", fontsize=16) 63 | ax.legend(loc="upper left") 64 | 65 | if show: 66 | plt.pause(1e-3) 67 | 68 | if save: 69 | buf = io.BytesIO() 70 | plt.savefig(buf, format="jpeg") 71 | buf.seek(0) 72 | return buf 73 | 74 | 75 | def visualize_multi(prediction, hist, fut, left_bound, right_bound, axes, ax_no): 76 | """Allows to visualize multiple predictions in subplots. 77 | 78 | Args: 79 | prediction ([type]): [description] 80 | hist ([type]): [description] 81 | fut ([type]): [description] 82 | left_bound ([type]): [description] 83 | right_bound ([type]): [description] 84 | axes ([type]): [description] 85 | ax_no ([type]): [description] 86 | """ 87 | 88 | if ax_no < len(axes): 89 | axes[ax_no].cla() 90 | 91 | visualize( 92 | prediction, hist, fut, left_bound, right_bound, axes[ax_no], ax_no=ax_no 93 | ) 94 | 95 | 96 | def confidence_ellipse( 97 | mu, 98 | cov, 99 | ax, 100 | n_std=3.0, 101 | facecolor="None", 102 | edgecolor="fuchsia", 103 | linestyle="--", 104 | label_ell=None, 105 | **kwargs 106 | ): 107 | """ 108 | Create a plot of the covariance confidence ellipse of *x* and *y*. 109 | 110 | Parameters 111 | ---------- 112 | x, y : array-like, shape (n, ) 113 | Input data. 114 | 115 | ax : matplotlib.axes.Axes 116 | The axes object to draw the ellipse into. 117 | 118 | n_std : float 119 | The number of standard deviations to determine the ellipse's radiuses. 120 | 121 | **kwargs 122 | Forwarded to `~matplotlib.patches.Ellipse` 123 | 124 | Returns 125 | ------- 126 | matplotlib.patches.Ellipse 127 | """ 128 | 129 | mu_x = mu[0] 130 | mu_y = mu[1] 131 | 132 | pearson = cov[0, 1] / (np.sqrt(cov[0, 0] * cov[1, 1]) + sys.float_info.epsilon) 133 | # Using a special case to obtain the eigenvalues of this 134 | # two-dimensionl dataset. 135 | ell_radius_x = np.sqrt(1 + pearson) 136 | ell_radius_y = np.sqrt(1 - pearson) 137 | ellipse = Ellipse( 138 | (0, 0), 139 | width=ell_radius_x * 2, 140 | height=ell_radius_y * 2, 141 | facecolor=facecolor, 142 | edgecolor=edgecolor, 143 | linestyle=linestyle, 144 | alpha=0.5, 145 | **kwargs 146 | ) 147 | 148 | # Calculating the stdandard deviation of x from 149 | # the squareroot of the variance and multiplying 150 | # with the given number of standard deviations. 151 | scale_x = np.sqrt(cov[0, 0]) * n_std 152 | 153 | # calculating the stdandard deviation of y ... 154 | scale_y = np.sqrt(cov[1, 1]) * n_std 155 | 156 | transf = ( 157 | transforms.Affine2D() 158 | .rotate_deg(45) 159 | .scale(scale_x, scale_y) 160 | .translate(mu_x, mu_y) 161 | ) 162 | 163 | ellipse.set_transform(transf + ax.transData) 164 | ax.add_patch(ellipse) 165 | if label_ell is not None: 166 | ellipse.set(label=label_ell) 167 | return None 168 | 169 | 170 | def draw_with_uncertainty(fut_pos_list, fut_cov_list, ax): 171 | fut_pos_list = np.array(fut_pos_list) 172 | fut_cov_list = np.array(fut_cov_list) 173 | 174 | if fut_pos_list.shape[1] < fut_pos_list.shape[2]: 175 | fut_pos_list = np.swapaxes(fut_pos_list, 1, 2) 176 | 177 | if fut_pos_list.shape[1] > 50: 178 | fut_pos_list = fut_pos_list[:, :50, :] 179 | 180 | for i, fut_pos in enumerate(fut_pos_list): 181 | ax.plot( 182 | fut_pos[:, 0], 183 | fut_pos[:, 1], 184 | "*c", 185 | markersize=2, 186 | alpha=0.8, 187 | zorder=15, 188 | label="Output - Prediction", 189 | ) 190 | for j, pos in enumerate(fut_pos): 191 | confidence_ellipse( 192 | pos, fut_cov_list[i][j], ax, n_std=3.0, facecolor="yellow" 193 | ) 194 | for j, pos in enumerate(fut_pos): 195 | confidence_ellipse( 196 | pos, fut_cov_list[i][j], ax, n_std=2.0, facecolor="orange" 197 | ) 198 | for j, pos in enumerate(fut_pos): 199 | confidence_ellipse(pos, fut_cov_list[i][j], ax, n_std=1.0, facecolor="red") 200 | 201 | 202 | def visualize_objects(ax, objector, n_std=3, show=True): 203 | if not objector.tracker.bool_detection_input: 204 | return 205 | 206 | mode = objector.visualization 207 | 208 | right_boundary = objector.right_bound 209 | left_boundary = objector.left_bound 210 | 211 | ax.cla() 212 | ax.plot(right_boundary[:, 0], right_boundary[:, 1], "k-") 213 | ax.plot(left_boundary[:, 0], left_boundary[:, 1], "k-", label="Track boundaries") 214 | 215 | xx = [] 216 | yy = [] 217 | 218 | col_list = [ 219 | "green", 220 | "blue", 221 | "magenta", 222 | "yellow", 223 | "cyan", 224 | "wheat", 225 | "lime", 226 | "silver", 227 | "olive", 228 | "coral", 229 | "beige", 230 | "pink", 231 | ] 232 | 233 | col_iter = iter(col_list) 234 | 235 | if mode == "filter": 236 | window_plot_size = 4 237 | for obj_id, vals in objector.tracker.delay_visz.items(): 238 | try: 239 | col_obj = next(col_iter) 240 | except Exception: 241 | col_iter = iter(col_list) 242 | col_obj = next(col_iter) 243 | 244 | label_est = "estimate, ID = {:d}".format(int(obj_id)) 245 | 246 | ax.plot( 247 | list(objector.tracker.observation_storage[obj_id]["x"])[ 248 | len(vals["x_est"]) : 249 | ], 250 | list(objector.tracker.observation_storage[obj_id]["y"])[ 251 | len(vals["y_est"]) : 252 | ], 253 | "-", 254 | label=None, 255 | color=col_obj, 256 | ) 257 | ax.plot(vals["x_est"], vals["y_est"], "-.", label=label_est, color=col_obj) 258 | ax.plot(vals["x_est"][-1], vals["y_est"][-1], "x", color=col_obj) 259 | 260 | # Take forelast position for residuum, as final prediction step was conducted for estimation 261 | if len(vals["x_upt"]) > 1: 262 | dx = vals["x_est"][0] - vals["x_upt"][-2] 263 | dy = vals["y_est"][0] - vals["y_upt"][-2] 264 | label_upt = ( 265 | "update, ID = {:d}, $\Delta$x = {:.02f}m, $\Delta$y = {:.02f}m".format( 266 | int(obj_id), dx, dy 267 | ) 268 | ) 269 | ax.plot(vals["x_upt"], vals["y_upt"], "-", label=label_upt, color=col_obj) 270 | ax.plot(vals["x_upt"][0], vals["y_upt"][0], "x", color=col_obj) 271 | ax.plot(vals["x_upt"][-1], vals["y_upt"][-1], "x", color=col_obj) 272 | 273 | xx.append(vals["x_upt"][-1]) 274 | yy.append(vals["y_upt"][-1]) 275 | xx.append(vals["x_upt"][0]) 276 | yy.append(vals["y_upt"][0]) 277 | else: 278 | if mode.find("observation") >= 0: 279 | plot_dict = objector.tracker.observation_storage 280 | elif mode == "prediction": 281 | plot_dict = objector.tracker.pred_dict 282 | 283 | for obj_id, obs in plot_dict.items(): 284 | if mode != "prediction": 285 | obs = { 286 | var: [obj_state[j] for obj_state in obs["state"]] 287 | for j, var in enumerate(objector.tracker._state_variables) 288 | } 289 | 290 | if obj_id == "ego": 291 | col_obj = "r" 292 | obs["cov"] /= 100.0 293 | else: 294 | try: 295 | col_obj = next(col_iter) 296 | except Exception: 297 | col_iter = iter(col_list) 298 | col_obj = next(col_iter) 299 | 300 | if obs.get("valid", True): 301 | xx.append(obs["x"][0]) 302 | yy.append(obs["y"][0]) 303 | if "cov" in obs.keys(): 304 | P = obs["cov"][-1] 305 | spec_idx = -1 306 | ax.plot(obs["x"][spec_idx], obs["y"][spec_idx], "x", color=col_obj) 307 | else: 308 | filter = objector.tracker.obj_filters.get(obj_id, None) 309 | if filter is None: 310 | P = np.eye(2) * 0.01 # Just template for ego object 311 | else: 312 | P = filter.P[:2, :2] 313 | if obj_id == "ego": 314 | ll_obj = None 315 | else: 316 | p_norm = np.linalg.norm(P) 317 | ll_obj = "ID: {}, |P|={:.2f}".format(obj_id, p_norm) 318 | spec_idx = 0 319 | ax.plot(obs["x"], obs["y"], label=ll_obj, color=col_obj) 320 | ax.plot(obs["x"][0], obs["y"][0], "x", color=col_obj) 321 | 322 | if mode == "prediction": 323 | ax.plot(obs["x_rail"], obs["y_rail"], "--", color=col_obj) 324 | window_plot_size = 300 325 | 326 | mu = [obs["x"][spec_idx], obs["y"][spec_idx]] 327 | confidence_ellipse( 328 | mu=mu, 329 | cov=P, 330 | ax=ax, 331 | n_std=n_std, 332 | facecolor="g", 333 | alpha=0.3, 334 | ) 335 | else: 336 | xx.append(obs["x"]) 337 | yy.append(obs["y"]) 338 | ll_obj = "ID: {} - invalid".format(obj_id) 339 | ax.plot(obs["x"], obs["y"], "x", label=ll_obj, color=col_obj) 340 | 341 | bool_limits_ego = bool( 342 | mode.find("glob") < 0 and mode.find("pred") < 0 and mode.find("filter") < 0 343 | ) 344 | 345 | if "x" in objector.tracker.ego_state.keys(): 346 | ax.plot( 347 | objector.tracker.ego_state["x"], 348 | objector.tracker.ego_state["y"], 349 | "o", 350 | color="r", 351 | markersize=7, 352 | label="Ego-state", 353 | ) 354 | if bool_limits_ego: 355 | x_min = objector.tracker.ego_state["x"] - window_plot_size 356 | x_max = objector.tracker.ego_state["x"] + window_plot_size 357 | y_min = objector.tracker.ego_state["y"] - window_plot_size 358 | y_max = objector.tracker.ego_state["y"] + window_plot_size 359 | elif mode.find("filter") < 0 and len(xx) > 0: 360 | xx.append(objector.tracker.ego_state["x"]) 361 | yy.append(objector.tracker.ego_state["y"]) 362 | 363 | try: 364 | _ = x_min # check if x_min is available from bool_limit_ego 365 | except Exception: 366 | try: # check if xx is avaible as list from all objects 367 | # max_sensor_range = max([val[2] for val in objector.measure_spec.values()]) 368 | x_min = np.min(xx) - window_plot_size 369 | x_max = np.max(xx) + window_plot_size 370 | y_min = np.min(yy) - window_plot_size 371 | y_max = np.max(yy) + window_plot_size 372 | except Exception: 373 | return # do not plot anything 374 | 375 | ax.set_xlim([x_min, x_max]) 376 | ax.set_ylim([y_min, y_max]) 377 | ax.set_aspect("equal", adjustable="box", anchor="C") 378 | ax.set_xlabel("x in m", fontsize=16) 379 | ax.set_ylabel("y in m", fontsize=16) 380 | ax.legend(loc="upper left") 381 | ax.grid(True) 382 | 383 | if show: 384 | plt.pause(1e-3) 385 | -------------------------------------------------------------------------------- /mix_net/mix_net/src/rulebased_interaction.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import numpy as np 4 | 5 | repo_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 6 | sys.path.append(repo_path) 7 | 8 | from utils.geometry import ( 9 | check_collision, 10 | check_collision_rect, 11 | get_heading, 12 | ) 13 | from utils.sort import quicksort, sort_collisions 14 | from src.overtake_decision import OvertakeDecisionMaker 15 | 16 | 17 | class RuleBasedInteraction(object): 18 | def __init__(self, all_params, logger): 19 | self.params = all_params["INTERACTION_PARAMS"] 20 | self.params["track_path"] = all_params["track_path"] 21 | self.params["sampling_frequency"] = all_params["MODEL_PARAMS"][ 22 | "sampling_frequency" 23 | ] 24 | self.logger = logger 25 | 26 | self.define_safety_distances() 27 | 28 | # Collision Function 29 | if self.params["collision_check"] == "euclidean": 30 | self.collision_fn = self.detect_collisions 31 | else: 32 | self.collision_fn = self.detect_collisions_rectangle 33 | 34 | # Overtake Decision Maker 35 | self.ot_decision_maker = OvertakeDecisionMaker( 36 | track_path=self.params["track_path"] 37 | ) 38 | 39 | def define_safety_distances(self): 40 | """Define safety distance in lateral and longitudinal direction.""" 41 | self.params["lat_safety_dist_m"] = ( 42 | self.params["lat_veh_half_m"] + self.params["lat_safety_m"] 43 | ) 44 | self.params["long_safety_dist_m"] = ( 45 | self.params["long_veh_half_m"] + self.params["long_safety_m"] 46 | ) 47 | self.params["max_overlap_dist_m"] = np.sqrt( 48 | self.params["lat_veh_half_m"] ** 2 + self.params["long_veh_half_m"] ** 2 49 | ) + np.sqrt( 50 | self.params["lat_safety_dist_m"] ** 2 51 | + self.params["long_safety_dist_m"] ** 2 52 | ) 53 | 54 | def apply_to_predictions(self, pred_dict): 55 | """Modify predictions that collide with each other according to the race rules.""" 56 | # Loop n-times over according to params 57 | self.pred_dict = pred_dict 58 | for iteration in range(self.params["no_iterations"]): 59 | # Create a list for collisions 60 | collision_list = self.collision_fn() 61 | 62 | # If there is a collision 63 | if len(collision_list) > 0: 64 | # Get a priority for all vehicles involved in collisions 65 | involved_predictions = [ 66 | collision["pred_ids"] for collision in collision_list 67 | ] 68 | involved_predictions_set = set( 69 | [item for sublist in involved_predictions for item in sublist] 70 | ) 71 | 72 | # Get the priority list 73 | priority_list = self.get_race_order(involved_predictions_set) # noqa 74 | 75 | # Sort collision list so that the one with higehst prio is handled first 76 | sorted_collision_list = sort_collisions(collision_list, priority_list) 77 | 78 | # Avoid the collisions 79 | for collision in sorted_collision_list: 80 | # Priority on ego vehicle in last iteration if activated 81 | if ( 82 | self.params["priority_on_ego"] 83 | and (iteration + 1) == self.params["no_iterations"] 84 | ): 85 | # Only adjust those predictions where ego is involved as leading vehicle 86 | if ( 87 | self.pred_dict[collision["leading_pred_id"]]["vehicle_id"] 88 | != "ego" 89 | ): 90 | continue 91 | 92 | # Dont adjust static predictions 93 | if ( 94 | self.pred_dict[collision["following_pred_id"]][ 95 | "prediction_type" 96 | ] 97 | != "static" 98 | ): 99 | self.adjust_prediction(collision) 100 | else: 101 | self.logger.info( 102 | "Iteration {}: Prediction with IDs {} (following) and {} (leading) was not adjusted. Following object is static".format( 103 | iteration, 104 | collision["following_pred_id"], 105 | collision["leading_pred_id"], 106 | ) 107 | ) 108 | 109 | return self.pred_dict 110 | 111 | def detect_collisions(self): 112 | """Detect collisions between all trajectory predictions based on euclidean distance. 113 | 114 | Returns: 115 | [list]: [list of dicts where every dict describes a collision with keys 116 | * pred_ids 117 | * time_step] 118 | 119 | """ 120 | collision_list = [] 121 | # Check all predicted trajectories for collisions 122 | pred_keys = [key for key, value in self.pred_dict.items() if value["valid"]] 123 | # Combine every possible pair only once 124 | for count, pred_id_1 in enumerate(pred_keys): 125 | for pred_id_2 in pred_keys[count + 1 :]: 126 | positions_1 = np.vstack( 127 | (self.pred_dict[pred_id_1]["x"], self.pred_dict[pred_id_1]["y"]) 128 | ).T 129 | positions_2 = np.vstack( 130 | (self.pred_dict[pred_id_2]["x"], self.pred_dict[pred_id_2]["y"]) 131 | ).T 132 | 133 | for ts in range(min(positions_1.shape[0], positions_2.shape[0])): 134 | # Check every point for collisions 135 | if check_collision( 136 | positions_1[ts, :], 137 | positions_2[ts, :], 138 | approx_radius=self.params["approx_radius"], 139 | ): 140 | # Collision detected 141 | collision_dict = { 142 | "pred_ids": [pred_id_1, pred_id_2], 143 | "time_step": ts, 144 | } 145 | collision_list.append(collision_dict) 146 | self.logger.info( 147 | "Collision detected between predictions with IDs {} ({}) and {} ({}) at timestep {}".format( 148 | pred_id_1, 149 | positions_1[ts], 150 | pred_id_2, 151 | positions_2[ts], 152 | ts, 153 | ) 154 | ) 155 | 156 | # Only temporal first collision needed 157 | break 158 | 159 | return collision_list 160 | 161 | def detect_collisions_rectangle(self): 162 | """Detect rectangle collisions between all trajectory predictions. 163 | 164 | Returns: 165 | [list]: [list of dicts where every dict describes a collision with keys 166 | * pred_ids 167 | * time_step] 168 | 169 | """ 170 | collision_list = [] 171 | collided_list = [] 172 | # Check all predicted trajectories for collisions 173 | pred_keys = [key for key, value in self.pred_dict.items() if value["valid"]] 174 | # Combine every possible pair only once 175 | # Order is important as only pred_id_1 is assigned with safety box 176 | for pred_id_1 in pred_keys: 177 | for pred_id_2 in pred_keys: 178 | if pred_id_1 == pred_id_2: 179 | continue 180 | 181 | # Skip if collison was already detected 182 | if sorted([pred_id_1, pred_id_2]) in collided_list: 183 | continue 184 | 185 | poses_1 = np.stack( 186 | ( 187 | self.pred_dict[pred_id_1]["x"], 188 | self.pred_dict[pred_id_1]["y"], 189 | self.pred_dict[pred_id_1]["heading"], 190 | ), 191 | axis=1, 192 | ) 193 | poses_2 = np.stack( 194 | ( 195 | self.pred_dict[pred_id_2]["x"], 196 | self.pred_dict[pred_id_2]["y"], 197 | self.pred_dict[pred_id_2]["heading"], 198 | ), 199 | axis=1, 200 | ) 201 | 202 | for ts in range(min(poses_1.shape[0], poses_2.shape[0])): 203 | # Check every point for collisions 204 | if ( 205 | np.linalg.norm(poses_1[ts, :2] - poses_1[ts, :2]) 206 | > self.params["max_overlap_dist_m"] 207 | ): 208 | continue 209 | 210 | bool_rectangle = check_collision_rect( 211 | poses_1[ts, :], 212 | poses_2[ts, :], 213 | lat_veh_half_m=self.params["lat_veh_half_m"], 214 | long_veh_half_m=self.params["long_veh_half_m"], 215 | lat_safety_dist_m=self.params["lat_safety_dist_m"], 216 | long_safety_dist_m=self.params["long_safety_dist_m"], 217 | ) 218 | 219 | if bool_rectangle: 220 | # Collision detected 221 | collided_list.append(sorted([pred_id_1, pred_id_2])) 222 | collision_dict = { 223 | "pred_ids": [pred_id_1, pred_id_2], 224 | "time_step": ts, 225 | } 226 | collision_list.append(collision_dict) 227 | self.logger.info( 228 | "Collision detected between predictions with IDs " 229 | "{} ({}) and {} ({}) at timestep {}".format( 230 | pred_id_1, 231 | poses_1[ts], 232 | pred_id_2, 233 | poses_2[ts], 234 | ts, 235 | ) 236 | ) 237 | 238 | # Only temporal first collision needed 239 | break 240 | 241 | return collision_list 242 | 243 | def compare_positions(self, id1, id2): 244 | """Return True if id 1 is in front of id2, else return false.""" 245 | # Call predictions for ids 246 | pred_1_t0 = np.array([self.pred_dict[id1]["x"][0], self.pred_dict[id1]["y"][0]]) 247 | pred_2_t0 = np.array([self.pred_dict[id2]["x"][0], self.pred_dict[id2]["y"][0]]) 248 | pred_2_t1 = np.array([self.pred_dict[id2]["x"][1], self.pred_dict[id2]["y"][1]]) 249 | 250 | follower_vect = pred_2_t1 - pred_2_t0 251 | follower_to_leader = pred_1_t0 - pred_2_t0 252 | 253 | # checking the scalar product, to determine whether id1 is 254 | # in front of id2: 255 | return (follower_vect @ follower_to_leader) > 0 256 | 257 | def get_race_order(self, prediction_ids): 258 | """Order a set of prediction IDs by their race order (position in the race). 259 | 260 | Args: 261 | prediction_ids ([set]): [A set of prediction ids] 262 | """ 263 | prediction_ids = list(prediction_ids) 264 | 265 | sorted_prediction_ids = quicksort(prediction_ids, self.compare_positions) 266 | 267 | return sorted_prediction_ids 268 | 269 | def adjust_prediction(self, collision): 270 | """Adjust the prediction according to a given collision. 271 | 272 | Args: 273 | collision ([dict]): [collsion dict] 274 | """ 275 | following_pred = self.pred_dict[collision["following_pred_id"]] 276 | leading_pred = self.pred_dict[collision["leading_pred_id"]] 277 | coll_ts = int(collision["time_step"]) 278 | 279 | # Get the overtake decision (left or right): 280 | ot_dec = self.ot_decision_maker.get_overtake_direction( 281 | pos_leading=np.array([leading_pred["x"][0], leading_pred["y"][0]]), 282 | pos_following=np.array([following_pred["x"][0], following_pred["y"][0]]), 283 | overtake_margin=self.params["lat_overtake_dist"], 284 | ) 285 | 286 | # getting the adjusted prediction: 287 | following_pred_adj, max_adjustment = self.ot_decision_maker.adjust_prediction( 288 | leading_pred=np.vstack((leading_pred["x"], leading_pred["y"])), 289 | following_pred=np.vstack((following_pred["x"], following_pred["y"])), 290 | ts=coll_ts, 291 | decision=ot_dec, 292 | approx_radius=self.params["approx_radius"], 293 | ot_length_ts=int( 294 | self.params["lanechange_time"] * self.params["sampling_frequency"] 295 | ), 296 | min_overtake_dist=self.params["lat_overtake_dist"], 297 | logger=self.logger, 298 | ) 299 | 300 | # for logging: 301 | # adj_vect = following_pred_adj[:, coll_ts] - np.array( 302 | # [following_pred["x"][coll_ts], following_pred["y"][coll_ts]] 303 | # ) 304 | # adj_dist = np.linalg.norm(adj_vect) 305 | 306 | # Adjust the predictions in pred_dict 307 | self.pred_dict[collision["following_pred_id"]]["x"] = following_pred_adj[0, :] 308 | self.pred_dict[collision["following_pred_id"]]["y"] = following_pred_adj[1, :] 309 | 310 | # recalculate the heading: 311 | self.pred_dict[collision["following_pred_id"]]["heading"] = get_heading( 312 | following_pred_adj.T 313 | ) 314 | 315 | # Log message 316 | self.logger.info( 317 | "Prediction with ID {} adjusted to {} maneuver with lat distance of {:.2f}".format( 318 | collision["following_pred_id"], 319 | ot_dec, 320 | max_adjustment, 321 | ) 322 | ) 323 | -------------------------------------------------------------------------------- /mix_net/mix_net/utils/map_utils.py: -------------------------------------------------------------------------------- 1 | import matplotlib 2 | import numpy as np 3 | from scipy.signal import savgol_filter 4 | 5 | 6 | def line_intersection(line1, line2): 7 | xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0]) 8 | ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1]) 9 | 10 | def det(a, b): 11 | return a[0] * b[1] - a[1] * b[0] 12 | 13 | div = det(xdiff, ydiff) 14 | if div == 0: 15 | raise Exception("lines do not intersect") 16 | 17 | d = (det(*line1), det(*line2)) 18 | x = det(d, xdiff) / div 19 | y = det(d, ydiff) / div 20 | return x, y 21 | 22 | 23 | def get_arc(boundary): 24 | """Calculate a list of cummulated arc lenghts of given x,y points. 25 | 26 | Args: 27 | boundary [np.array with shape=(N, 2)]: the points for which the arc lengths are calculated. 28 | 29 | Returns: 30 | np.array with shape=(N,) the arc lengths corresponding to the points in boundary. 31 | """ 32 | 33 | dists = np.linalg.norm((boundary[1:, :] - boundary[:-1, :]), axis=1) 34 | 35 | return np.hstack((np.array([0]), np.cumsum(dists))) 36 | 37 | 38 | def get_arc_length(track_array): 39 | """Calculate cumulative arc length along track array. 40 | track_array: np.array, shape = (n, 2) 41 | arc_length: np.array, shape = (n) 42 | """ 43 | arc_length = np.concatenate( 44 | [np.zeros(1), np.cumsum(np.linalg.norm(np.diff(track_array, axis=0), axis=1))] 45 | ) 46 | return arc_length 47 | 48 | 49 | def get_track_paths( 50 | track_path: str, 51 | bool_track_width: bool = False, 52 | bool_raceline: bool = False, 53 | bool_all_kinematics: bool = False, 54 | ) -> tuple: 55 | """Reads the map data from the unified map file.""" 56 | 57 | ( 58 | refline, 59 | t_width_right, 60 | t_width_left, 61 | normvec_normalized, 62 | alpha_dist, 63 | s_rl, 64 | _, 65 | _, 66 | _, 67 | _, 68 | _, 69 | s_refline, 70 | psi_refline, 71 | kappa_refline, 72 | _, 73 | ) = import_global_trajectory_csv(import_path=track_path) 74 | 75 | x_intp = 0.999 76 | close_bound = x_intp * (refline[0, :] - refline[-1, :]) + refline[-1, :] 77 | refline = np.vstack([refline, close_bound]) 78 | 79 | close_bound = ( 80 | x_intp * (normvec_normalized[0, :] - normvec_normalized[-1, :]) 81 | + normvec_normalized[-1, :] 82 | ) 83 | normvec_normalized = np.vstack([normvec_normalized, close_bound]) 84 | 85 | close_bound = x_intp * (t_width_right[0] - t_width_right[-1]) + t_width_right[-1] 86 | t_width_right = np.append(t_width_right, close_bound) 87 | 88 | close_bound = x_intp * (t_width_left[0] - t_width_left[-1]) + t_width_left[-1] 89 | t_width_left = np.append(t_width_left, close_bound) 90 | 91 | bound_right = refline + normvec_normalized * np.expand_dims(t_width_right, 1) 92 | bound_left = refline - normvec_normalized * np.expand_dims(t_width_left, 1) 93 | 94 | track_width = t_width_right + t_width_left 95 | if bool_track_width: 96 | return (s_rl, refline, bound_right, bound_left, track_width) 97 | elif bool_raceline: 98 | close_bound = x_intp * (alpha_dist[0] - alpha_dist[-1]) + alpha_dist[-1] 99 | alpha_dist = np.append(alpha_dist, close_bound) 100 | raceline = refline + normvec_normalized * alpha_dist[:, np.newaxis] 101 | return (s_refline, refline, bound_right, bound_left, raceline) 102 | elif bool_all_kinematics: 103 | close_bound = ( 104 | x_intp * (kappa_refline[0] - kappa_refline[-1]) + kappa_refline[-1] 105 | ) 106 | kappa_refline = np.append(kappa_refline, close_bound) 107 | close_bound = x_intp * (psi_refline[0] - psi_refline[-1]) + psi_refline[-1] 108 | psi_refline = np.append(psi_refline, close_bound) 109 | return ( 110 | s_refline, 111 | refline, 112 | bound_right, 113 | bound_left, 114 | track_width, 115 | psi_refline, 116 | kappa_refline, 117 | ) 118 | else: 119 | return (s_refline, refline, bound_right, bound_left) 120 | 121 | 122 | def import_global_trajectory_csv(import_path: str) -> tuple: 123 | """Import global trajectory 124 | 125 | :param import_path: path to the csv file containing the optimal global trajectory 126 | :type import_path: str 127 | :return: - xy_refline: x and y coordinate of reference-line 128 | 129 | - t_width_right: width to right track bound at given reference-line coordinates in meters 130 | 131 | - t_width_left: width to left track bound at given reference-line coordinates in meters 132 | 133 | - normvec_normalized: x and y components of normalized normal vector at given reference-line coordinates 134 | 135 | - alpha_dist: distance from optimal racing-line to reference-line at given reference-line coordinates 136 | 137 | - s_refline: s-coordinate of reference-line at given reference-line coordinates 138 | 139 | - psi_refline: heading at given reference-line coordinates 140 | 141 | - kappa_refline: curvature at given reference-line coordinates 142 | 143 | - dkappa_refline: derivative of curvature at given reference-line coordinates 144 | 145 | - s_rl: s-coordinate of racing-line at given racing-line coordinates 146 | 147 | - vel_rl: velocity at given racing-line coordinates 148 | 149 | - acc_rl: acceleration at given racing-line coordinates 150 | 151 | - psi_rl: heading at given racing-line coordinates 152 | 153 | - kappa_rl: curvature at given racing-line coordinates 154 | 155 | - banking: banking at given racling-line coordinates 156 | :rtype: tuple 157 | """ 158 | 159 | # ------------------------------------------------------------------------------------------------------------------ 160 | # IMPORT DATA ------------------------------------------------------------------------------------------------------ 161 | # ------------------------------------------------------------------------------------------------------------------ 162 | 163 | # load data from csv file (closed; assumed order listed below) 164 | # x_ref_m, y_ref_m, width_right_m, width_left_m, x_normvec_m, y_normvec_m, alpha_m, s_racetraj_m, 165 | # psi_racetraj_rad, kappa_racetraj_radpm, vx_racetraj_mps, ax_racetraj_mps2 166 | csv_data_temp = np.loadtxt(import_path, delimiter=";") 167 | 168 | # get reference-line 169 | xy_refline = csv_data_temp[:-1, 0:2] 170 | 171 | # get distances to right and left bound from reference-line 172 | t_width_right = csv_data_temp[:-1, 2] 173 | t_width_left = csv_data_temp[:-1, 3] 174 | 175 | # get normalized normal vectors 176 | normvec_normalized = csv_data_temp[:-1, 4:6] 177 | 178 | # get racing-line alpha 179 | alpha_dist = csv_data_temp[:-1, 6] 180 | 181 | # get racing-line s-coordinate 182 | s_rl = csv_data_temp[:, 7] 183 | 184 | # get heading at racing-line points 185 | psi_rl = csv_data_temp[:-1, 8] 186 | 187 | # get kappa at racing-line points 188 | kappa_rl = csv_data_temp[:-1, 9] 189 | 190 | # get velocity at racing-line points 191 | vel_rl = csv_data_temp[:-1, 10] 192 | 193 | # get acceleration at racing-line points 194 | acc_rl = csv_data_temp[:-1, 11] 195 | 196 | # get banking 197 | banking = csv_data_temp[:-1, 12] 198 | 199 | # get reference-line s-coordinate 200 | s_refline = csv_data_temp[:, 13] 201 | 202 | # get heading at reference-line points 203 | psi_refline = csv_data_temp[:-1, 14] 204 | 205 | # get curvature at reference-line points 206 | kappa_refline = csv_data_temp[:-1, 15] 207 | 208 | # get derivative of curvature at reference-line points 209 | dkappa_refline = csv_data_temp[:-1, 16] 210 | 211 | return ( 212 | xy_refline, 213 | t_width_right, 214 | t_width_left, 215 | normvec_normalized, 216 | alpha_dist, 217 | s_rl, 218 | psi_rl, 219 | kappa_rl, 220 | vel_rl, 221 | acc_rl, 222 | banking, 223 | s_refline, 224 | psi_refline, 225 | kappa_refline, 226 | dkappa_refline, 227 | ) 228 | 229 | 230 | def get_glob_raceline( 231 | loctraj_param_path, bool_vel_const=True, velocity=100.0, vel_scale=1.0 232 | ): 233 | """load data from csv files.""" 234 | ( 235 | refline, 236 | _, 237 | _, 238 | normvec_normalized, 239 | alpha_mincurv, 240 | s_rl, 241 | psi_rl, 242 | _, 243 | vel_rl, 244 | acc_rl, 245 | _, 246 | _, 247 | _, 248 | _, 249 | _, 250 | ) = import_global_trajectory_csv(import_path=loctraj_param_path) 251 | 252 | # get race line 253 | raceline = refline + normvec_normalized * alpha_mincurv[:, np.newaxis] 254 | 255 | x_intp = 0.999 256 | close_bound = x_intp * (raceline[0, :] - raceline[-1, :]) + raceline[-1, :] 257 | raceline = np.vstack([raceline, close_bound]) 258 | 259 | close_bound = x_intp * (psi_rl[0] - psi_rl[-1]) + psi_rl[-1] 260 | psi_rl = np.append(psi_rl, close_bound) 261 | 262 | close_bound = x_intp * (vel_rl[0] - vel_rl[-1]) + vel_rl[-1] 263 | vel_rl = np.append(vel_rl, close_bound) 264 | 265 | close_bound = x_intp * (acc_rl[0] - acc_rl[-1]) + acc_rl[-1] 266 | acc_rl = np.append(acc_rl, close_bound) 267 | 268 | if bool_vel_const: 269 | vel_rl = np.ones(vel_rl.shape) * velocity 270 | acc_rl = np.zeros(vel_rl.shape) 271 | else: 272 | vel_rl *= vel_scale 273 | acc_rl *= vel_scale 274 | 275 | psi_rl = remove_psi_step(psi_rl) 276 | 277 | dpsi_rl = get_dpsi(psi_rl, s_rl, vel_rl) 278 | 279 | return list((s_rl, vel_rl, raceline, psi_rl, dpsi_rl, acc_rl)) 280 | 281 | 282 | def remove_psi_step(psi_rl): 283 | step = np.diff(psi_rl) 284 | max_step = np.argmax(abs(step)) 285 | if step[max_step] > 0: 286 | psi_rl[max_step + 1 :] -= 2 * np.pi 287 | else: 288 | psi_rl[max_step + 1 :] += 2 * np.pi 289 | if np.min(psi_rl) < -6.5: 290 | psi_rl += 2 * np.pi 291 | return psi_rl 292 | 293 | 294 | def get_dpsi(psi_rl, s_rl, vel_rl): 295 | if np.max(vel_rl) == 0.0: 296 | dts = np.diff(s_rl) / 0.01 297 | else: 298 | dts = np.diff(s_rl) / vel_rl[:-1] 299 | delta_psi = np.diff(psi_rl) 300 | delta_psi = np.append(delta_psi, delta_psi[-1]) 301 | dts = np.append(dts, dts[-1]) 302 | for j in range(len(delta_psi)): 303 | if delta_psi[j] > np.pi / 2.0: 304 | delta_psi[j] -= 2 * np.pi 305 | elif delta_psi[j] < -np.pi / 2.0: 306 | delta_psi[j] += 2 * np.pi 307 | 308 | delta_psi /= dts 309 | dpsi_rl = savgol_filter(delta_psi, 5, 2, 0) 310 | 311 | return dpsi_rl 312 | 313 | 314 | def get_track_kinematics( 315 | path_name, track_path, velocity=100, vel_scale=1.0, bool_get_yaw_curv=False 316 | ): 317 | """Get kinematics for a single path of the track 318 | 319 | Kinematics is the list: [arc_length, vel_rl, raceline, psi_rl, dpsi_rl, acc_rl] 320 | Possible Inputs: 321 | track_name = 'trackboundary_right', 'trackboundary_left', 'centerline', 'glob_optimal_raceline', 'raceline' 322 | """ 323 | 324 | if "raceline" in path_name and "optimal" in path_name: 325 | return get_glob_raceline(track_path, bool_vel_const=False, vel_scale=vel_scale) 326 | elif path_name == "raceline": 327 | return get_glob_raceline(track_path, bool_vel_const=True, velocity=velocity) 328 | else: 329 | tracks_arrays = get_track_paths(track_path, bool_all_kinematics=True) 330 | 331 | if bool_get_yaw_curv: 332 | return tracks_arrays[5], tracks_arrays[6] 333 | 334 | if "left" in path_name: 335 | idx = 3 336 | elif "right" in path_name: 337 | idx = 2 338 | else: # centerline 339 | idx = 1 340 | 341 | # use heading from refline 342 | psi = tracks_arrays[5] 343 | 344 | arc_length = tracks_arrays[0] 345 | line_path = tracks_arrays[idx] 346 | 347 | vel_rl = np.ones(len(line_path)) * velocity 348 | acc_rl = np.zeros(len(line_path)) 349 | 350 | psi = remove_psi_step(psi) 351 | 352 | dpsi_rl = get_dpsi(psi, arc_length, vel_rl) 353 | 354 | return [arc_length, vel_rl, line_path, psi, dpsi_rl, acc_rl] 355 | 356 | 357 | if __name__ == "__main__": 358 | import matplotlib.pyplot as plt 359 | 360 | matplotlib.use("TkAgg") 361 | 362 | track_path = "mix_net/mix_net/data/map/traj_ltpl_cl_IMS_GPS.csv" 363 | 364 | track_list = get_track_kinematics("centerline", track_path=track_path, velocity=100) 365 | plt.plot(track_list[2][:, 0], track_list[2][:, 1]) 366 | track_list = get_track_kinematics("raceline", track_path=track_path, velocity=100) 367 | plt.plot(track_list[2][:, 0], track_list[2][:, 1]) 368 | track_list = get_track_kinematics("left", track_path=track_path, velocity=100) 369 | plt.plot(track_list[2][:, 0], track_list[2][:, 1]) 370 | track_list = get_track_kinematics("right", track_path=track_path, velocity=100) 371 | plt.plot(track_list[2][:, 0], track_list[2][:, 1]) 372 | plt.show() 373 | idx = 3 # yaw 374 | track_list = get_track_kinematics("centerline", track_path=track_path, velocity=100) 375 | plt.plot(track_list[idx]) 376 | track_list = get_track_kinematics("raceline", track_path=track_path, velocity=100) 377 | plt.plot(track_list[idx]) 378 | track_list = get_track_kinematics("left", track_path=track_path, velocity=100) 379 | plt.plot(track_list[idx]) 380 | track_list = get_track_kinematics("right", track_path=track_path, velocity=100) 381 | plt.plot(track_list[idx]) 382 | plt.show() 383 | idx = 4 # yawrate 384 | track_list = get_track_kinematics("centerline", track_path=track_path, velocity=100) 385 | plt.plot(track_list[idx]) 386 | track_list = get_track_kinematics("raceline", track_path=track_path, velocity=100) 387 | plt.plot(track_list[idx]) 388 | track_list = get_track_kinematics("left", track_path=track_path, velocity=100) 389 | plt.plot(track_list[idx]) 390 | track_list = get_track_kinematics("right", track_path=track_path, velocity=100) 391 | plt.plot(track_list[idx]) 392 | plt.show() 393 | idx = 5 # accl. 394 | track_list = get_track_kinematics("centerline", track_path=track_path, velocity=100) 395 | plt.plot(track_list[idx]) 396 | track_list = get_track_kinematics("raceline", track_path=track_path, velocity=100) 397 | plt.plot(track_list[idx]) 398 | track_list = get_track_kinematics("left", track_path=track_path, velocity=100) 399 | plt.plot(track_list[idx]) 400 | track_list = get_track_kinematics("right", track_path=track_path, velocity=100) 401 | plt.plot(track_list[idx]) 402 | plt.show() 403 | --------------------------------------------------------------------------------