Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
vadere
vadere
Commits
bedc4daa
Commit
bedc4daa
authored
May 20, 2019
by
Benedikt Zoennchen
Browse files
add python notebook which defines and computes different trajectory metrices.
parent
984d6105
Pipeline
#114759
passed with stages
in 139 minutes and 51 seconds
Changes
30
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Tools/Notebooks/TrajectoryMetric.ipynb
0 → 100644
View file @
bedc4daa
{
"cells": [
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"code_folding": []
},
"outputs": [],
"source": [
"import json\n",
"import numpy as np\n",
"import pandas as pd\n",
"import math\n",
"import matplotlib.pyplot as plt\n",
"from matplotlib.lines import Line2D\n",
"\n",
"file = \"./data/trajectories_distance.txt\"\n",
"real_file = \"./data/KO/ko-240-120-240/ko-240-120-240_combined_MB.txt\"\n",
"f = open(file, \"r\")\n",
"header = f.readline();\n",
"trajectories = dict({});\n",
"for row in f:\n",
" s = row.split(\" \");\n",
" pedId = int(s[0]);\n",
" footsteps = json.loads(s[1]);\n",
" trajectories[pedId] = footsteps[0]['footSteps'];\n",
" \n",
"def get_trajectory(pedId):\n",
" return trajectories[pedId]\n",
"\n",
"def get_footstep(trajectory, i):\n",
" return trajectory[i];\n",
"\n",
"def start_time(trajectory):\n",
" return trajectory[0]['startTime'];\n",
"\n",
"def max_start_time(trajectories):\n",
" return max(map(lambda i: start_time(trajectories[i]), trajectories))\n",
"\n",
"def end_time(trajectory):\n",
" return trajectory[-1]['endTime'];\n",
"\n",
"def min_end_time(trajectories):\n",
" return min(map(lambda i: end_time(trajectories[i]), trajectories))\n",
"\n",
"def length(fs):\n",
" start = fs['start'];\n",
" end = fs['end'];\n",
" x1 = start['x'];\n",
" y1 = start['y'];\n",
" x2 = end['x'];\n",
" y2 = end['y'];\n",
" dx = x1-x2;\n",
" dy = y1-y2;\n",
" return np.sqrt(dx*dx + dy*dy);\n",
"\n",
"def trajectory_length(trajectory):\n",
" return sum(map(lambda fs : length(fs), trajectory))\n",
"\n",
"def direction(fs):\n",
" start = fs['start'];\n",
" end = fs['end'];\n",
" x1 = start['x'];\n",
" y1 = start['y'];\n",
" x2 = end['x'];\n",
" y2 = end['y'];\n",
" return np.array([x2-x1, y2-y1]);\n",
"\n",
"def duration(fs):\n",
" startTime = fs['startTime'];\n",
" endTime = fs['endTime'];\n",
" return endTime-startTime;\n",
"\n",
"def speed(fs):\n",
" return length(fs) / duration(fs);\n",
"\n",
"def is_between(fs, time):\n",
" startTime = fs['startTime'];\n",
" endTime = fs['endTime'];\n",
" return startTime <= time and time < endTime;\n",
"\n",
"def footstep(trajectory, time):\n",
" l = list(filter(lambda fs : is_between(fs, time), trajectory))\n",
" assert len(l) <= 1;\n",
" if len(l) == 1:\n",
" return l[0];\n",
"\n",
"def position(trajectory, time):\n",
" fs = footstep(trajectory, time);\n",
" if fs != None:\n",
" startTime = fs['startTime'];\n",
" endTime = fs['endTime'];\n",
" dur = duration(fs);\n",
" partial_dur = time - startTime;\n",
" ratio = partial_dur / dur;\n",
" start = fs['start'];\n",
" x1 = start['x'];\n",
" y1 = start['y'];\n",
" l = length(fs);\n",
" if l == 0.0:\n",
" return np.array([x1, y1])\n",
" else: \n",
" partial_l = l * ratio;\n",
" v = direction(fs) / l * partial_l;\n",
" return np.array([x1, y1]) + v;\n",
" \n",
"def cut(trajectory, sTime, eTime):\n",
" return list(filter(lambda fs : fs['startTime'] >= sTime and fs['endTime'] < eTime, trajectory))\n",
"\n",
"def cut_soft(trajectory, sTime, eTime):\n",
" return list(filter(lambda fs : fs['startTime'] >= sTime and fs['startTime'] < eTime, trajectory))\n",
"\n",
"def euclid_d(traj1, traj2, times):\n",
" \"\"\"Computes the total (Euclidean) distance between two trajectories at certain times.\"\"\"\n",
" return 0\n",
" sT = max([start_time(traj1), start_time(traj2)])\n",
" eT = min([end_time(traj1), end_time(traj2)])\n",
" filtered_times = list(filter(lambda t: t >= sT and t <= eT, times))\n",
" overlaps = len(filtered_times)\n",
" if overlaps == 0:\n",
" return 0\n",
" return sum(map(lambda t: np.linalg.norm(position(traj1, t)- position(traj2, t)), filtered_times)) / overlaps\n",
" \n",
"def euclid_path_length(traj1, traj2, times):\n",
" sT = max([start_time(traj1), start_time(traj2)]);\n",
" eT = min([end_time(traj1), end_time(traj2)]);\n",
" filtered_times = list(filter(lambda t: t >= sT and t <= eT, times));\n",
" s = np.array([0, 0])\n",
" for i in range(len(filtered_times)-1):\n",
" t1 = filtered_times[i]\n",
" t2 = filtered_times[i+1]\n",
" d1 = position(traj1, t1) - position(traj1, t2)\n",
" d2 = position(traj2, t1) - position(traj2, t2)\n",
" diff = d1 - d2\n",
" s = s + diff\n",
" return s;\n",
"\n",
"def inter_agent_d(trajectories, t):\n",
" s = 0\n",
" min_index = min(trajectories.keys())\n",
" c = 0\n",
" for i in range(len(trajectories)):\n",
" pos1 = position(trajectories[i+min_index], t)\n",
" for j in range(i+1, len(trajectories)):\n",
" pos2 = position(trajectories[j+min_index], t)\n",
" if pos1 is not None and pos2 is not None:\n",
" s = s + np.linalg.norm(pos1 - pos2)\n",
" c = c + 1\n",
" if c == 0:\n",
" return 0\n",
" else:\n",
" return s / c\n",
" \n",
"def total_inter_agent(trajectories1, trajectories2, times):\n",
" return sum(map(lambda t: inter_agent_d(trajectories1, t) - inter_agent_d(trajectories2, t), times)) / len(times)\n",
" \n",
"def euclid_len(trajectory, sTime, eTime):\n",
" \"\"\"Computes the total (Euclidean) length of the trajectory in between [sTime;eTime].\"\"\"\n",
" cut_traj = cut_soft(trajectory, sTime, eTime);\n",
" return trajectory_length(cut_traj)\n",
"\n",
"def greedy_match(trajectories1, trajectories2, times, f):\n",
" \"\"\"Computes a match of trajectories by using a greedy algorithm.\"\"\"\n",
" assert len(trajectories1) == len(trajectories2)\n",
" min_index1 = min(trajectories1.keys())\n",
" min_index2 = min(trajectories2.keys())\n",
" match = {}\n",
" indexSet = set(range(min_index2, len(trajectories2)))\n",
" for i in range(min_index1, len(trajectories1)):\n",
" traj1 = trajectories1[i]\n",
" minVal = None\n",
" minIndex = None\n",
" for j in indexSet:\n",
" traj2 = trajectories2[j]\n",
" if overlap(traj1, traj2, 0.4):\n",
" val = f(traj1, traj2, times)\n",
" if(minVal == None or val < minVal):\n",
" minIndex = j\n",
" minVal = val\n",
" match[i] = minIndex\n",
" indexSet.remove(minIndex)\n",
" return match\n",
" \n",
"def overlap(traj1, traj2, dt):\n",
" return True\n",
" \n",
"def load_experiment(file):\n",
" fps = 16\n",
" data = pd.read_csv(\n",
" file, \n",
" sep=' ', \n",
" names=['pedestrianId', 'timeStep', 'x', 'y', 'e'], \n",
" index_col=False, \n",
" header=None, \n",
" skiprows=0)\n",
" \n",
" data['x'] = data['x'] / 100\n",
" data['y'] = data['y'] / 100\n",
" data['timeStep'] = data['timeStep'] / fps\n",
" return data\n",
" \n",
"def to_trajectories(data):\n",
" trajectories = dict({})\n",
" trajectory = []\n",
" for i in range(len(data)-1):\n",
" pedId = data['pedestrianId'][i]\n",
" if pedId == data['pedestrianId'][i+1]:\n",
" pedId = data['pedestrianId'][i]\n",
" x1 = data['x'][i]\n",
" y1 = data['y'][i]\n",
" x2 = data['x'][i+1]\n",
" y2 = data['y'][i+1]\n",
" startTime = data['timeStep'][i] \n",
" endTime = data['timeStep'][i+1]\n",
" fs = {'startTime':startTime, 'endTime': endTime, 'start':{'x':x1, 'y':y1}, 'end':{'x':x2, 'y':y2}}\n",
" trajectory.append(fs)\n",
" else:\n",
" trajectories[pedId] = trajectory\n",
" trajectory = []\n",
" pedId = data['pedestrianId'][i]\n",
" return trajectories\n",
"\n",
"#times = np.linspace(4,10,10)\n",
"#euclid_d(get_trajectory(1), get_trajectory(1), times)\n",
"#to_trajectories(load_experiment(real_file))[1]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"trajectoriesReal = to_trajectories(load_experiment(real_file));"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import seaborn as sns\n",
"sns.set(style=\"ticks\")\n",
"\n",
"current_palette = sns.color_palette()\n",
"\n",
"def to_line(trajectory, xleft):\n",
" x = []\n",
" y = []\n",
" for fs in trajectory:\n",
" x.append(fs['start']['x'])\n",
" y.append(fs['start']['y'])\n",
" if x[0] < xleft:\n",
" c = current_palette[2]\n",
" else:\n",
" c = current_palette[0]\n",
" return x, y, Line2D(x, y, color=c, linewidth=0.2)\n",
"\n",
"fig1 = plt.figure(figsize=(10,10))\n",
"ax1 = fig1.add_subplot(111)\n",
"\n",
"x_rcenter = -1.0\n",
"y_rcenter = 1.0\n",
"\n",
"x_vcenter = 17.5\n",
"y_vcenter = 5.2\n",
"for i in range(len(trajectoriesReal)):\n",
" x, y, line = to_line(trajectoriesReal[i+1], -2)\n",
" ax1.add_line(line)\n",
" \n",
"ax1.set_xlim(x_rcenter-5, x_rcenter+5)\n",
"ax1.set_ylim(y_rcenter-4, y_rcenter+4)\n",
"ax1.set_aspect(1)\n",
"\n",
"fig2 = plt.figure(figsize=(10,10))\n",
"ax2 = fig2.add_subplot(111)\n",
"\n",
"for i in range(len(trajectories)):\n",
" x, y, line = to_line(trajectories[i+1], 14)\n",
" ax2.add_line(line)\n",
"\n",
"ax2.set_xlim(x_vcenter-5, x_vcenter+5)\n",
"ax2.set_ylim(y_vcenter-4, y_vcenter+4)\n",
"ax2.set_aspect(1)\n",
"\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"times = np.arange(0,80,2)\n",
"y = list(map(lambda t: inter_agent_d(trajectories, t), times))\n",
"plt.plot(times, y, 'o')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"start_time(trajectories[1])\n",
"print(max_start_time(trajectories))\n",
"print(min_end_time(trajectories))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(position(map(lambda traj: traj[\"startTime\"], trajectories)[1], 0))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"[] is not None"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"min(trajectories.keys())"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"np.arange(14,15,0.4)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"range(1, 5)[0]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"for i in set(range(1,10)):\n",
" print(i)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"print(greedy_match(trajectories, trajectories, np.arange(14,15,0.4), euclid_d))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.6.8"
}
},
"nbformat": 4,
"nbformat_minor": 2
}
%% Cell type:code id: tags:
```
python
import
json
import
numpy
as
np
import
pandas
as
pd
import
math
import
matplotlib.pyplot
as
plt
from
matplotlib.lines
import
Line2D
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
]
def
get_footstep
(
trajectory
,
i
):
return
trajectory
[
i
];
def
start_time
(
trajectory
):
return
trajectory
[
0
][
'startTime'
];
def
max_start_time
(
trajectories
):
return
max
(
map
(
lambda
i
:
start_time
(
trajectories
[
i
]),
trajectories
))
def
end_time
(
trajectory
):
return
trajectory
[
-
1
][
'endTime'
];
def
min_end_time
(
trajectories
):
return
min
(
map
(
lambda
i
:
end_time
(
trajectories
[
i
]),
trajectories
))
def
length
(
fs
):
start
=
fs
[
'start'
];
end
=
fs
[
'end'
];
x1
=
start
[
'x'
];
y1
=
start
[
'y'
];
x2
=
end
[
'x'
];
y2
=
end
[
'y'
];
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'
];
return
np
.
array
([
x2
-
x1
,
y2
-
y1
]);
def
duration
(
fs
):
startTime
=
fs
[
'startTime'
];
endTime
=
fs
[
'endTime'
];
return
endTime
-
startTime
;
def
speed
(
fs
):
return
length
(
fs
)
/
duration
(
fs
);
def
is_between
(
fs
,
time
):
startTime
=
fs
[
'startTime'
];
endTime
=
fs
[
'endTime'
];
return
startTime
<=
time
and
time
<
endTime
;
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
];
def
position
(
trajectory
,
time
):
fs
=
footstep
(
trajectory
,
time
);
if
fs
!=
None
:
startTime
=
fs
[
'startTime'
];
endTime
=
fs
[
'endTime'
];
dur
=
duration
(
fs
);
partial_dur
=
time
-
startTime
;
ratio
=
partial_dur
/
dur
;
start
=
fs
[
'start'
];
x1
=
start
[
'x'
];
y1
=
start
[
'y'
];
l
=
length
(
fs
);
if
l
==
0.0
:
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
)])
eT
=
min
([
end_time
(
traj1
),
end_time
(
traj2
)])
filtered_times
=
list
(
filter
(
lambda
t
:
t
>=
sT
and
t
<=
eT
,
times
))
overlaps
=
len
(
filtered_times
)
if
overlaps
==
0
:
return
0
return
sum
(
map
(
lambda
t
:
np
.
linalg
.
norm
(
position
(
traj1
,
t
)
-
position
(
traj2
,
t
)),
filtered_times
))
/
overlaps
def
euclid_path_length
(
traj1
,
traj2
,
times
):
sT
=
max
([
start_time
(
traj1
),
start_time
(
traj2
)]);
eT
=
min
([
end_time
(
traj1
),
end_time
(
traj2
)]);
filtered_times
=
list
(
filter
(
lambda
t
:
t
>=
sT
and
t
<=
eT
,
times
));
s
=
np
.
array
([
0
,
0
])
for
i
in
range
(
len
(
filtered_times
)
-
1
):
t1
=
filtered_times
[
i
]
t2
=
filtered_times
[
i
+
1
]
d1
=
position
(
traj1
,
t1
)
-
position
(
traj1
,
t2
)
d2
=
position
(
traj2
,
t1
)
-
position
(
traj2
,
t2
)
diff
=
d1
-
d2
s
=
s
+
diff
return
s
;
def
inter_agent_d
(
trajectories
,
t
):
s
=
0
min_index
=
min
(
trajectories
.
keys
())
c
=
0
for
i
in
range
(
len
(
trajectories
)):
pos1
=
position
(
trajectories
[
i
+
min_index
],
t
)
for
j
in
range
(
i
+
1
,
len
(
trajectories
)):
pos2
=
position
(
trajectories
[
j
+
min_index
],
t
)
if
pos1
is
not
None
and
pos2
is
not
None
:
s
=
s
+
np
.
linalg
.
norm
(
pos1
-
pos2
)
c
=
c
+
1
if
c
==
0
:
return
0
else
:
return
s
/
c
def
total_inter_agent
(
trajectories1
,
trajectories2
,
times
):
return
sum
(
map
(
lambda
t
:
inter_agent_d
(
trajectories1
,
t
)
-
inter_agent_d
(
trajectories2
,
t
),
times
))
/
len
(
times
)
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
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
def
overlap
(
traj1
,
traj2
,
dt
):
return
True
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
)
data
[
'x'
]
=
data
[
'x'
]
/
100
data
[
'y'
]
=
data
[
'y'
]
/
100
data
[
'timeStep'
]
=
data
[
'timeStep'
]
/
fps
return
data
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