change the metric notebook, small changes to the postvis such that other...

change the metric notebook, small changes to the postvis such that other simTimeSteps not equal to 0.4 will be used if they are contained in the postvis.trajectories file.

Tools/Notebooks/TrajectoryMetric.ipynb

 %% Cell type:code id: tags:
 
 ``` python
+
+```
+
+%% Cell type:code id: tags:
+
+``` python
+# expand the cell of the notebook
 import json
 import numpy as np
 import pandas as pd
 import math
 import matplotlib.pyplot as plt
 from matplotlib.lines import Line2D
 
+from IPython.core.display import display, HTML
+display(HTML('<style>.container { width:100% !important; }</style>'))
+```
+
+%% Cell type:markdown id: tags:
+
+# Methods to convert Vadere trajectories into a DataFrame
+
+%% Cell type:code id: tags:
+
+``` python
+def fs_append(pedestrianId, fs, llist):
+    llist.append([pedestrianId, fs['start']['x'], fs['start']['y'], fs['startTime'], fs['end']['x'], fs['end']['y'], fs['endTime']])
+   
+def trajectory_append(pedestrianId, trajectory, llist):
+    for fs in trajectory:
+        fs_append(pedestrianId, fs, llist)
+
+def trajectories_to_dataframe(trajectories):
+    llist = []
+    for pedId in trajectories:
+        trajectory_append(pedId, trajectories[pedId], llist)
+    dataframe = pd.DataFrame(llist, columns=['pedestrianId','startX','startY','startTime','endX','endY','endTime'])
+    return dataframe
+
 file = "./data/trajectories_distance.txt"
-real_file = "./data/KO/ko-240-120-240/ko-240-120-240_combined_MB.txt"
 f = open(file, "r")
 header = f.readline();
 trajectories = dict({});
+
 for row in f:
     s = row.split(" ");
     pedId = int(s[0]);
     footsteps = json.loads(s[1]);
     trajectories[pedId] = footsteps[0]['footSteps'];
-    
-def get_trajectory(pedId):
-    return trajectories[pedId]
+
+ptrajectories = trajectories_to_dataframe(trajectories)
+ptrajectories.head()
+```
+
+%% Cell type:markdown id: tags:
+
+# Helpler method to access parts of the trajectory
+
+%% Cell type:code id: tags:
+
+``` python
+def get_trajectory(pedId, trajectories):
+    """returns a data frame containing the trajectory of one specific agent."""
+    query = 'pedestrianId == ' + str(pedId)
+    return trajectories.query(query)
+
+def get_pedestrianIds(trajectories):
+    return trajectories['pedestrianId'].unique()
 
 def get_footstep(trajectory, i):
-    return trajectory[i];
+    """returns the i-ths footstep."""
+    return trajectory.iloc[i];
 
-def start_time(trajectory):
-    return trajectory[0]['startTime'];
+def get_footstep_by_time(trajectory, time):
+    """returns the footstep which happens at time or nothing (None)."""
+    query = 'startTime <= ' + str(time) + ' and ' + str(time) + ' < endTime'
+    fs = trajectories.query(query)
+    assert len(fs) >= 1
+    return fs
 
-def max_start_time(trajectories):
-    return max(map(lambda i: start_time(trajectories[i]), trajectories))
+def start_time(trajectory):
+    """returns the time of the first footstep of the trajectory."""
+    return get_footstep(trajectory, 0)['startTime'];
 
 def end_time(trajectory):
-    return trajectory[-1]['endTime'];
+    return get_footstep(trajectory, len(trajectory)-1)['endTime'];
+
+def max_start_time(trajectories):
+    """returns the time of the first footstep of the trajectory which starts last."""
+    pedestrianIds = get_pedestrianIds(trajectories)
+    return max(map(lambda pedId: start_time(get_trajectory(pedId, trajectories)), pedestrianIds))
 
 def min_end_time(trajectories):
-    return min(map(lambda i: end_time(trajectories[i]), trajectories))
+    """returns the time of the last footstep of the trajectory which ends first."""
+    pedestrianIds = get_pedestrianIds(trajectories)
+    return min(map(lambda pedId: end_time(get_trajectory(pedId, trajectories)), pedestrianIds))
+
+def footstep_is_between(fs, time):
+    """true if the foostep and the intersection with time is not empty."""
+    startTime = fs['startTime'];
+    endTime = fs['endTime'];
+    return startTime <= time and time < endTime;
+
+def cut(trajectory, sTime, eTime):
+    query = 'startTime >= ' + str(sTime) + ' and endTime < ' + str(eTime)
+    return trajectory.query(query)
+
+def cut_soft(trajectory, sTime, eTime):
+    query = 'endTime > ' + str(sTime) + ' and startTime < ' + str(eTime)
+    return trajectory.query(query)
+```
+
+%% Cell type:markdown id: tags:
+
+# Helper methods to compute different metrices
 
