Merge branch 'master' of gitlab.lrz.de:vadere/vadere

Tools/Notebooks/TrajectoryMetric.ipynb

 %% Cell type:code id: tags:
 
 ``` python
 # expand the cell of the notebook
 import json
+import gc
 import numpy as np
 import pandas as pd
 import math
 import matplotlib as mpl
 import matplotlib.pyplot as plt
 from matplotlib.lines import Line2D
 import seaborn as sns
 sns.set(style="ticks")
 
 from IPython.core.display import display, HTML
 display(HTML('<style>.container { width:100% !important; }</style>'))
 ```
 
 %% Cell type:markdown id: tags:
 
 # 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'])
     dataframe["distance"] = np.sqrt(np.square(dataframe["endX"] - dataframe["startX"]) + np.square(dataframe["endY"] - dataframe["startY"]))
     dataframe["velocity"] = dataframe["distance"] / (dataframe["endTime"] - dataframe["startTime"])
     return dataframe
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 file = "./data/TrajectoryMetric/trajectories_simulation.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'];
 
 ptrajectories = trajectories_to_dataframe(trajectories)
 ptrajectories.head()
 ```
 
 %% Cell type:markdown id: tags:
 
 # Convert experiment data into a DataFrame
 
 %% Cell type:code id: tags:
 
 ``` python
 def load_experiment(file):
     fps = 16
     data = pd.read_csv(
         file,
         sep=' ',
         names=['pedestrianId', 'timeStep', 'x', 'y', 'e'],
         index_col=False,
         header=None,
         skiprows=0)
 
     rows = []
     #print(trajectories)
     last_ped_id = None
     lastX = None
     lastY = None
     for row in data.itertuples():
         endX = row.x / 100 + 18.7
         endY = row.y / 100 + 4.2
         startTime = row.timeStep / fps - 1/fps
         endTime = row.timeStep / fps
         if last_ped_id is None or last_ped_id != row.pedestrianId:
             startX = np.nan
             startY = np.nan
             distance = np.nan
             velocity = np.nan
         else:
             startX = lastX / 100 + 18.7
             startY = lastY / 100 + 4.2
             distance = np.sqrt(np.square(endX - startX) + np.square(endY - startY))
             velocity = distance / (endTime - startTime)
         last_ped_id = row.pedestrianId
         lastX = row.x
         lastY = row.y
 
         rows.append([row.pedestrianId, startX, startY, endX, endY, startTime, endTime, distance, velocity])
     dataframe = pd.DataFrame(rows, columns=['pedestrianId', 'startX', 'startY', 'endX', 'endY','startTime','endTime','distance','velocity'])
     return dataframe
 
 def to_trajectories(data):
     trajectories = dict({})
     trajectory = []
     for i in range(len(data)-1):
         pedId = data['pedestrianId'][i]
         if pedId == data['pedestrianId'][i+1]:
             pedId = data['pedestrianId'][i]
             x1 = data['x'][i]
             y1 = data['y'][i]
             x2 = data['x'][i+1]
             y2 = data['y'][i+1]
             startTime = data['timeStep'][i]
             endTime = data['timeStep'][i+1]
             fs = {'startTime':startTime, 'endTime': endTime, 'start':{'x':x1, 'y':y1}, 'end':{'x':x2, 'y':y2}}
             trajectory.append(fs)
         else:
             trajectories[pedId] = trajectory
             trajectory = []
             pedId = data['pedestrianId'][i]
     return trajectories
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 #times = np.linspace(4,10,10)
 #euclid_d(get_trajectory(1), get_trajectory(1), times)
 #to_trajectories(load_experiment(real_file))[1]
 
 real_file = "./data/TrajectoryMetric/KO/ko-240-120-240/ko-240-120-240_combined_MB.txt"
 trajectoriesReal = load_experiment(real_file)
 #trajectoriesReal = to_trajectories(data)
 trajectoriesReal.query('pedestrianId == 1').head()
 ```
 
 %% Cell type:markdown id: tags:
 
 # Convert DataFrame to postvis DataFrame
 
 %% Cell type:code id: tags:
 
 ``` python
 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
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 to_postVis(trajectoriesReal).to_csv('expteriment_2.trajectories',index=False,sep=' ')
 to_postVis(trajectoriesReal).head(10)
 ```
 
 %% Cell type:markdown id: tags:
 
 # Calculate evacution time
 
 %% Cell type:markdown id: tags:
 
 Evacuation time = endTime - startTime
 
 %% Cell type:markdown id: tags:
 
 ## Real data
 
 %% Cell type:code id: tags:
 
 ``` python
 # Sum up all measured time deltas of a pedestrian to get the final evacuation time
