Mercurial > hg > nsaunier > 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 |