-def length(fs):
-    start = fs['start'];
-    end = fs['end'];
-    x1 = start['x'];
-    y1 = start['y'];
-    x2 = end['x'];
-    y2 = end['y'];
+%% Cell type:code id: tags:
+
+``` python
+def footstep_length(fs):
+    """Euclidean length of a footstep."""
+    x1 = fs['startX'];
+    y1 = fs['startY'];
+    x2 = fs['endX'];
+    y2 = fs['endY'];
     dx = x1-x2;
     dy = y1-y2;
     return np.sqrt(dx*dx + dy*dy);
 
 def trajectory_length(trajectory):
-    return sum(map(lambda fs : length(fs), trajectory))
-
-def direction(fs):
-    start = fs['start'];
-    end = fs['end'];
-    x1 = start['x'];
-    y1 = start['y'];
-    x2 = end['x'];
-    y2 = end['y'];
+    """Euclidean length of a trajectory."""
+    dx = trajectory['startX']-trajectory['endX']
+    dy = trajectory['startY']-trajectory['endY']
+    return np.sqrt(dx*dx + dy*dy).sum();
+
+def footstep_direction(fs):
+    """Vector from start to end position."""
+    x1 = fs['startX'];
+    y1 = fs['startY'];
+    x2 = fs['endX'];
+    y2 = fs['endY'];
     return np.array([x2-x1, y2-y1]);
 
-def duration(fs):
+def footstep_duration(fs):
+    """Duration of a footstep."""
     startTime = fs['startTime'];
     endTime = fs['endTime'];
     return endTime-startTime;
 
-def speed(fs):
-    return length(fs) / duration(fs);
+def trajectory_duration(trajectory):
+    """Euclidean length of a trajectory."""
+    return (trajectory['endTime'] - trajectory['startTime']).sum();
+
+def footstep_speed(fs):
+    """Speed of the footstep."""
+    return footstep_length(fs) / footstep_duration(fs);
+
+def trajectory_speed(fs):
+    """Speed of the trajectory."""
+    return trajectory_length(fs) / trajectory_duration(fs);
+
+#def trajectory_positions(trajectory, times):
+#    mask = trajectory[['startTime', 'endTime']].mask(lambda x: x**2)
+#    (df['date'] > '2000-6-1') & (df['date'] <= '2000-6-10')
+#    duration = trajectory['endTime'] - trajectory['startTime']
+#    dx = trajectory['endX'] - trajectory['startX']
+#    dy = trajectory['endY'] - trajectory['startY']
+#    direction = 
+    
+def filter_trajectories(trajectories, times):
+    """Filters trajectory by times."""
+    rows = []
+    for row in trajectories.itertuples():
+        if len(list(filter(lambda b: b, map(lambda t: row.startTime <= t and t < row.endTime, times)))) > 0:
+            rows.append(row)
+    return pd.DataFrame(rows)
+
+def trajectories_position(trajectories, times):
+    """Transforms trajectories into positions at each time in times such that each position is computed by linear interpolation."""
+    rows = []
+    #print(trajectories)
+    for row in trajectories.itertuples():
+        llist = list(filter(lambda t: row.startTime <= t and t < row.endTime, times))
+        assert len(llist) == 1 or len(llist) == 0
+        if len(llist) > 0:
+            time = llist[0]
+            dur = row.endTime - row.startTime
+            partial_dur = time - row.startTime
+            ratio = partial_dur / dur
+            direction = np.array([row.endX - row.startX, row.endY - row.startY])
+            l = np.linalg.norm(direction)
+            if l > 0:
+                partial_l = l * ratio;
+                v = direction / l * partial_l;
+                pos = np.array([row.startX, row.startY]) + v;
+                rows.append([row.pedestrianId, pos[0], pos[1], time])
+            else:
+                rows.append([row.pedestrianId, np.nan, np.nan, time])
+    dataframe = pd.DataFrame(rows, columns=['pedestrianId','x','y','time'])
+    return dataframe
+    
+def euclid_d(trajPos1, trajPos2):
+    """Computes the total (Euclidean) distance between two trajectories.
+       Assumption: trajectories are both cut acccordingly!   
+    """
+    assert len(trajPos1) == len(trajPos2)
+    dx = trajPos1['x'] - trajPos2['x']
+    dy = trajPos1['y'] - trajPos2['y']
+    norm = np.sqrt(dx**2 + dy**2)
+    return norm.sum() / len(dx)
+
+def euclid_path_length(trajPos1, trajPos2):
+    """Computes the total (Euclidean) path length difference between two trajectories.
+       Assumption: trajectories are both cut acccordingly!
+    """
+    count = len(trajPos1)
+    pad = pd.DataFrame([[np.nan, np.nan, np.nan, np.nan]], columns=['pedestrianId','x','y','time'])
+    trajPos1Pad = pd.concat([pad, trajPos1], ignore_index=True)
+    trajPos2Pad = pd.concat([pad, trajPos1], ignore_index=True)
+    dx1 = trajPos1['x'] - trajPos1Pad['x']
+    dy1 = trajPos1['y'] - trajPos1Pad['y']
+    dx2 = trajPos2['x'] - trajPos2Pad['x']
+    dy2 = trajPos2['y'] - trajPos2Pad['y']
+    dx = dx1 - dx2
+    dy = dy1 - dy2
+    diff = np.sqrt(dx**2 + dy**2)
+    return diff.sum()
 
-def is_between(fs, time):
-    startTime = fs['startTime'];
-    endTime = fs['endTime'];
-    return startTime <= time and time < endTime;
+def euclid_len(trajectory, sTime, eTime):
+    """Computes the total (Euclidean) length of the trajectory in between [sTime;eTime]."""
+    cut_traj = cut_soft(trajectory, sTime, eTime);
+    return trajectory_length(cut_traj)
+    
+def inter_agent_d(trajPos):
+    """Computes the inter agent (Euclidean) distance between all pairs of agents.
+       Assumption: the trajectory is cut accordingly, ie the time is equal for
+       each position.
+    """
+    s = 0
+    min_index = min(trajectories.keys())
+    c = 0
+    llen = len(trajPos)
+    for index1, row1 in trajPos.iterrows():
+        for index2, row2 in trajPos.tail(llen-1-index1).iterrows():
+            x1 = row1['x']
+            y1 = row1['y']
+            x2 = row2['x']
+            y2 = row2['y']
+            dx = x1 - x2
+            dy = y1 - y2
+            s = s + np.sqrt(dx**2 + dy**2)
+            c = c + 1
+    if c == 0:
+        return 0
+    else:
+        return s / c
+    
+def total_inter_agent(trajectories1, trajectories2, times):
+    """too expensive! TODO!"""
+    return sum(map(lambda t: inter_agent_d(trajectories_position(trajectories1, [t])) - inter_agent_d(trajectories_position(trajectories2, [t])), times)) / len(times)
+    
+#start_time(get_trajectory(1, ptrajectories))
+#max_start_time(ptrajectories)
+#end_time(get_trajectory(1, ptrajectories))
+#foot_step_length(get_footstep(get_trajectory(1, ptrajectories), 0))
+#trajectory_length(get_trajectory(1, ptrajectories))
+#trajectory_speed(get_trajectory(1, ptrajectories))
+cutTraj = cut(get_trajectory(1, ptrajectories), 0.0, 10.0)[['startTime', 'endTime']]
+#cutTraj.mask(cutTraj['startTime'] <= 4 and 4 > cutTraj['endTime'])
+#start_time(get_trajectory(1, ptrajectories))
+#trajectories_position(ptrajectories, [1,2,3,4]).head()
+trajPos1 = trajectories_position(get_trajectory(2, ptrajectories), [1,2,3,4,5,6,8,9,10,11,12,13])
+trajPos2 = trajectories_position(get_trajectory(7, ptrajectories), [1,2,3,4,5,6,8,9,10,11,12,13])
+trajPos1 = trajPos1[~np.isnan(trajPos1.x)]
+trajPos2 = trajPos2[~np.isnan(trajPos2.x)]
+euclid_path_length(trajPos1, trajPos2)
+euclid_len(ptrajectories,0,10000)
+#print(total_inter_agent(ptrajectories, ptrajectories, [1,2]))
+```
+
+%% Cell type:code id: tags:
+
+``` python
+trajPos2
+```
+
+%% Cell type:code id: tags:
+
+``` python
+
+```
 
