Mercurial Hosting > traffic-intelligence
comparison python/prediction.py @ 943:b1e8453c207c
work on motion prediction using motion patterns
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 19 Jul 2017 18:02:38 -0400 |
parents | ab13aaf41432 |
children | 84ebe1b031f1 |
comparison
equal
deleted
inserted
replaced
942:ab13aaf41432 | 943:b1e8453c207c |
---|---|
80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() | 80 self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2() |
81 | 81 |
82 def predictPosition(self, nTimeSteps): | 82 def predictPosition(self, nTimeSteps): |
83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): | 83 if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): |
84 if self.constantSpeed: | 84 if self.constantSpeed: |
85 # calculate cumulative distance | |
86 traj = self.prototype.getPositions() | 85 traj = self.prototype.getPositions() |
87 trajLength = traj.length() | 86 trajLength = traj.length() |
88 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) | 87 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx) |
89 i = self.closestPointIdx | 88 i = self.closestPointIdx |
90 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: | 89 while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance: |
159 p2 = predictedTrajectory2.predictPosition(t) | 158 p2 = predictedTrajectory2.predictPosition(t) |
160 collision = (p1-p2).norm2() <= collisionDistanceThreshold | 159 collision = (p1-p2).norm2() <= collisionDistanceThreshold |
161 return collision, t, p1, p2 | 160 return collision, t, p1, p2 |
162 | 161 |
163 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): | 162 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon): |
164 from matplotlib.pyplot import figure, axis, title, close, savefig | 163 from matplotlib.pyplot import figure, axis, title, clf, savefig |
165 figure() | 164 figure() |
165 #clf() | |
166 for et in predictedTrajectories1: | 166 for et in predictedTrajectories1: |
167 et.predictPosition(int(np.round(timeHorizon))) | 167 et.predictPosition(int(np.round(timeHorizon))) |
168 et.plot('rx') | 168 et.plot('rx') |
169 | 169 |
170 for et in predictedTrajectories2: | 170 for et in predictedTrajectories2: |
171 et.predictPosition(int(np.round(timeHorizon))) | 171 et.predictPosition(int(np.round(timeHorizon))) |
172 et.plot('bx') | 172 et.plot('bx') |
173 obj1.plot('r') | 173 obj1.plot('r', withOrigin = True) |
174 obj2.plot('b') | 174 obj2.plot('b', withOrigin = True) |
175 title('instant {0}'.format(currentInstant)) | 175 title('instant {0}'.format(currentInstant)) |
176 axis('equal') | 176 axis('equal') |
177 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) | 177 savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) |
178 close() | |
179 | 178 |
180 def calculateProbability(nMatching,similarity,objects): | 179 def calculateProbability(nMatching,similarity,objects): |
181 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) | 180 sumFrequencies=sum([nMatching[p] for p in similarity.keys()]) |
182 prototypeProbability={} | 181 prototypeProbability={} |
183 for i in similarity.keys(): | 182 for i in similarity.keys(): |
253 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) | 252 prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) |
254 else: | 253 else: |
255 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) | 254 prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) |
256 return prototypeTrajectories | 255 return prototypeTrajectories |
257 | 256 |
258 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): | 257 def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
259 '''returns the lists of collision points and crossing zones''' | 258 '''returns the lists of collision points and crossing zones''' |
260 if usePrototypes: | 259 # if usePrototypes: |
261 prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 260 # prototypeTrajectories1 = getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
262 prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 261 # prototypeTrajectories2 = getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
263 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) | 262 # predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) |
264 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) | 263 # predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) |
265 else: | 264 # else: |
266 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) | 265 predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) |
267 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) | 266 predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) |
268 | 267 |
269 collisionPoints = [] | 268 collisionPoints = [] |
270 crossingZones = [] | 269 crossingZones = [] |
271 for et1 in predictedTrajectories1: | 270 for et1 in predictedTrajectories1: |
272 for et2 in predictedTrajectories2: | 271 for et2 in predictedTrajectories2: |
273 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) | 272 collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) |
274 | |
275 if collision: | 273 if collision: |
276 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) | 274 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) |
277 elif computeCZ: # check if there is a crossing zone | 275 elif computeCZ: # check if there is a crossing zone |
278 # TODO? zone should be around the points at which the traj are the closest | 276 # TODO? zone should be around the points at which the traj are the closest |
279 # look for CZ at different times, otherwise it would be a collision | 277 # look for CZ at different times, otherwise it would be a collision |
307 return '{0} {1}'.format(self.name, self.maxSpeed) | 305 return '{0} {1}'.format(self.name, self.maxSpeed) |
308 | 306 |
309 def generatePredictedTrajectories(self, obj, instant): | 307 def generatePredictedTrajectories(self, obj, instant): |
310 return [] | 308 return [] |
311 | 309 |
312 def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): | 310 # def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True): |
313 return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 311 # return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
314 | 312 |
315 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): | 313 def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):#,usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): |
316 #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): | 314 #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1): |
317 '''Computes all crossing and collision points at each common instant for two road users. ''' | 315 '''Computes all crossing and collision points at each common instant for two road users. ''' |
318 collisionPoints={} | 316 collisionPoints={} |
319 crossingZones={} | 317 crossingZones={} |
320 if timeInterval: | 318 if timeInterval: |
321 commonTimeInterval = timeInterval | 319 commonTimeInterval = timeInterval |
322 else: | 320 else: |
323 commonTimeInterval = obj1.commonTimeInterval(obj2) | 321 commonTimeInterval = obj1.commonTimeInterval(obj2) |
324 if nProcesses == 1: | 322 if nProcesses == 1: |
325 if usePrototypes: | 323 # if usePrototypes: |
326 firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) | 324 # firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last) |
327 commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors | 325 # commonTimeIntervalList1= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors |
328 commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors | 326 # commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors |
329 for i in commonTimeIntervalList2: | 327 # for i in commonTimeIntervalList2: |
330 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 328 # i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
331 if len(cp) != 0: | 329 # if len(cp) != 0: |
332 collisionPoints[i] = cp | 330 # collisionPoints[i] = cp |
333 if len(cz) != 0: | 331 # if len(cz) != 0: |
334 crossingZones[i] = cz | 332 # crossingZones[i] = cz |
335 if collisionPoints!={} or crossingZones!={}: | 333 # if collisionPoints!={} or crossingZones!={}: |
336 for i in commonTimeIntervalList1: | 334 # for i in commonTimeIntervalList1: |
337 if i not in commonTimeIntervalList2: | 335 # if i not in commonTimeIntervalList2: |
338 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 336 # i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
339 if len(cp) != 0: | 337 # if len(cp) != 0: |
340 collisionPoints[i] = cp | 338 # collisionPoints[i] = cp |
341 if len(cz) != 0: | 339 # if len(cz) != 0: |
342 crossingZones[i] = cz | 340 # crossingZones[i] = cz |
343 else: | 341 # else: |
344 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors | 342 for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors |
345 i, cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) | 343 i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)#,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype) |
346 if len(cp) != 0: | 344 if len(cp) != 0: |
347 collisionPoints[i] = cp | 345 collisionPoints[i] = cp |
348 if len(cz) != 0: | 346 if len(cz) != 0: |
349 crossingZones[i] = cz | 347 crossingZones[i] = cz |
350 else: | 348 else: |
351 pool = Pool(processes = nProcesses) | 349 pool = Pool(processes = nProcesses) |
352 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]] | 350 jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] #,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype |
353 #results = [j.get() for j in jobs] | 351 #results = [j.get() for j in jobs] |
354 #results.sort() | 352 #results.sort() |
355 for j in jobs: | 353 for j in jobs: |
356 i, cp, cz = j.get() | 354 i, cp, cz = j.get() |
357 #if len(cp) != 0 or len(cz) != 0: | 355 #if len(cp) != 0 or len(cz) != 0: |
435 probability, | 433 probability, |
436 maxSpeed = self.maxSpeed)) | 434 maxSpeed = self.maxSpeed)) |
437 return predictedTrajectories | 435 return predictedTrajectories |
438 | 436 |
439 class PointSetPredictionParameters(PredictionParameters): | 437 class PointSetPredictionParameters(PredictionParameters): |
440 # todo generate several trajectories with normal adaptatoins from each position (feature) | |
441 def __init__(self, maxSpeed): | 438 def __init__(self, maxSpeed): |
442 PredictionParameters.__init__(self, 'point set', maxSpeed) | 439 PredictionParameters.__init__(self, 'point set', maxSpeed) |
443 #self.nPredictedTrajectories = nPredictedTrajectories | 440 #self.nPredictedTrajectories = nPredictedTrajectories |
444 | 441 |
445 def generatePredictedTrajectories(self, obj, instant): | 442 def generatePredictedTrajectories(self, obj, instant): |
454 return predictedTrajectories | 451 return predictedTrajectories |
455 else: | 452 else: |
456 print('Object {} has no features'.format(obj.getNum())) | 453 print('Object {} has no features'.format(obj.getNum())) |
457 return None | 454 return None |
458 | 455 |
456 | |
459 class EvasiveActionPredictionParameters(PredictionParameters): | 457 class EvasiveActionPredictionParameters(PredictionParameters): |
460 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): | 458 def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False): |
461 '''Suggested acceleration distribution may not be symmetric, eg | 459 '''Suggested acceleration distribution may not be symmetric, eg |
462 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)''' | 460 lambda: random.triangular(self.minAcceleration, self.maxAcceleration, 0.)''' |
463 | 461 |
578 else: | 576 else: |
579 pass # compute pPET | 577 pass # compute pPET |
580 | 578 |
581 return currentInstant, collisionPoints, crossingZones | 579 return currentInstant, collisionPoints, crossingZones |
582 | 580 |
583 #### | |
584 # Other Methods | |
585 #### | |
586 class PrototypePredictionParameters(PredictionParameters): | 581 class PrototypePredictionParameters(PredictionParameters): |
587 def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True): | 582 def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters |
588 name = 'prototype' | 583 PredictionParameters.__init__(self, 'prototypes', None) |
589 PredictionParameters.__init__(self, name, maxSpeed) | 584 self.prototypes = prototypes |
590 self.nPredictedTrajectories = nPredictedTrajectories | 585 self.nPredictedTrajectories = nPredictedTrajectories |
586 self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance) | |
587 #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y) | |
588 self.minSimilarity = minSimilarity | |
589 self.minFeatureTime = minFeatureTime | |
591 self.constantSpeed = constantSpeed | 590 self.constantSpeed = constantSpeed |
591 self.useFeatures = useFeatures | |
592 | 592 |
593 def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): | 593 def generatePredictedTrajectories(self, obj, instant): |
594 predictedTrajectories = [] | 594 predictedTrajectories = [] |
595 initialPosition = obj.getPositionAtInstant(instant) | 595 if instant-obj.getFirstInstant()+1 >= self.minFeatureTime: |
596 initialVelocity = obj.getVelocityAtInstant(instant) | 596 if self.useFeatures and obj.hasFeatures(): |
597 for prototypeTraj in prototypeTrajectories.keys(): | 597 # get current features existing for the most time, sort on first instant of feature and take n first |
598 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) | 598 pass |
599 else: | |
600 if not hasattr(obj, 'prototypeSimilarities'): | |
601 obj.prototypeSimilarities = [] | |
602 for proto in self.prototypes: | |
603 self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T) | |
604 similarities = self.lcss.similarityTable[-1, :-1].astype(float) | |
605 obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities)))) | |
606 for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities): | |
607 if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity: | |
608 initialPosition = obj.getPositionAtInstant(instant) | |
609 initialVelocity = obj.getVelocityAtInstant(instant) | |
610 predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings())) | |
599 return predictedTrajectories | 611 return predictedTrajectories |
600 | 612 |
601 if __name__ == "__main__": | 613 if __name__ == "__main__": |
602 import doctest | 614 import doctest |
603 import unittest | 615 import unittest |