def update(self, observations):
self.observedX = observations[:, 0]
self.observedY = observations[:, 1]
self.observedT = observations[:, 2]
# Update each regressor with its corresponding observations
for i in range(self.goalsData.goals_n):
if self.gpTrajectoryRegressor[i] is not None:
goalCenter, __ = goal_center_and_size(
self.goalsData.goals_areas[i][1:])
distToGoal = euclidean_distance(
[self.observedX[-1], self.observedY[-1]], goalCenter)
dist = euclidean_distance(
[self.observedX[0], self.observedY[0]], goalCenter)
# Update observations and re-compute the kernel matrices
self.gpTrajectoryRegressor[i].update_observations(observations)
# Compute the model likelihood
self.goalsLikelihood[i] = self.gpTrajectoryRegressor[
i].compute_likelihood()
# Sum of the likelihoods
s = sum(self.goalsLikelihood)
# Compute the mean likelihood
self.meanLikelihood = mean(self.goalsLikelihood)
# Maintain a list of likely goals
for i in range(self.goalsData.goals_n):
self.goalsLikelihood[i] /= s
if (self.goalsLikelihood[i] > 0.85 * self.meanLikelihood):
self.likelyGoals.append(i)
# Save most likely goal
mostLikely = 0
for i in range(self.goalsData.goals_n):
if self.goalsLikelihood[i] > self.goalsLikelihood[mostLikely]:
mostLikely = i
self.mostLikelyGoal = mostLikely
return self.goalsLikelihood
def interaction_potential_DCA(fi, fj):
i, j = 0, 0
n, m = len(fi[2]), len(fj[2])
potentialVal = 0.
dca = 1100
while (i < n and j < m):
ti, tj = fi[2][i], fj[2][j]
if ti < tj:
if ti > fj[2][j - 1]:
val = (ti - tj) / (fj[2][j - 1] - tj)
x, y = get_approximation(val, fj, j)
p = [fi[0][i], fi[1][i]]
q = [x, y]
dist = euclidean_distance(p, q)
if dist < dca:
potentialVal = potential_value([fi[0][i], fi[1][i]],
[x, y])
dca = dist
if i < n:
i += 1
elif tj < ti:
if tj > fi[2][i - 1]:
val = (tj - ti) / (fi[2][i - 1] - ti)
x, y = get_approximation(val, fi, i)
p = [fj[0][j], fj[1][j]]
q = [x, y]
dist = euclidean_distance(p, q)
if dist < dca:
potentialVal = potential_value([fj[0][j], fj[1][j]],
[x, y])
dca = dist
if j < m:
j += 1
else:
p = [fj[0][j], fj[1][j]]
q = [fi[0][i], fi[1][i]]
dist = euclidean_distance(p, q)
if dist < dca:
dca = dist
potentialVal = potential_value([fj[0][j], fj[1][j]],
[fi[0][i], fi[1][i]])
if i < n:
i += 1
if j < m:
j += 1
return potentialVal
def compute_euclidean_distances(self):
for i in range(self.goals_n):
# Take the centroid of the ROI i
p = goal_centroid(self.goals_areas[i][1:])
for j in range(self.goals_n):
# Take the centroid of the ROI j
q = goal_centroid(self.goals_areas[j][1:])
d = euclidean_distance(p, q)
self.euclideanDistances[i][j] = d
def prediction_set_arclength(self, lastObs, finishPoint):
# Coordinates of the last observed point
x, y, l = lastObs[0], lastObs[1], lastObs[2]
print("[INF] Current point ", x, y)
print("[INF] Last point ", finishPoint)
# Euclidean distance between the last observed point and the finish point
euclideanDist = euclidean_distance([x, y], finishPoint)
# Rough estimate of the remaining arc length
distToGoal = euclideanDist * self.distUnit
size = int(distToGoal * self.stepUnit)
predset = np.zeros((size, 1))
if size > 0:
step = distToGoal / float(size)
for i in range(1, size + 1):
predset[i - 1, 0] = l + i * step
return predset, l + distToGoal, distToGoal
def interaction_potential_DCA_(fi, fj):
minDist = -1.
iMin, jMin = 0, 0
for i in range(len(fi[0])):
for j in range(len(fj[0])):
p = [fi[0][i], fi[1][i]]
q = [fj[0][j], fj[1][j]]
dist = euclidean_distance(p, q)
if minDist < 0:
minDist = dist
elif dist < minDist:
minDist = dist
iMin = i
jMin = j
p = [fi[0][iMin], fi[1][iMin]]
q = [fj[0][jMin], fj[1][jMin]]
val = potential_value(p, q)
return val
def prediction_set_time(self, lastObs, finishPoint, elapsedTime, timeStep):
# Time of the last observed point
x, y, t = lastObs[0], lastObs[1], lastObs[2]
distToGoal = euclidean_distance([x, y], finishPoint)
# TODO: I think we should first do here a fully deterministic model (conditioned on the mean transition time)
# Sample a duration
transitionTime = int(
np.random.normal(self.timeTransitionMean, self.timeTransitionStd))
# Remaining time
remainingTime = transitionTime - elapsedTime
#!Problem: timeTransitionMean <= elapsedTime
if remainingTime <= 0:
return np.zeros((0, 1)), 0, 0
size = int(remainingTime / timeStep)
predset = np.zeros((size, 1))
if size > 0:
for i in range(1, size + 1):
predset[i - 1, 0] = t + i * timeStep
if predset[-1, 0] < t + remainingTime:
predset[-1, 0] = t + remainingTime
return predset, t + remainingTime, distToGoal
def potential_value(p, q):
alpha, h = 0.9, 10.
val = 1. - alpha * math.exp(
(-1.) * (1. / (2 * h**2)) * euclidean_distance(p, q))
return val