learning-corridor-selection/evaluation/evaluate_datasets.py at main · BreenSammy/learning-corridor-selection · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
import os
import os.path as osp

from hydra import compose, initialize
import numpy as np
import pandas as pd

from learning_corridor_selection.evaluation.dataset_evaluation_data import DatasetEvaluationData
from learning_corridor_selection.visualization.dataset.cost_distribution import plot_cost_distribution
from learning_corridor_selection.visualization.dataset.reference_trajectory import plot_reference_trajectory_data
from learning_corridor_selection.visualization.dataset.initial_state import plot_initial_state_distribution
from learning_corridor_selection.visualization.dataset.corridor_density import plot_corridor_vertices
from learning_corridor_selection.visualization.dataset.samples_per_scenario import plot_samples_per_scenario
from learning_corridor_selection.visualization.dataset.replanning_comparison import plot_replanning_comparison


def load_dataset(config_name, config_path, root, data_root) -> DatasetEvaluationData:
    """
    Loads a hydra dataset config to initialize the dataset data
    """

    with initialize(version_base=None, config_path=config_path, job_name="load_dataset"):
        config = compose(config_name=config_name, overrides=None)

    data = DatasetEvaluationData(root, config, data_root=data_root)

    return data

def main():
    # Columnwidth to specifiy plot sizes
    columnwidth = 211 # Half width of thesis text width

    ### Setup folders and paths
    # Hydra creates its own folder for execution, fix the path to the dataset
    root = osp.join(osp.dirname(osp.abspath(__file__)))
    data_root = osp.join(osp.dirname(osp.abspath(__file__)), "..","data")
    config_path = osp.join("..","learning_corridor_selection", "configs", "dataset")
    plots_dir = osp.join(root, 'plots')
    os.makedirs(plots_dir, exist_ok=True)

    # Load the highD and exiD dataset
    highD_data = load_dataset(config_name='highD', config_path=config_path, root=root, data_root=data_root)
    exiD_data = load_dataset(config_name='exiD', config_path=config_path, root=root, data_root=data_root)

    dataset_statistics = pd.DataFrame([highD_data.get_dataset_statistics(), exiD_data.get_dataset_statistics()], index=['highD', 'exiD'])
    print(dataset_statistics)
    dataset_statistics.to_csv(osp.join(plots_dir, 'dataset_statistics.csv'))

    # Plot vertices distribution
    highD_vertices = highD_data.get_corridor_vertices()
    exiD_vertices = exiD_data.get_corridor_vertices()
    vertices_df = pd.concat([highD_vertices, exiD_vertices])
    plot_corridor_vertices(vertices_df, osp.join(plots_dir, "vertices.svg"))

    # Create combined dataset metadata
    highD = highD_data.get_dataset_metadata()
    highD["Dataset"] = "highD"
    exiD = exiD_data.get_dataset_metadata()
    exiD["Dataset"] = "exiD"
    df = pd.concat([highD, exiD])

    # Plot initial state distribution
    initial_state_kde_settings = {
        'initial_d': dict(kdeplot=dict(clip=[-8, 8]), xlabel='Lateral position in m', xlim=(-5.5, 5.5), ylim=(0, 1.725)),
        'initial_orientation': dict(kdeplot=dict(clip = [-0.2, 0.2]), xlabel='Orientation in rad', xlim=(-0.23, 0.23), ylim=(0, 3.45e1)),
        'initial_v': dict(kdeplot=dict(clip=[0, 60]), xlabel='Velocity in m/s', xlim=(0, 60), ylim=(0, 4.6e-2)),
        'initial_a': dict(kdeplot=dict(), xlabel='Acceleration in m/s^2', xlim=(-9, 9), ylim=(0, 6.9e-1))
    }
    columns = ['Dataset']
    columns.extend(list(initial_state_kde_settings.keys()))
    df_initial_state = df[columns]
    plot_initial_state_distribution(
        df_initial_state,
        initial_state_kde_settings,
        file_path=osp.join(plots_dir, 'initial_state_distribution.svg'),
        hue='Dataset',
        width=2*columnwidth
        )

    # Build dataframe for replanning comparison
    df_wo_replanning = df[df['initial_step'] == 0]
    df_initial_state_wo_replanning = df_wo_replanning[columns]
    df_initial_state_wo_replanning['Replanning'] = 'Without'
    df_initial_state['Replanning'] = 'With'
    df_replanning_comparison = pd.concat([df_initial_state, df_initial_state_wo_replanning])
    # Plot replanning comparison
    plot_replanning_comparison(
        df_replanning_comparison,
        file_path=osp.join(plots_dir, 'replanning_comparison_initial_state.svg'))

    # Plot comparison of samples per scenario plot (data only from highD)
    df_samples_per_scenario = pd.read_csv(osp.join(root,'samples_per_scenario.zip'))
    plot_samples_per_scenario(
        df_samples_per_scenario,
        file_path=osp.join(plots_dir, 'samples_per_scenario.svg'),
        columnwidth=columnwidth)


    # Plot cost distribution for nominal costs and logartihmit costs
    df['lon_cost_log'] = np.log(df["longitudinal_cost"])
    df['lat_cost_log'] = np.log(df["lateral_cost"])
    # Delete inf values, because they are infeasible and not needed for cost distribution
    df.replace([np.inf, -np.inf], np.nan, inplace=True)
    df_feasible = df[df['lateral_cost'].notna()]
    df_feasible_logarithmic = df_feasible[['Dataset', 'lon_cost_log', 'lat_cost_log']]
    df_feasible_nominal = df_feasible[['Dataset', 'longitudinal_cost', 'lateral_cost']]
    plot_cost_distribution(
        df_feasible_nominal, x='longitudinal_cost', y='lateral_cost',
        file_path=osp.join(plots_dir, "cost_distribution_nominal.svg"),
        columnwidth=columnwidth)
    plot_cost_distribution(
        df_feasible_logarithmic, x='lon_cost_log', y='lat_cost_log',
        file_path=osp.join(plots_dir, "cost_distribution_log.svg"),
        xlabel='Logarithmic longitudinal cost',
        ylabel='Logarithmic lateral cost',
        columnwidth=columnwidth)

    # Plot reference trajectories
    highD_reference_trajectories = highD_data.get_reference_trajectory_dataset()
    exiD_reference_trajectories = exiD_data.get_reference_trajectory_dataset()
    reference_trajectories = pd.concat([highD_reference_trajectories, exiD_reference_trajectories])
    plot_reference_trajectory_data(
        reference_trajectories,
        file_path=osp.join(plots_dir, 'reference_trajectories.svg'),
        columnwidth=2*columnwidth)


if __name__ == '__main__':
    main()