Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 887:e2452abba0e7
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) |