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