-def footstep(trajectory, time):
-    l = list(filter(lambda fs : is_between(fs, time), trajectory))
-    assert len(l) <= 1;
-    if len(l) == 1:
-        return l[0];
+%% Cell type:code id: tags:
 
+``` python
 def position(trajectory, time):
     fs = footstep(trajectory, time);
     if fs != None:
         startTime = fs['startTime'];
         endTime = fs['endTime'];
@@ -95,16 +312,11 @@
             return np.array([x1, y1])
         else:    
             partial_l = l * ratio;
             v = direction(fs) / l * partial_l;
             return np.array([x1, y1]) + v;
-    
-def cut(trajectory, sTime, eTime):
-    return list(filter(lambda fs : fs['startTime'] >= sTime and fs['endTime'] < eTime, trajectory))
 
-def cut_soft(trajectory, sTime, eTime):
-    return list(filter(lambda fs : fs['startTime'] >= sTime and fs['startTime'] < eTime, trajectory))
 
 def euclid_d(traj1, traj2, times):
     """Computes the total (Euclidean) distance between two trajectories at certain times."""
     return 0
     sT = max([start_time(traj1), start_time(traj2)])
@@ -178,21 +390,28 @@
 def overlap(traj1, traj2, dt):
     return True
     
 def load_experiment(file):
     fps = 16
+    pad = pd.DataFrame([[np.nan, np.nan, np.nan, np.nan, np.nan]], columns=['pedestrianId', 'timeStep', 'x', 'y', 'e'])
     data = pd.read_csv(
         file, 
         sep=' ', 
         names=['pedestrianId', 'timeStep', 'x', 'y', 'e'], 
         index_col=False, 
         header=None, 
         skiprows=0)
+    
+    cc = pd.concat([pad, data], ignore_index=True)
         
-    data['x'] = data['x'] / 100
-    data['y'] = data['y'] / 100
-    data['timeStep'] = data['timeStep'] / fps
+    data['endX'] = data['x'] / 100 + 18.7
+    data['endY'] = data['y'] / 100 + 4.2
+    data['startX'] = cc['x'] / 100 + 18.7
+    data['startY'] = cc['y'] / 100 + 4.2
+    data['startTime'] = data['timeStep'] / fps - 1/fps
+    data['endTime'] = data['timeStep'] / fps
+    data = data.drop(columns=['timeStep','x','y','e'])
     return data
     
 def to_trajectories(data):
     trajectories = dict({})
     trajectory = []
@@ -212,19 +431,35 @@
             trajectories[pedId] = trajectory
             trajectory = []
             pedId = data['pedestrianId'][i]
     return trajectories
 
+def to_postVis(df):
+    simTimeStep = 0.4
+    fps = 16
+    df['timeStep'] = np.ceil(df['endTime'] / (1/fps)).astype(np.int)
+    df['x'] = df['endX']
+    df['y'] = df['endY']
+    df['simTime'] = df['endTime']
+    df = df.drop(columns=['startX','startY','endX','endY','startTime', 'endTime'])    
+    return df
 #times = np.linspace(4,10,10)
 #euclid_d(get_trajectory(1), get_trajectory(1), times)
 #to_trajectories(load_experiment(real_file))[1]
+
+file = "./data/trajectories_distance.txt"
+real_file = "./data/KO/ko-240-120-240/ko-240-120-240_combined_MB.txt"
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-trajectoriesReal = to_trajectories(load_experiment(real_file));
+data = load_experiment(real_file)
+#trajectoriesReal = to_trajectories(data)
+data.query('pedestrianId == 1').head()
+to_postVis(data).to_csv('expteriment_2.trajectories',index=False,sep=' ')
+to_postVis(data).head(10)
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
@@ -246,21 +481,19 @@
     return x, y, Line2D(x, y, color=c, linewidth=0.2)
 
 fig1 = plt.figure(figsize=(10,10))
 ax1 = fig1.add_subplot(111)
 
-x_rcenter = -1.0
-y_rcenter = 1.0
 
 x_vcenter = 17.5
 y_vcenter = 5.2
 for i in range(len(trajectoriesReal)):
-    x, y, line = to_line(trajectoriesReal[i+1], -2)
+    x, y, line = to_line(trajectoriesReal[i+1], 14)
     ax1.add_line(line)
     
-ax1.set_xlim(x_rcenter-5, x_rcenter+5)
-ax1.set_ylim(y_rcenter-4, y_rcenter+4)
+ax1.set_xlim(x_vcenter-5, x_vcenter+5)
+ax1.set_ylim(y_vcenter-4, y_vcenter+4)
 ax1.set_aspect(1)
 
 fig2 = plt.figure(figsize=(10,10))
 ax2 = fig2.add_subplot(111)
 
@@ -298,42 +531,53 @@
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-[] is not None
+pd.DataFrame([[1,2,3,4,5],[6,7,8,9,10]], columns=['pedestrianId','timeStep','x','y','time'])
+```
+
+%% Cell type:code id: tags:
+
+``` python
+
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-min(trajectories.keys())
+
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-np.arange(14,15,0.4)
+
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-range(1, 5)[0]
+
+```
+
+%% Cell type:code id: tags:
+
+``` python
+
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-for i in set(range(1,10)):
-    print(i)
+to_postVis(data)
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
-print(greedy_match(trajectories, trajectories, np.arange(14,15,0.4), euclid_d))
+1 in [1,2,3]
 ```
 
 %% Cell type:code id: tags:
 
 ``` python

VadereSimulator/src/org/vadere/simulator/projects/io/TrajectoryReader.java

@@ -54,10 +54,12 @@ public class TrajectoryReader {
 	private Set<String> groupIdKeys;
 	private Set<String> groupSizeKeys;
 	private Set<String> stridesKeys;
+	private Set<String> simTimeKeys;
 
 
 	private int pedIdIndex;
 	private int stepIndex;
+	private int simTimeIndex;
 	private int xIndex;
 	private int yIndex;
 	private int targetIdIndex;
@@ -84,12 +86,15 @@ public class TrajectoryReader {
 		groupIdKeys = new HashSet<>();
 		groupSizeKeys = new HashSet<>();
 		stridesKeys = new HashSet<>();
+		simTimeKeys = new HashSet<>();
 
 		//should be set via Processor.getHeader
 		pedestrianIdKeys.add("id");
 		pedestrianIdKeys.add("pedestrianId");
 		stepKeys.add("timeStep");
 		stepKeys.add("step");
+		simTimeKeys.add("simTime");
+		simTimeKeys.add("time");
 		xKeys.add("x");
 		yKeys.add("y");
 		targetIdKeys.add("targetId");
@@ -100,6 +105,7 @@ public class TrajectoryReader {
 
 		pedIdIndex = -1;
 		stepIndex = -1;
+		simTimeIndex = -1;
 		xIndex = -1;
 		yIndex = -1;
 		targetIdIndex = -1;
@@ -146,6 +152,9 @@ public class TrajectoryReader {
 			else if(stridesKeys.contains(columns[index])) {
 				stridesIndex = index;
 			}
+			else if(simTimeKeys.contains(columns[index])) {
+				simTimeIndex = index;
+			}
 		}
 		try {
 			if (pedIdIndex != -1 && xIndex != -1 && yIndex != -1 && stepIndex != -1) {
@@ -212,6 +221,7 @@ public class TrajectoryReader {
 	private Pair<Step, Agent> parseRowTokens(@NotNull final String[] rowTokens) {
 		// time step
 		int step = Integer.parseInt(rowTokens[stepIndex]);
+		double simTime = 0.0;
 
 		// pedestrian id
 		int pedestrianId = Integer.parseInt(rowTokens[pedIdIndex]);
@@ -227,6 +237,10 @@ public class TrajectoryReader {
 		targets.addFirst(targetId);
 		ped.setTargets(targets);
 
+		if(simTimeIndex != -1) {
+			simTime = Double.parseDouble(rowTokens[simTimeIndex]);
+		}
+
 		if(groupIdIndex != -1) {
 			int groupId = Integer.parseInt(rowTokens[groupIdIndex]);
 			int groupSize = groupSizeIndex != -1 ? Integer.parseInt(rowTokens[groupSizeIndex]) : -1;
@@ -240,6 +254,6 @@ public class TrajectoryReader {
 			}
 		}
 
-		return Pair.create(new Step(step), ped);
+		return simTimeIndex == -1 ? Pair.create(new Step(step), ped) : Pair.create(new Step(step, simTime), ped);
 	}
 }

VadereState/src/org/vadere/state/simulation/Step.java

@@ -15,6 +15,11 @@ public class Step implements Comparable<Step> {
 		this.simTimeInSec = null;
 	}
 
+	public Step(final int stepNumber, final double simTimeInSec) {
+		this.stepNumber = stepNumber;
+		this.simTimeInSec = simTimeInSec;
+	}
+
 	/**
 	 * Returns an Optional<Double> since the simulation time in seconds may not stored.
 	 * </