# HG changeset patch
# User MohamedGomaa
# Date 1416973787 18000
# Node ID 84690dfe55605bce5f09a18c0acabfda3b8c7f08
# Parent  75ad9c0d6cc328d54d8a33969c5537d9aefcd08b
add some functions for behaviour analysis

diff -r 75ad9c0d6cc3 -r 84690dfe5560 c/Makefile
--- 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++
 
diff -r 75ad9c0d6cc3 -r 84690dfe5560 python/events.py
--- 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	
 
diff -r 75ad9c0d6cc3 -r 84690dfe5560 python/moving.py
--- 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'])
diff -r 75ad9c0d6cc3 -r 84690dfe5560 python/prediction.py
--- 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
diff -r 75ad9c0d6cc3 -r 84690dfe5560 python/storage.py
--- 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
diff -r 75ad9c0d6cc3 -r 84690dfe5560 python/utils.py
--- 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
         '''