Mercurial Hosting > traffic-intelligence
changeset 607:84690dfe5560
add some functions for behaviour analysis
| author | MohamedGomaa |
|---|---|
| date | Tue, 25 Nov 2014 22:49:47 -0500 |
| parents | 75ad9c0d6cc3 |
| children | 078adacd72a4 0dc36203973d |
| files | c/Makefile python/events.py python/moving.py python/prediction.py python/storage.py python/utils.py |
| diffstat | 6 files changed, 180 insertions(+), 32 deletions(-) [+] |
line wrap: on
line diff
--- a/c/Makefile Mon Nov 24 13:02:10 2014 -0500 +++ b/c/Makefile Tue Nov 25 22:49:47 2014 -0500 @@ -1,6 +1,6 @@ EXE_DIR=../bin SCRIPTS_DIR=../scripts -TRAJECTORYMANAGEMENT_DIR=$(HOME)/Research/Code/trajectorymanagementandanalysis/trunk/src/TrajectoryManagementAndAnalysis +TRAJECTORYMANAGEMENT_DIR=D:/00-trackingSoftware/trajectories-trajectorymanagementandanalysis/trunk/src/TrajectoryManagementAndAnalysis CXX = g++
--- a/python/events.py Mon Nov 24 13:02:10 2014 -0500 +++ b/python/events.py Tue Nov 25 22:49:47 2014 -0500 @@ -8,8 +8,10 @@ import multiprocessing import itertools +import sys import moving, prediction, indicators, utils - +sys.path.append("D:/behaviourAnalysis/libs") +import trajLearning __metaclass__ = type class Interaction(moving.STObject): @@ -147,6 +149,67 @@ pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i]) self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs)) + def computeCrosssingCollisionsPrototypeAtInstant(self, instant,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None, computeCZ = False, debug = False,useDestination=True,useSpeedPrototype=True): + inter1=moving.Interval(self.roadUser1.timeInterval.first,instant) + inter2=moving.Interval(self.roadUser2.timeInterval.first,instant) + partialObjPositions1= self.roadUser1.getObjectInTimeInterval(inter1).positions + partialObjPositions2= self.roadUser2.getObjectInTimeInterval(inter2).positions + if useSpeedPrototype: + prototypeTrajectories1=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route1,partialObjPositions1,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) + prototypeTrajectories2=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route2,partialObjPositions2,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination) + else: + prototypeTrajectories1=trajLearning.findPrototypes(prototypes,nMatching,objects,route1,partialObjPositions1,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) + prototypeTrajectories2=trajLearning.findPrototypes(prototypes,nMatching,objects,route2,partialObjPositions2,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched) + if prototypeTrajectories1=={}: + print self.roadUser1.num, 'is abnormal at instant', str(instant) + return [],[] + elif prototypeTrajectories2=={}: + print self.roadUser2.num, 'is abnormal at instant', str(instant) + return [],[] + else: + currentInstant,collisionPoints, crossingZones = predictionParameters.computeCrossingsCollisionsAtInstant(instant, self.roadUser1, self.roadUser2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) + return collisionPoints,crossingZones + + def computeCrossingsCollisionsPrototype2stages(self, predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,step=10,minSimilarity=0.1,mostMatched=None, computeCZ = False, debug = False, timeInterval = None,acceptPartialLength=30,useDestination=True,useSpeedPrototype=True): + '''Computes all crossing and collision points at each common instant for two road users. ''' + self.collisionPoints={} + self.crossingZones={} + TTCs = {} + route1=(self.roadUser1.startRouteID,self.roadUser1.endRouteID) + route2=(self.roadUser2.startRouteID,self.roadUser2.endRouteID) + if useDestination: + if route1 not in prototypes.keys(): + route1= trajLearning.findRoute(prototypes,objects,route1,self.roadUser1.num,noiseEntryNums,noiseExitNums) + if route2 not in prototypes.keys(): + route2= trajLearning.findRoute(prototypes,objects,route2,self.roadUser2.num,noiseEntryNums,noiseExitNums) + + if timeInterval: + commonTimeInterval = timeInterval + else: + commonTimeInterval = self.timeInterval + reCompute=False + for i in xrange(commonTimeInterval.first,commonTimeInterval.last,step): # incremental calculation of CP,CZ to save time + if i-self.roadUser1.timeInterval.first >= acceptPartialLength and i-self.roadUser2.timeInterval.first >= acceptPartialLength: + self.collisionPoints[i], self.crossingZones[i] = self.computeCrosssingCollisionsPrototypeAtInstant(i,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched, computeCZ, debug,useDestination,useSpeedPrototype) + if len(self.collisionPoints[i]) >0 or len(self.crossingZones[i])>0: + reCompute=True + break + if reCompute: + for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors + if i-self.roadUser1.timeInterval.first >= acceptPartialLength and i-self.roadUser2.timeInterval.first >= acceptPartialLength: + self.collisionPoints[i], self.crossingZones[i] = self.computeCrosssingCollisionsPrototypeAtInstant(i,route1,route2,predictionParameters, collisionDistanceThreshold, timeHorizon, prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched, computeCZ, debug,useDestination,useSpeedPrototype) + if len(self.collisionPoints[i]) > 0: + TTCs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.collisionPoints[i]) + # add probability of collision, and probability of successful evasive action + self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[7], TTCs)) + + if computeCZ: + pPETs = {} + for i in list(commonTimeInterval)[:-1]: + if i in self.crossingZones.keys() and len(self.crossingZones[i]) > 0: + pPETs[i] = prediction.SafetyPoint.computeExpectedIndicator(self.crossingZones[i]) + self.addIndicator(indicators.SeverityIndicator(Interaction.indicatorNames[9], pPETs)) + def addVideoFilename(self,videoFilename): self.videoFilename= videoFilename
--- a/python/moving.py Mon Nov 24 13:02:10 2014 -0500 +++ b/python/moving.py Tue Nov 25 22:49:47 2014 -0500 @@ -5,7 +5,7 @@ import cvutils from math import sqrt -from numpy import median +from numpy import median,percentile try: from shapely.geometry import Polygon, Point as shapelyPoint @@ -790,7 +790,7 @@ return False def wiggliness(self): - return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) + return self.computeCumulativeDistances()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))) def getIntersections(self, p1, p2): '''Returns a list of the indices at which the trajectory @@ -1156,15 +1156,16 @@ ### # User Type Classification ### - def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, ignoreNInstantsAtEnds = 0): + def classifyUserTypeSpeedMotorized(self, threshold, percentileFactor=95, ignoreNInstantsAtEnds = 0): '''Classifies slow and fast road users slow: non-motorized -> pedestrians - fast: motorized -> cars''' + fast: motorized -> cars + The percentile function is the same as the median if percentileFactor=50, the same as the minimum if percentileFactor=0 and the same as the maximum if percentileFactor=100.''' if ignoreNInstantsAtEnds > 0: speeds = self.getSpeeds()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds] else: speeds = self.getSpeeds() - if aggregationFunc(speeds) >= threshold: + if percentile(speeds,percentileFactor) >= threshold: self.setUserType(userType2Num['car']) else: self.setUserType(userType2Num['pedestrian'])
--- a/python/prediction.py Mon Nov 24 13:02:10 2014 -0500 +++ b/python/prediction.py Tue Nov 25 22:49:47 2014 -0500 @@ -4,6 +4,7 @@ import moving import math import random +import numpy as np class PredictedTrajectory: '''Class for predicted trajectories with lazy evaluation @@ -46,6 +47,14 @@ def getControl(self): return self.control + +def findNearestParams(initialPosition,prototypeTrajectory): + ''' nearest parameters are the index of minDistance and the orientation ''' + distances=[] + for position in prototypeTrajectory.positions: + distances.append(moving.Point.distanceNorm2(initialPosition, position)) + minDistanceIndex= np.argmin(distances) + return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle class PredictedTrajectoryPrototype(PredictedTrajectory): '''Predicted trajectory that follows a prototype trajectory @@ -64,15 +73,31 @@ self.constantSpeed = constantSpeed self.probability = probability self.predictedPositions = {0: initialPosition} - self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)} - + self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)} + def predictPosition(self, nTimeSteps): if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys(): - if constantSpeed: + if self.constantSpeed: # calculate cumulative distance - pass + speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm + anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] + self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype) + self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) + else: # see c++ code, calculate ratio - pass + speedNorm= self.predictedSpeedOrientations[0].norm + instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0] + prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:] + ratio=float(speedNorm)/prototypeSpeeds[0] + resampledSpeeds=[sp*ratio for sp in prototypeSpeeds] + anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1] + if nTimeSteps<len(resampledSpeeds): + self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype) + self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) + else: + self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype) + self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None) + return self.predictedPositions[nTimeSteps] class PredictedTrajectoryRandomControl(PredictedTrajectory): @@ -141,10 +166,14 @@ savefig('predicted-trajectories-t-{0}.png'.format(currentInstant)) close() -def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): +def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None): '''returns the lists of collision points and crossing zones''' - predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) - predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) + if prototypeTrajectories1==None: + predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant) + else: + predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1) + predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2) collisionPoints = [] crossingZones = [] @@ -166,17 +195,17 @@ #if (et1.predictPosition(t1)-et2.predictPosition(t2)).norm2() < collisionDistanceThreshold: # cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5) cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1)) - if cz != None: - crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2))) + if cz: + 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 - return currentInstant, collisionPoints, crossingZones - -def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): +def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,prototypeTrajectories1=None,prototypeTrajectories2=None): '''Computes all crossing and collision points at each common instant for two road users. ''' collisionPoints={} crossingZones={} @@ -186,7 +215,7 @@ 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(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) + i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) if len(cp) != 0: collisionPoints[i] = cp if len(cz) != 0: @@ -194,7 +223,7 @@ else: from multiprocessing import Pool pool = Pool(processes = nProcesses) - jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] + jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2)) for i in list(commonTimeInterval)[:-1]] #results = [j.get() for j in jobs] #results.sort() for j in jobs: @@ -205,7 +234,7 @@ if len(cz) != 0: crossingZones[i] = cz pool.close() - return collisionPoints, crossingZones + return collisionPoints, crossingZones class PredictionParameters: def __init__(self, name, maxSpeed): @@ -218,11 +247,11 @@ def generatePredictedTrajectories(self, obj, instant): return [] - def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False): - return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug) + def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,prototypeTrajectories1=None,prototypeTrajectories2=None): + return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,prototypeTrajectories1,prototypeTrajectories2) - def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1): - return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses) + def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,prototypeTrajectories1=None,prototypeTrajectories2=None): + return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,prototypeTrajectories1,prototypeTrajectories2) def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None): '''Computes only collision probabilities @@ -428,10 +457,20 @@ #### # Other Methods #### - - - - +class prototypePredictionParameters(PredictionParameters): + def __init__(self, maxSpeed, nPredictedTrajectories,constantSpeed = True): + name = 'prototype' + PredictionParameters.__init__(self, name, maxSpeed) + self.nPredictedTrajectories = nPredictedTrajectories + self.constantSpeed = constantSpeed + + def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories): + predictedTrajectories = [] + initialPosition = obj.getPositionAtInstant(instant) + initialVelocity = obj.getVelocityAtInstant(instant) + for prototypeTraj in prototypeTrajectories.keys(): + predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) + return predictedTrajectories if __name__ == "__main__": import doctest
--- a/python/storage.py Mon Nov 24 13:02:10 2014 -0500 +++ b/python/storage.py Tue Nov 25 22:49:47 2014 -0500 @@ -167,6 +167,50 @@ connection.close() return labels +def writeSpeedPrototypeToSqlite(prototypes,nmatching, outFilename): + """ to match the format of second layer prototypes""" + connection = sqlite3.connect(outFilename) + cursor = connection.cursor() + + cursor.execute("CREATE TABLE IF NOT EXISTS \"speedprototypes\"(spdprototype_id INTEGER,prototype_id INTEGER,routeID_start INTEGER, routeID_end INTEGER, nMatching INTEGER, PRIMARY KEY(spdprototype_id))") + + for route in prototypes.keys(): + if prototypes[route]!={}: + for i in prototypes[route]: + if prototypes[route][i]!= []: + for j in prototypes[route][i]: + cursor.execute("insert into speedprototypes (spdprototype_id,prototype_id, routeID_start, routeID_end, nMatching) values (?,?,?,?,?)",(j,i,route[0],route[1],nmatching[j])) + + connection.commit() + connection.close() + +def loadSpeedPrototypeFromSqlite(filename): + """ + This function loads the prototypes table in the database of name <filename>. + """ + prototypes = {} + nMatching={} + connection = sqlite3.connect(filename) + cursor = connection.cursor() + + try: + cursor.execute('SELECT * from speedprototypes order by spdprototype_id,prototype_id, routeID_start, routeID_end, nMatching') + except sqlite3.OperationalError as error: + utils.printDBError(error) + return [] + + for row in cursor: + route=(row[2],row[3]) + if route not in prototypes.keys(): + prototypes[route]={} + if row[1] not in prototypes[route].keys(): + prototypes[route][row[1]]=[] + prototypes[route][row[1]].append(row[0]) + nMatching[row[0]]=row[4] + + connection.close() + return prototypes,nMatching + def writeRoutesToSqlite(Routes, outputFilename): """ This function writes the activity path define by start and end IDs""" @@ -794,6 +838,7 @@ return configDict + if __name__ == "__main__": import doctest import unittest
--- a/python/utils.py Mon Nov 24 13:02:10 2014 -0500 +++ b/python/utils.py Tue Nov 25 22:49:47 2014 -0500 @@ -363,7 +363,7 @@ based on the threshold on distance between two elements of lists l1, l2 similarityFunc returns True or False whether the two points are considered similar - if aligned, returns the best matching if using a finite delta by shiftinig the series alignments + if aligned, returns the best matching if using a finite delta by shifting the series alignments eg distance(p1, p2) < epsilon '''