Mercurial Hosting > traffic-intelligence

annotate python/objectsmoothing.py @ 605:3550da215e7a

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