-trajectoriesReal["timeDelta"] = trajectoriesReal["endTime"] - trajectoriesReal["startTime"]
-evacuation_time = trajectoriesReal.groupby(["pedestrianId"])["timeDelta"].sum()
+copy = trajectoriesReal.copy(deep=True)
+copy["timeDelta"] = copy["endTime"] - copy["startTime"]
+evacuation_time = copy.groupby(["pedestrianId"])["timeDelta"].sum()
 
 # trajectories starting from left
 cut_minX = trajectoriesReal[trajectoriesReal["endX"] < 15].groupby(["pedestrianId"])["endX"].min().max()
 
 # trajectories starting from right
 cut_maxX = trajectoriesReal[trajectoriesReal["endX"] > 21].groupby(["pedestrianId"])["endX"].max().min()
 cut_maxY = trajectoriesReal.groupby(["pedestrianId"])["endY"].max().min()
 
 print("Evacuation time (real data)")
 print("- mean: {:.2f} [s]".format(evacuation_time.mean()))
 print("- std: {:.2f} [s]".format(evacuation_time.std()))
 print("- min: {:.2f} [s]".format(evacuation_time.min()))
 print("- max: {:.2f} [s]".format(evacuation_time.max()))
 print("- minX: {:.2f} [m]".format(cut_minX))
 print("- maxX: {:.2f} [m]".format(cut_maxX))
 print("- maxY: {:.2f} [m]".format(cut_maxY))
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 rightX = trajectoriesReal[trajectoriesReal.endX < 16].groupby(["pedestrianId"])["endX"].min().max()
 leftX = trajectoriesReal[trajectoriesReal.endX > 16].groupby(["pedestrianId"])["endX"].max().min()
 topY = trajectoriesReal.groupby(["pedestrianId"])["endY"].max().min()
 topY
 ```
 
 %% Cell type:markdown id: tags:
 
 ## Simulation data
 
 %% Cell type:markdown id: tags:
 
 TODO: Use `PedestrianEvacuationTimeProcessor` to log evacuation time during simulation and analyze it here.
 
 %% Cell type:markdown id: tags:
 
 # Helper 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_trajectories(t, trajectories):
     return trajectories[np.logical_and(trajectories.startTime <= t, trajectories.endTime >= t)]
 
 def get_pedestrianIds(trajectories):
     return trajectories['pedestrianId'].unique()
 
 #def get_velocity(trajectories, t, dt):
 #    trajectories[np.logical_and(trajectory.endX >= xmax, trajectory.startX < xmax)]
 
 def get_footstep(trajectory, i):
     """returns the i-ths footstep."""
     return trajectory.iloc[i];
 
 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 start_time(trajectory):
     """returns the time of the first footstep of the trajectory."""
     return get_footstep(trajectory, 0)['startTime'];
 
 def end_time(trajectory):
     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):
     """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)
 
 def cuthead_trajectory_by(trajectory, ymin, ymax):
     i1 = trajectory[trajectory.endY >= ymax].index.min()
     i2 = trajectory[trajectory.endY <= ymin].index.min()
     #assert (i1 is np.nan and i2 is not np.nan) or (i1 is not np.nan and i2 is np.nan)
     y = ymax if i2 is np.nan or (i1 is not np.nan and i1 < i2) else ymin
     i = i1 if y == ymax else i2
     #print(i)
     # cut the footstep at the tail to exactly fit xmin or xmax
     fs = trajectory.loc[i]
     start = np.array([fs["startX"], fs["startY"]])
     end = np.array([fs["endX"], fs["endY"]])
     endTime = fs["endTime"]
     startTime = fs["startTime"]
     distance = fs["distance"]
     velocity = fs["velocity"]
     d = end - start
     if abs(fs["endY"] - fs["startY"]) > 0.00001:
         r = (y - fs["startY"]) / (fs["endY"] - fs["startY"])
         end = start + (d * r)
         time = fs["endTime"] - fs["startTime"]
         endTime = fs["startTime"] + (time * r)
         distance = np.linalg.norm(end - start)
         velocity = distance / (endTime - startTime)
 
     df = trajectory.loc[:i-1]
     llist = [[fs["pedestrianId"],fs["startX"],fs["startY"],fs["startTime"],end[0],end[1],endTime,distance,velocity]]
     df_head = pd.DataFrame(llist, columns=['pedestrianId','startX','startY','startTime','endX','endY','endTime','distance','velocity'])
     df = df.append(df_head, ignore_index=True)
     return df
 
 def cuttail_trajectory_by(trajectory, xmin, xmax):
     #i1 = trajectory[np.logical_and(trajectory.endX >= xmax, trajectory.startX < xmax)].index.max()
-    i1 = trajectory[trajectory.endX >= xmax].index.max()
-    i2 = trajectory[trajectory.endX <= xmin].index.max()
+    i1 = trajectory[np.logical_or(trajectory.endX >= xmax, trajectory.startX is np.nan)].index.max()
+    i2 = trajectory[np.logical_or(trajectory.endX <= xmin, trajectory.startX is np.nan)].index.max()
     #assert (i1 is np.nan and i2 is not np.nan) or (i1 is not np.nan and i2 is np.nan)
     x = xmax if i2 is np.nan or (i1 is not np.nan and i1 > i2) else xmin
     i = i1 if x == xmax else i2
     i = i+1
     # cut the footstep at the tail to exactly fit xmin or xmax
     fs = trajectory.loc[i]
     start = np.array([fs["startX"], fs["startY"]])
     end = np.array([fs["endX"], fs["endY"]])
     startTime = fs["startTime"]
     endTime = fs["endTime"]
     distance = fs["distance"]
     velocity = fs["velocity"]
     d = end - start
     if abs(fs["endX"] - fs["startX"]) > 0.00001:
         r = (x - fs["startX"]) / (fs["endX"] - fs["startX"])
         end = start + (d * r)
         time = fs["endTime"] - fs["startTime"]
         endTime = fs["startTime"] + (time * r)
         distance = np.linalg.norm(end - start)
-        velocity = distance / (endTime - startTime)
+        if abs(endTime - startTime) < 0.00001:
+            if distance < 0.00001:
+                velocity = 0
+            else:
+                raise exception
+        else:
+            velocity = distance / (endTime - startTime)
 
     df = trajectory.loc[i+1:]
     llist = [[fs["pedestrianId"],fs["startX"],fs["startY"],fs["startTime"],end[0],end[1],endTime,distance,velocity]]
     df_tail = pd.DataFrame(llist, columns=['pedestrianId','startX','startY','startTime','endX','endY','endTime','distance','velocity'])
     df_tail = df_tail.append(df, ignore_index=True)
     return df_tail
 
 def cuthead_by(trajectories, ymin, ymax):
     df = pd.DataFrame([], columns=['pedestrianId','startX','startY','startTime','endX','endY','endTime'])
     pedIds = get_pedestrianIds(trajectories)
     for pedId in pedIds:
         df = df.append(cuthead_trajectory_by(get_trajectory(pedId, trajectories), ymin, ymax), ignore_index=True)
     return df
 
 def cuttail_by(trajectories, xmin, xmax):
     df = pd.DataFrame([], columns=['pedestrianId','startX','startY','startTime','endX','endY','endTime'])
     pedIds = get_pedestrianIds(trajectories)
     for pedId in pedIds:
         df = df.append(cuttail_trajectory_by(get_trajectory(pedId, trajectories), xmin, xmax), ignore_index=True)
     return df
 
 def cut(trajectories):
     df = cuttail_by(trajectories, cut_minX, cut_maxX)
     df = cuthead_by(df, -1000, cut_maxY)
     return df
 
 traj = get_trajectory(2, trajectoriesReal)
 ts = traj[traj.endX > 22].index.max()
 ts
 #cut(trajectoriesReal)
-#cuttail_by(trajectoriesReal, 13, 22).head()
+cuttail_by(trajectoriesReal, cut_minX, cut_maxX).head()
 #cuthead_by(trajectoriesReal, 0, 4).tail()
 #traj.loc[100]
 #trajectoriesReal.tail()
+trajectoriesReal.head()
 ```
 
 %% Cell type:markdown id: tags:
 
 # Helper methods to compute different metrices
 
 %% 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 mean_velocity_at(t, trajectories):
     return get_trajectories(t, trajectories)['velocity'].mean()
 
 def trajectory_length(trajectory):
     """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 footstep_duration(fs):
     """Duration of a footstep."""
     startTime = fs['startTime'];
     endTime = fs['endTime'];
     return endTime-startTime;
 
 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 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)
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 #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.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]))
 t = 0.5
 ttraj = ptrajectories[np.logical_and(ptrajectories.startTime <= t, ptrajectories.endTime >= t)]
 #ptrajectories["velocity"] = numpy.linalg.norm(
 get_trajectories(0.5, ptrajectories).head()
 ```
 
 %% Cell type:code id: tags:
 
 ``` python
 def greedy_match(trajectories1, trajectories2, times, f):
     """Computes a match of trajectories by using a greedy algorithm."""
     assert len(trajectories1) == len(trajectories2)
     min_index1 = min(trajectories1.keys())
     min_index2 = min(trajectories2.keys())
     match = {}
     indexSet = set(range(min_index2, len(trajectories2)))
     for i in range(min_index1, len(trajectories1)):
         traj1 = trajectories1[i]
         minVal = None
         minIndex = None
         for j in indexSet:
             traj2 = trajectories2[j]
             if overlap(traj1, traj2, 0.4):
                 val = f(traj1, traj2, times)
                 if(minVal == None or val < minVal):
                     minIndex = j
                     minVal = val
         match[i] = minIndex
         indexSet.remove(minIndex)
     return match
 ```
 
 %% Cell type:markdown id: tags:
 
 Here we cut all trajectory data such each left trajectory starts at the same $x$-coordinate and each right trajectory starts at the same $x$-coordinate. In addition each trajectory ends a the same $y$-coordinate.
 
 %% Cell type:code id: tags: