Mercurial Hosting > traffic-intelligence
changeset 987:f026ce2af637
found bug with direct ttc computation
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 07 Mar 2018 23:37:00 -0500 |
parents | 3be8aaa47651 |
children | dc0be55e2bf5 |
files | python/moving.py python/prediction.py python/tests/moving.txt scripts/process.py |
diffstat | 4 files changed, 68 insertions(+), 53 deletions(-) [+] |
line wrap: on
line diff
--- a/python/moving.py Tue Mar 06 23:54:10 2018 -0500 +++ b/python/moving.py Wed Mar 07 23:37:00 2018 -0500 @@ -318,6 +318,10 @@ return p1.x*p2.y-p1.y*p2.x @staticmethod + def parallel(p1, p2): + return Point.cross(p1, p2) == 0. + + @staticmethod def cosine(p1, p2): return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
--- a/python/prediction.py Tue Mar 06 23:54:10 2018 -0500 +++ b/python/prediction.py Wed Mar 07 23:37:00 2018 -0500 @@ -264,37 +264,6 @@ prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) return prototypeTrajectories -def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): - '''returns the lists of collision points and crossing zones''' - predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) - predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) - - collisionPoints = [] - crossingZones = [] - for et1 in predictedTrajectories1: - for et2 in predictedTrajectories2: - collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) - if collision: - collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) - elif computeCZ: # check if there is a crossing zone - # TODO same computation as PET with metric + concatenate past trajectory with future trajectory - cz = None - t1 = 0 - while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 - t2 = 0 - while not cz and t2 < timeHorizon: - cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) - if cz is not None: - deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() - crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) - t2 += 1 - t1 += 1 - - if debug: - savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) - - return currentInstant, collisionPoints, crossingZones - class PredictionParameters(object): def __init__(self, name, maxSpeed): @@ -305,33 +274,60 @@ return '{0} {1}'.format(self.name, self.maxSpeed) def generatePredictedTrajectories(self, obj, instant): - return [] + return None + + def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): + '''returns the lists of collision points and crossing zones''' + predictedTrajectories1 = self.generatePredictedTrajectories(obj1, currentInstant) + predictedTrajectories2 = self.generatePredictedTrajectories(obj2, currentInstant) + + collisionPoints = [] + if computeCZ: + crossingZones = [] + else: + crossingZones = None + for et1 in predictedTrajectories1: + for et2 in predictedTrajectories2: + collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon) + if collision: + collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t)) + elif computeCZ: # check if there is a crossing zone + # TODO same computation as PET with metric + concatenate past trajectory with future trajectory + cz = None + t1 = 0 + while not cz and t1 < timeHorizon: # t1 <= timeHorizon-1 + t2 = 0 + while not cz and t2 < timeHorizon: + cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) + if cz is not None: + deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2() + crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV))) + t2 += 1 + t1 += 1 + + if debug: + savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon) + + return collisionPoints, crossingZones def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None):#, nProcesses = 1): '''Computes all crossing and collision points at each common instant for two road users. ''' - collisionPoints={} - crossingZones={} + collisionPoints = {} + if computeCZ: + crossingZones = {} + else: + crossingZones = None if timeInterval: commonTimeInterval = timeInterval else: commonTimeInterval = obj1.commonTimeInterval(obj2) #if nProcesses == 1: for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors - i, cp, cz = computeCrossingsCollisionsAtInstant(self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) + cp, cz = self.computeCrossingsCollisionsAtInstant(i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) if len(cp) != 0: collisionPoints[i] = cp - if len(cz) != 0: + if computeCZ and len(cz) != 0: crossingZones[i] = cz - # else: - # pool = Pool(processes = nProcesses) - # jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] - # for j in jobs: - # i, cp, cz = j.get() - # if len(cp) != 0: - # collisionPoints[i] = cp - # if len(cz) != 0: - # crossingZones[i] = cz - # pool.close() return collisionPoints, crossingZones def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): @@ -475,7 +471,10 @@ def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, *kwargs): collisionPoints = [] - crossingZones = [] + if computeCZ: + crossingZones = [] + else: + crossingZones = None p1 = obj1.getPositionAtInstant(currentInstant) p2 = obj2.getPositionAtInstant(currentInstant) @@ -518,7 +517,7 @@ title('instant {0}'.format(currentInstant)) axis('equal') - return currentInstant, collisionPoints, crossingZones + return collisionPoints, crossingZones class CVExactPredictionParameters(PredictionParameters): '''Prediction parameters of prediction at constant velocity @@ -537,16 +536,16 @@ p2 = obj2.getPositionAtInstant(currentInstant) v1 = obj1.getVelocityAtInstant(currentInstant) v2 = obj2.getVelocityAtInstant(currentInstant) - intersection = moving.intersection(p1, p1+v1, p2, p2+v2) + #intersection = moving.intersection(p1, p1+v1, p2, p2+v2) - if intersection is not None: + if not moving.Point.parallel(v1, v2): ttc = moving.Point.timeToCollision(p1, p2, v1, v2, collisionDistanceThreshold) if ttc is not None: - collisionPoints = [SafetyPoint(intersection, 1., ttc)] + collisionPoints = [SafetyPoint((p1+(v1*ttc)+p2+(v2*ttc))*0.5, 1., ttc)] else: pass # compute pPET - return currentInstant, collisionPoints, crossingZones + return collisionPoints, crossingZones class PrototypePredictionParameters(PredictionParameters): def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True):
--- a/python/tests/moving.txt Tue Mar 06 23:54:10 2018 -0500 +++ b/python/tests/moving.txt Wed Mar 07 23:37:00 2018 -0500 @@ -160,6 +160,14 @@ True >>> Point.midPoint(p1, p2) (0.500000,0.500000) +>>> p1=Point(0.,0.) +>>> p2=Point(5.,0.) +>>> v1 = Point(2.,0.) +>>> v2 = Point(1.,0.) +>>> Point.timeToCollision(p1, p2, v1, v2, 0.) +5.0 +>>> Point.timeToCollision(p1, p2, v1, v2, 1.) +4.0 >>> objects = storage.loadTrajectoriesFromSqlite('../samples/laurier.sqlite', 'object') >>> len(objects)
--- a/scripts/process.py Tue Mar 06 23:54:10 2018 -0500 +++ b/scripts/process.py Wed Mar 07 23:37:00 2018 -0500 @@ -13,6 +13,7 @@ parser = argparse.ArgumentParser(description='This program manages the processing of several files based on a description of the sites and video data in an SQLite database following the metadata module.') parser.add_argument('--db', dest = 'metadataFilename', help = 'name of the metadata file', required = True) parser.add_argument('--videos', dest = 'videoIds', help = 'indices of the video sequences', nargs = '*', type = int) +parser.add_argument('--prediction-method', dest = 'predictionMethod', help = 'prediction method (constant velocity (cvd: vector computation (approximate); cve: equation solving; cv: discrete time (approximate)), normal adaptation, point set prediction)', choices = ['cvd', 'cve', 'cv', 'na', 'ps', 'mp']) parser.add_argument('--pet', dest = 'computePET', help = 'computes PET', action = 'store_true') parser.add_argument('--delete', dest = 'delete', help = 'data to delete', choices = ['feature', 'object', 'classification', 'interaction']) parser.add_argument('--process', dest = 'process', help = 'data to process', choices = ['feature', 'object', 'classification', 'interaction']) @@ -40,7 +41,10 @@ if args.process == 'interaction': # safety analysis TODO make function in safety analysis script - predictionParameters = prediction.CVDirectPredictionParameters()#prediction.CVExactPredictionParameters() + if args.predictionMethod == 'cvd': + predictionParameters = prediction.CVDirectPredictionParameters() + if args.predictionMethod == 'cve': + predictionParameters = prediction.CVExactPredictionParameters() for videoId in args.videoIds: vs = session.query(VideoSequence).get(videoId) print('Processing '+vs.getDatabaseFilename())