def e_erp(t0, t1, g):
"""
Usage
-----
The Edit distance with Real Penalty between trajectory t0 and t1.
Parameters
----------
param t0 : len(t0)x2 numpy_array
param t1 : len(t1)x2 numpy_array
Returns
-------
dtw : float
The Dynamic-Time Warping distance between trajectory t0 and t1
"""
n0 = len(t0)
n1 = len(t1)
C = np.zeros((n0 + 1, n1 + 1))
gt0_dist = map(lambda x: abs(eucl_dist(g, x)), t0)
gt1_dist = map(lambda x: abs(eucl_dist(g, x)), t1)
mdist = eucl_dist_traj(t0, t1)
C[1:, 0] = sum(gt0_dist)
C[0, 1:] = sum(gt1_dist)
for i in np.arange(n0) + 1:
for j in np.arange(n1) + 1:
derp0 = C[i - 1, j] + gt0_dist[i - 1]
derp1 = C[i, j - 1] + gt1_dist[j - 1]
derp01 = C[i - 1, j - 1] + mdist[i - 1, j - 1]
C[i, j] = min(derp0, derp1, derp01)
erp = C[n0, n1]
return erp
def e_hausdorff(t1, t2):
"""
Usage
-----
hausdorff distance between trajectories t1 and t2.
Parameters
----------
param t1 : len(t1)x2 numpy_array
param t2 : len(t2)x2 numpy_array
Returns
-------
h : float, hausdorff from trajectories t1 and t2
"""
mdist = eucl_dist_traj(t1, t2)
l_t1 = len(t1)
l_t2 = len(t2)
# t1_dist = map(lambda it1: eucl_dist(t1[it1], t1[it1 + 1]), range(l_t1 - 1))
# t2_dist = map(lambda it2: eucl_dist(t2[it2], t2[it2 + 1]), range(l_t2 - 1))
t1_dist = [eucl_dist(t1[it1], t1[it1 + 1]) for it1 in range(l_t1 - 1)]
t2_dist = [eucl_dist(t2[it2], t2[it2 + 1]) for it2 in range(l_t2 - 1)]
h = max(e_directed_hausdorff(t1, t2, mdist, l_t1, l_t2, t2_dist),
e_directed_hausdorff(t2, t1, mdist.T, l_t2, l_t1, t1_dist))
return h
def e_sspd(t1, t2):
"""
Usage
-----
The sspd-distance between trajectories t1 and t2.
The sspd-distance isjthe mean of the spd-distance between of t1 from t2 and the spd-distance of t2 from t1.
Parameters
----------
param t1 : len(t1)x2 numpy_array
param t2 : len(t2)x2 numpy_array
Returns
-------
sspd : float
sspd-distance of trajectory t2 from trajectory t1
"""
mdist = eucl_dist_traj(t1, t2)
l_t1 = len(t1)
l_t2 = len(t2)
# t1_dist_orig = map(lambda it1: eucl_dist(t1[it1], t1[it1 + 1]), range(l_t1 - 1))
# t2_dist_orig = map(lambda it2: eucl_dist(t2[it2], t2[it2 + 1]), range(l_t2 - 1))
t1_dist = [eucl_dist(t1[it1], t1[it1 + 1]) for it1 in range(l_t1 - 1)]
t2_dist = [eucl_dist(t2[it2], t2[it2 + 1]) for it2 in range(l_t2 - 1)]
sspd = (e_spd(t1, t2, mdist, l_t1, l_t2, t2_dist) +
e_spd(t2, t1, mdist.T, l_t2, l_t1, t1_dist)) / 2
return sspd
def compute_critical_values(P, Q, p, q, mdist, P_dist, Q_dist):
"""
Usage
-----
Compute all the critical values between trajectories P and Q
Parameters
----------
param P : px2 numpy_array, Trajectory P
param Q : qx2 numpy_array, Trajectory Q
param p : int, number of points in Trajectory P
param q : int, number of points in Trajectory Q
mdist : p x q numpy array, pairwise distance between points of trajectories t1 and t2
param P_dist: p x 1 numpy_array, distances between consecutive points in P
param Q_dist: q x 1 numpy_array, distances between consecutive points in Q
Returns
-------
cc : list, all critical values between trajectories P and Q
"""
origin = eucl_dist(P[0], Q[0])
end = eucl_dist(P[-1], Q[-1])
end_point = max(origin, end)
cc = set([end_point])
for i in range(p - 1):
for j in range(q - 1):
Lij = point_to_seg(Q[j], P[i], P[i + 1], mdist[i, j], mdist[i + 1, j], P_dist[i])
if Lij > end_point:
cc.add(Lij)
Bij = point_to_seg(P[i], Q[j], Q[j + 1], mdist[i, j], mdist[i, j + 1], Q_dist[j])
if Bij > end_point:
cc.add(Bij)
return sorted(list(cc))
def e_edr(t0, t1, eps):
"""
Usage
-----
The Edit Distance on Real sequence between trajectory t0 and t1.
Parameters
----------
param t0 : len(t0)x2 numpy_array
param t1 : len(t1)x2 numpy_array
eps : float
Returns
-------
edr : float
The Longuest-Common-Subsequence distance between trajectory t0 and t1
"""
n0 = len(t0)
n1 = len(t1)
# An (m+1) times (n+1) matrix
C = [[0] * (n1 + 1) for _ in range(n0 + 1)]
for i in range(1, n0 + 1):
for j in range(1, n1 + 1):
if eucl_dist(t0[i - 1], t1[j - 1]) < eps:
subcost = 0
else:
subcost = 1
C[i][j] = min(C[i][j - 1] + 1, C[i - 1][j] + 1,
C[i - 1][j - 1] + subcost)
edr = float(C[n0][n1]) / max([n0, n1])
return edr
def e_dtw(t0, t1):
"""
Usage
-----
The Dynamic-Time Warping distance between trajectory t0 and t1.
Parameters
----------
param t0 : len(t0)x2 numpy_array
param t1 : len(t1)x2 numpy_array
Returns
-------
dtw : float
The Dynamic-Time Warping distance between trajectory t0 and t1
"""
n0 = len(t0)
n1 = len(t1)
C = np.zeros((n0 + 1, n1 + 1))
C[1:, 0] = float('inf')
C[0, 1:] = float('inf')
for i in np.arange(n0) + 1:
for j in np.arange(n1) + 1:
C[i, j] = eucl_dist(t0[i - 1], t1[j - 1]) + min(C[i, j - 1], C[i - 1, j - 1], C[i - 1, j])
dtw = C[n0, n1]
return dtw
def frechet(P, Q):
"""
Usage
-----
Compute the frechet distance between trajectories P and Q
Parameters
----------
param P : px2 numpy_array, Trajectory P
param Q : qx2 numpy_array, Trajectory Q
Returns
-------
frech : float, the frechet distance between trajectories P and Q
"""
p = len(P)
q = len(Q)
mdist = eucl_dist_traj(P, Q)
#P_dist = map(lambda ip: eucl_dist(P[ip], P[ip + 1]), range(p - 1))
#Q_dist = map(lambda iq: eucl_dist(Q[iq], Q[iq + 1]), range(q - 1))
P_dist = [eucl_dist(P[ip], P[ip + 1]) for ip in range(p-1)]
Q_dist = [eucl_dist(Q[iq], Q[iq + 1]) for iq in range(q-1)]
cc = compute_critical_values(P, Q, p, q, mdist, P_dist, Q_dist)
eps = cc[0]
while (len(cc) != 1):
m_i = int(len(cc) / 2 - 1)
eps = cc[m_i]
rep = decision_problem(P, Q, p, q, eps, mdist, P_dist, Q_dist)
if rep:
cc = cc[:m_i + 1]
else:
cc = cc[m_i + 1:]
frech = eps
return frech
def ordered_mixed_distance(si, ei, sj, ej, siei, sjej, siei_norm_2,
sjej_norm_2):
siei_norm = math.sqrt(siei_norm_2)
sjej_norm = math.sqrt(sjej_norm_2)
sisj = sj - si
siej = ej - si
u1 = (sisj[0] * siei[0] + sisj[1] * siei[1]) / siei_norm_2
u2 = (siej[0] * siei[0] + siej[1] * siei[1]) / siei_norm_2
ps = si + u1 * siei
pe = si + u2 * siei
cos_theta = max(
-1,
min(1,
(sjej[0] * siei[0] + sjej[1] * siei[1]) / (siei_norm * sjej_norm)))
theta = math.acos(cos_theta)
# perpendicular distance
lpe1 = eucl_dist(sj, ps)
lpe2 = eucl_dist(ej, pe)
if lpe1 == 0 and lpe2 == 0:
dped = 0
else:
dped = (lpe1 * lpe1 + lpe2 * lpe2) / (lpe1 + lpe2)
# parallel_distance
lpa1 = min(eucl_dist(si, ps), eucl_dist(ei, ps))
lpa2 = min(eucl_dist(si, pe), eucl_dist(ei, pe))
dpad = min(lpa1, lpa2)
# angle_distance
if 0 <= theta < HPI:
dad = sjej_norm * math.sin(theta)
elif HPI <= theta <= PI:
dad = sjej_norm
else:
raise ValueError("WRONG THETA")
fdist = (dped + dpad + dad) / 3
return fdist
def free_line(p, eps, s, dps1, dps2, ds):
"""
Usage
-----
Return the free space in the segment s, from point p.
This free space is the set of all point in s whose distance from p is at most eps.
Since s is a segment, the free space is also a segment.
We return a 1x2 array whit the fraction of the segment s which are in the free space.
If no part of s are in the free space, return [-1,-1]
Parameters
----------
param p : 1x2 numpy_array, centre of the circle
param eps : float, radius of the circle
param s : 2x2 numpy_array, line
Returns
-------
lf : 1x2 numpy_array
fraction of segment which is in the free space (i.e [0.3,0.7], [0.45,1], ...)
If no part of s are in the free space, return [-1,-1]
"""
px = p[0]
py = p[1]
s1x = s[0, 0]
s1y = s[0, 1]
s2x = s[1, 0]
s2y = s[1, 1]
if s1x == s2x and s1y == s2y:
if eucl_dist(p, s[0]) > eps:
lf = [-1, -1]
else:
lf = [0, 1]
else:
if point_to_seg(p, s[0], s[1], dps1, dps2, ds) > eps:
# print("No Intersection")
lf = [-1, -1]
else:
segl = eucl_dist(s[0], s[1])
segl2 = segl * segl
intersect = circle_line_intersection(px, py, s1x, s1y, s2x, s2y, eps)
if intersect[0][0] != intersect[1][0] or intersect[0][1] != intersect[1][1]:
i1x = intersect[0, 0]
i1y = intersect[0, 1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) * (s2y - s1y))) / segl2
i2x = intersect[1, 0]
i2y = intersect[1, 1]
u2 = (((i2x - s1x) * (s2x - s1x)) + ((i2y - s1y) * (s2y - s1y))) / segl2
ordered_point = sorted((0, 1, u1, u2))
lf = ordered_point[1:3]
else:
if px == s1x and py == s1y:
lf = [0, 0]
elif px == s2x and py == s2y:
lf = [1, 1]
else:
i1x = intersect[0][0]
i1y = intersect[0][1]
u1 = (((i1x - s1x) * (s2x - s1x)) + ((i1y - s1y) * (s2y - s1y))) / segl2
if 0 <= u1 <= 1:
lf = [u1, u1]
else:
lf = [-1, -1]
return lf