Mercurial Hosting > traffic-intelligence

annotate python/objectsmoothing.py @ 641:9fe254f11743

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cleaning under way and renaming
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 09 Apr 2015 16:55:24 +0200
parents dc2d0a0d7fe1
children e54751e71d61
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
1 import storage, moving, utils
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
2 from math import * #atan2,asin,degrees,sin,cos,pi
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
3 import numpy as np
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
4
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
5 import matplotlib.pyplot as plt
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
6
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
7 def findNearest(feat, featureSet,t,reverse=True):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
8 dist={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
9 for f in featureSet:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
10 if reverse:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
11 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
12 else:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
13 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
14 return min(dist, key=dist.get) # = utils.argmaxDict(dist)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
15
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
16 def getFeatures(obj,features,featureID):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
17 t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
18 listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
19 # find the features to fill in the beginning of the object existence
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
20 currentFeature = features[featureID]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
21 while t1!=obj.getFirstInstant():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
22 delta=listFeatures[-1][3]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
23 featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
24 feat = findNearest(currentFeature,featureSet,t1-1,reverse=True)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
25 if feat.existsAtInstant(t1):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
26 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
27 else:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
28 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
29 currentFeature = feat
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
30 t1= feat.getFirstInstant()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
31 # find the features to fill in the end of the object existence
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
32 delta=moving.Point(0,0)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
33 currentFeature = features[featureID]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
34 while t3!= obj.getLastInstant():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
35 featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
36 feat = findNearest(currentFeature,featureSet,t3+1,reverse=False)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
37 if feat.existsAtInstant(t3):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
38 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
39 else:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
40 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
41 currentFeature = feat
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
42 t3= feat.getLastInstant()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
43 delta=listFeatures[-1][3]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
44 return listFeatures
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
45
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
46 def buildFeature(obj,features,featureID,num=1):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
47 listFeatures= getFeatures(obj,features,featureID)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
48 tmp={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
49 delta={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
50 for i in listFeatures:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
51 for t in xrange(i[1],i[2]+1):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
52 tmp[t]=[i[0],i[3]]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
53 newTraj = moving.Trajectory()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
54
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
55 for instant in obj.getTimeInterval():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
56 newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
57 newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
58 return newFeature
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
59
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
60 def getBearing(p1,p2,p3):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
61 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
62 bearing1 = (90 - angle) % 360
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
63 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
64 bearing2 = (90 - angle2) % 360
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
65 dist= moving.Point.distanceNorm2(p1, p2)
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
66 return [dist,bearing1,bearing2,bearing2-bearing1]
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
67
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
68 #Quantitative analysis "CSJ" functions
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
69 def computeVelocities(obj,smoothing=True,halfWidth=3): #compute velocities from positions
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
70 velocities={}
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
71 for i in list(obj.timeInterval)[:-1]:
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
72 p1= obj.getPositionAtInstant(i)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
73 p2= obj.getPositionAtInstant(i+1)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
74 velocities[i]=p2-p1
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
75 velocities[obj.getLastInstant()]= velocities[obj.getLastInstant()-1] # duplicate last point
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
76 if smoothing:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
77 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
78 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
79 v1= list(utils.filterMovingWindow(velX, halfWidth))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
80 v2= list(utils.filterMovingWindow(velY, halfWidth))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
81 smoothedVelocity={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
82 for t,i in enumerate(sorted(velocities.keys())):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
83 smoothedVelocity[i]=moving.Point(v1[t], v2[t])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
84 velocities=smoothedVelocity
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
85 return velocities
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
86
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
87 def computeAcceleration(obj,fromPosition=True):
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
88 acceleration={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
89 if fromPosition:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
90 velocities=computeVelocities(obj,False,1)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
91 for i in sorted(velocities.keys()):
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
92 if i != sorted(velocities.keys())[-1]:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
93 acceleration[i]= velocities[i+1]-velocities[i]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
94 else:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
95 for i in list(obj.timeInterval)[:-1]:
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
96 v1= obj.getVelocityAtInstant(i)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
97 v2= obj.getVelocityAtInstant(i+1)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
98 acceleration[i]= v2-v1
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
99 return acceleration
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
100
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
101 def computeJerk(obj,fromPosition=True):
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
102 jerk={}
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
103 acceleration=computeAcceleration(obj,fromPosition=fromPosition)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
104 for i in sorted(acceleration.keys()):
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
105 if i != sorted(acceleration.keys())[-1]:
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
106 jerk[i] = (acceleration[i+1]-acceleration[i]).norm2()
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
107 return jerk
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
108
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
109 def sumSquaredJerk(obj,fromPosition=True):
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
110 jerk= computeJerk(obj,fromPosition=fromPosition)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
111 t=0
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
112 for i in sorted(jerk.keys()):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
113 t+= jerk[i]* jerk[i]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
114 return t
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
115
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
116 def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
117 results=[]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
118 bearing={}
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
119 if create:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
120 feature = buildFeature(obj,features,featureID,num=1)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
121 else:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
122 feature = features[featureID]
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
123 for t in feature.getTimeInterval():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
124 p1= feature.getPositionAtInstant(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
125 p2= obj.getPositionAtInstant(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
126 if t!=feature.getLastInstant():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
127 p3= feature.getPositionAtInstant(t+1)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
128 else:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
129 p1= feature.getPositionAtInstant(t-1)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
130 p3= feature.getPositionAtInstant(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
131 bearing[t]= getBearing(p1,p2,p3)[1]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
132 results.append(getBearing(p1,p2,p3))
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
133
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
134 medianResults=np.median(results,0)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
135 dist= medianResults[0]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
136 angle= medianResults[3]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
137
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
138 for i in sorted(bearing.keys()):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
139 bearing[i]= bearing[i]+angle
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
140
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
141 if smoothing:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
142 bearingInput=[]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
143 for i in sorted(bearing.keys()):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
144 bearingInput.append(bearing[i])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
145 import utils
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
146 bearingOut=utils.filterMovingWindow(bearingInput, halfWidth)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
147 for t,i in enumerate(sorted(bearing.keys())):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
148 bearing[i]=bearingOut[t]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
149
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
150 #solve a smoothing problem in case of big drop in computing bearing (0,360)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
151 for t,i in enumerate(sorted(bearing.keys())):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
152 if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
153 for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
154 bearing[x]=bearingInput[t-i+x]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
155
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
156 translated = moving.Trajectory()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
157 for t in feature.getTimeInterval():
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
158 p1= feature.getPositionAtInstant(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
159 p1.x = p1.x + dist*sin(bearing[t]*pi/180)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
160 p1.y = p1.y + dist*cos(bearing[t]*pi/180)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
161 translated.addPosition(p1)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
162
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
163 #modify first and last un-smoothed positions (half width)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
164 if smoothing:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
165 d1= translated[halfWidth]- feature.positions[halfWidth]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
166 d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
167 for i in xrange(halfWidth):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
168 p1= feature.getPositionAt(i)+d1
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
169 p2= feature.getPositionAt(-i-1)+d2
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
170 translated.setPosition(i,p1)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
171 translated.setPosition(-i-1,p2)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
172
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
173 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
174 return newObj
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
175
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
176 def smoothObject(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3, _computeVelocities=True,optimize=True,create=False):
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
177 '''Computes a smoother trajectory for the object
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
178 and optionnally smoother velocities
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
179
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
180 TODO: check whether features are necessary'''
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
181 featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
182 if featureList==[]:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
183 featureList.append(utils.argmaxDict({f.getNum():f.length() for f in obj.features}))
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
184 create = True
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
185 newObjects = []
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
186 for featureID in featureList:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
187 newObjects.append(smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create))
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
188
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
189 newTranslated = moving.Trajectory()
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
190 newInterval = []
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
191 for t in obj.getTimeInterval():
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
192 xCoord=[]
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
193 yCoord=[]
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
194 for i in newObjects:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
195 if i.existsAtInstant(t):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
196 p1= i.getPositionAtInstant(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
197 xCoord.append(p1.x)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
198 yCoord.append(p1.y)
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
199 if xCoord != []:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
200 tmp= moving.Point(np.median(xCoord),np.median(yCoord))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
201 newInterval.append(t)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
202 newTranslated.addPosition(tmp)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
203
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
204 newObj= moving.MovingObject(newNum, timeInterval = moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
205
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
206 if _computeVelocities:
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
207 tmpTraj = moving.Trajectory()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
208 velocities= computeVelocities(newObj,True,5)
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
209 for i in sorted(velocities.keys()):
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
210 tmpTraj.addPosition(velocities[i])
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
211 newObj.velocities=tmpTraj
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
212 else:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
213 newObj.velocities=obj.velocities
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
214
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
215 if optimize:
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
216 csj1= sumSquaredJerk(obj,fromPosition=True)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
217 csj2= sumSquaredJerk(newObj,fromPosition=True)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
218 if csj1<csj2:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
219 newObj=obj
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
220 newObj.velocities=obj.velocities
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
221 if _computeVelocities and csj1>=csj2:
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
222 csj3= sumSquaredJerk(obj,fromPosition=False)
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
223 csj4= sumSquaredJerk(newObj,fromPosition=False)
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
224 if csj4<=csj3:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
225 newObj.velocities= obj.velocities
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
226
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
227 newObj.featureNumbers=obj.featureNumbers
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
228 newObj.features=obj.features
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
229 newObj.userType=obj.userType
641
9fe254f11743 cleaning under way and renaming
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 619
diff changeset
230
619
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
231 if plotResults:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
232 plt.figure()
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
233 plt.title('objects_id = {}'.format(obj.num))
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
234 for i in featureList:
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
235 features[i].plot('cx-')
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
236 obj.plot('rx-')
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
237 newObj.plot('gx-')
dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 612
diff changeset
238 return newObj