Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 887:e2452abba0e7

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added option to compute PET in safety analysis script, and save in database
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 22 Mar 2017 10:44:24 -0400
parents 8ba82b371eea
children ff92801e5c54
comparison
equal deleted inserted replaced
886:d2eb8c93f7de 887:e2452abba0e7
3 3
4 import utils, cvutils 4 import utils, cvutils
5 from base import VideoFilenameAddable 5 from base import VideoFilenameAddable
6 6
7 from math import sqrt, atan2, cos, sin 7 from math import sqrt, atan2, cos, sin
8 from numpy import median, array, zeros, hypot, NaN, std, floor, float32 8 from numpy import median, array, zeros, hypot, NaN, std, floor, float32, argwhere
9 from matplotlib.pyplot import plot, text 9 from matplotlib.pyplot import plot, text
10 from scipy.stats import scoreatpercentile 10 from scipy.stats import scoreatpercentile
11 from scipy.spatial.distance import cdist 11 from scipy.spatial.distance import cdist
12 from scipy.signal import savgol_filter 12 from scipy.signal import savgol_filter
13 13
1424 1424
1425 @staticmethod 1425 @staticmethod
1426 def computePET(obj1, obj2, collisionDistanceThreshold): 1426 def computePET(obj1, obj2, collisionDistanceThreshold):
1427 '''Post-encroachment time based on distance threshold 1427 '''Post-encroachment time based on distance threshold
1428 1428
1429 Returns the smallest time difference when the object positions are within collisionDistanceThreshold''' 1429 Returns the smallest time difference when the object positions are within collisionDistanceThreshold
1430 and the instants at which each object is passing through its corresponding position'''
1430 positions1 = [p.astuple() for p in obj1.getPositions()] 1431 positions1 = [p.astuple() for p in obj1.getPositions()]
1431 positions2 = [p.astuple() for p in obj2.getPositions()] 1432 positions2 = [p.astuple() for p in obj2.getPositions()]
1432 pets = zeros((int(obj1.length()), int(obj2.length()))) 1433 n1 = len(positions1)
1434 n2 = len(positions2)
1435 pets = zeros((n1, n2))
1433 for i,t1 in enumerate(obj1.getTimeInterval()): 1436 for i,t1 in enumerate(obj1.getTimeInterval()):
1434 for j,t2 in enumerate(obj2.getTimeInterval()): 1437 for j,t2 in enumerate(obj2.getTimeInterval()):
1435 pets[i,j] = abs(t1-t2) 1438 pets[i,j] = abs(t1-t2)
1436 distances = cdist(positions1, positions2, metric = 'euclidean') 1439 distances = cdist(positions1, positions2, metric = 'euclidean')
1437 if distances.min() <= collisionDistanceThreshold: 1440 smallDistances = (distances <= collisionDistanceThreshold)
1438 #idx = distances.argmin() 1441 if smallDistances.any():
1439 return pets[distances <= collisionDistanceThreshold].min() 1442 smallPets = pets[smallDistances]
1440 else: 1443 petIdx = smallPets.argmin()
1441 return None 1444 distanceIndices = argwhere(smallDistances)[petIdx]
1445 return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1]
1446 else:
1447 return None, None, None
1442 1448
1443 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)): 1449 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
1444 '''Predicts the position of object at instant+deltaT, 1450 '''Predicts the position of object at instant+deltaT,
1445 at constant speed''' 1451 at constant speed'''
1446 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration) 1452 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)