Mercurial Hosting > traffic-intelligence

annotate python/objectsmoothing.py @ 607:84690dfe5560

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
add some functions for behaviour analysis
author MohamedGomaa
date Tue, 25 Nov 2014 22:49:47 -0500
parents 75ad9c0d6cc3
children 6ee8765bb8db
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)
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
15
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
16 def FeatureList(obj,minLengthParam=0.7):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
17 featureList=[]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
18 for i in obj.features:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
19 if i.length>= minLengthParam*obj.length():
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
20 featureList.append(i.num)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
21 return featureList
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
22
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
23 def getFeatures(obj,features,featureID):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
24 #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
25 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
26 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
27 # 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
28 currentFeature = features[featureID]
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
29 while t1!=obj.getFirstInstant():
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
30 delta=listFeatures[-1][3]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
31 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
32 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
33 if feat.existsAtInstant(t1):
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
34 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
35 else:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
36 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
37 currentFeature = feat
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
38 t1= feat.getFirstInstant()
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
39 # 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
40 delta=moving.Point(0,0)
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
41 currentFeature = features[featureID]
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
42 while t3!= obj.getLastInstant():
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
43 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
44 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
45 if feat.existsAtInstant(t3):
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
46 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
47 else:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
48 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
49 currentFeature = feat
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
50 t3= feat.getLastInstant()
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
51 delta=listFeatures[-1][3]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
52 return listFeatures
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
53
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
54 def buildFeature(obj,features,featureID,num=1):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
55 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
56 tmp={}
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
57 delta={}
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
58 for i in listFeatures:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
59 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
60 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
61 newTraj = moving.Trajectory()
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
62
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
63 for instant in obj.getTimeInterval():
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
64 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
65 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
66 return newFeature
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
67
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
68 def getBearing(p1,p2,p3):
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
69 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
70 bearing1 = (90 - angle) % 360
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
71 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
72 bearing2 = (90 - angle2) % 360
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
73 dist= moving.Point.distanceNorm2(p1, p2)
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
74 return [dist,bearing1,bearing2,bearing2-bearing1]
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
75
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
76 #Quantitative analysis "CSJ" functions
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
77 def computeVelocities (object,smoothing=True,halfWidth=3): #compute velocities from positions
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
78 velocities={}
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
79 for i in list(object.timeInterval)[:-1]:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
80 p1= object.getPositionAtInstant(i)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
81 p2= object.getPositionAtInstant(i+1)
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
82 velocities[i]=p2-p1
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
83 velocities[object.timeInterval.last]= velocities[object.timeInterval.last-1] # duplicate last point
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
84 if smoothing:
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
85 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
86 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
87 v1= list(utils.filterMovingWindow(velX, halfWidth))
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
88 v2= list(utils.filterMovingWindow(velY, halfWidth))
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
89 smoothedVelocity={}
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
90 for t,i in enumerate(sorted(velocities.keys())):
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
91 smoothedVelocity[i]=moving.Point(v1[t], v2[t])
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
92 velocities=smoothedVelocity
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
93 return velocities
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
94
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
95 def computeAcceleration (object,fromPosition=True):
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
96 acceleration={}
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
97 if fromPosition:
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
98 velocities=computeVelocities(object,False,1)
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
99 for i in sorted (velocities.keys()):
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
100 if i != sorted (velocities.keys())[-1]:
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
101 acceleration[i]= velocities[i+1]-velocities[i]
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
102 else:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
103 for i in list(object.timeInterval)[:-1]:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
104 v1= object.getVelocityAtInstant(i)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
105 v2= object.getVelocityAtInstant(i+1)
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
106 acceleration[i]= v2-v1
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
107 return acceleration
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
108
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
109 def computeJerk (object,fromPosition=True):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
110 jerk={}
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
111 acceleration=computeAcceleration (object,fromPosition=fromPosition)
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
112 for i in sorted (acceleration.keys()):
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
113 if i != sorted (acceleration.keys())[-1]:
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
114 jerk[i]= (acceleration[i+1]-acceleration[i]).norm2()
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
115 return jerk
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
116
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
117 def sumSquaredJerk (object,fromPosition=True):
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
118 jerk= computeJerk (object,fromPosition=fromPosition)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
119 t=0
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
120 for i in sorted(jerk.keys()):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
121 t+= jerk[i]* jerk[i]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
122 return t
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
123
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
124 def getObject(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False):
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
125 results=[]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
126 bearing={}
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
127 if create:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
128 feature= buildFeature(obj,features,featureID,num=1)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
129 else:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
130 feature=features[featureID]
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
131 for t in feature.getTimeInterval():
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
132 p1= feature.getPositionAtInstant(t)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
133 p2= obj.getPositionAtInstant(t)
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
134 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
135 p3= feature.getPositionAtInstant(t+1)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
136 else:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
137 p1= feature.getPositionAtInstant(t-1)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
138 p3= feature.getPositionAtInstant(t)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
139 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
140 results.append(getBearing(p1,p2,p3))
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
141
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
142
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
143 medianResults=np.median(results,0)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
144 dist= medianResults[0]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
145 angle= medianResults[3]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
146
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
147 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
148 bearing[i]= bearing[i]+angle
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
149
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
150 if smoothing:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
151 bearingInput=[]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
152 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
153 bearingInput.append(bearing[i])
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
154 import utils
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
155 bearingOut=utils.filterMovingWindow(bearingInput, halfWidth)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
156 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
157 bearing[i]=bearingOut[t]
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
158
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
159 #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
160 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
161 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
162 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
163 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
164
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
165 translated = moving.Trajectory()
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
166 for t in feature.getTimeInterval():
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
167 p1= feature.getPositionAtInstant(t)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
168 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
169 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
170 translated.addPosition(p1)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
171
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
172 #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
173 if smoothing:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
174 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
175 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
176 for i in xrange(halfWidth):
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
177 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
178 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
179 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
180 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
181 translated.setPosition(i,p1)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
182 translated.setPosition(-i-1,p2)
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
183
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
184 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
185 return newObj
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
186
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
187 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
188 featureList=FeatureList(obj,minLengthParam=minLengthParam)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
189 if featureList==[]:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
190 featureList.append(longestFeature(obj))
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
191 create=True
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
192 objs=[]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
193 for featureID in featureList:
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
194 objTMP=getObject(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create)
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
195 objs.append(objTMP)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
196 newTranslated = moving.Trajectory()
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
197 newInterval=[]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
198 for t in obj.timeInterval:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
199 xCoord=[]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
200 yCoord=[]
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
201 for i in objs:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
202 if i.existsAtInstant(t):
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
203 p1= i.getPositionAtInstant(t)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
204 xCoord.append(p1.x)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
205 yCoord.append(p1.y)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
206 if xCoord!=[]:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
207 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
208 newInterval.append(t)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
209 newTranslated.addPosition(tmp)
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
210
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
211 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
212
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
213 if computeVelocities:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
214 tmpTraj = moving.Trajectory()
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
215 velocities= computeVelocities(newObj,True,5)
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
216 for i in sorted(velocities.keys()):
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
217 tmpTraj.addPosition(velocities[i])
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
218 newObj.velocities=tmpTraj
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
219 else:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
220 newObj.velocities=obj.velocities
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
221
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
222 if optimize:
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
223 csj1= sumSquaredJerk (obj,fromPosition=True)
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
224 csj2= sumSquaredJerk (newObj,fromPosition=True)
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
225 if csj1<csj2:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
226 newObj=obj
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
227 newObj.velocities=obj.velocities
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
228 if computeVelocities and csj1>=csj2:
606
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
229 csj3= sumSquaredJerk (obj,fromPosition=False)
75ad9c0d6cc3 update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 605
diff changeset
230 csj4= sumSquaredJerk (newObj,fromPosition=False)
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
231 if csj4<=csj3:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
232 newObj.velocities= obj.velocities
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
233 newObj.featureNumbers=obj.featureNumbers
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
234 newObj.features=obj.features
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
235 newObj.userType=obj.userType
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
236 if plotResults:
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
237 plt.figure()
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
238 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
239 for i in featureList:
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
240 features[i].plot('cx-')
561
ee45c6eb6d49 added Mohamed Gomaa Mohamed function to smooth object trajectories
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
diff changeset
241 obj.plot('rx-')
605
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
242 newObj.plot('gx-')
3550da215e7a update the method to use multi featutes instead on single feature
MohamedGomaa
parents: 561
diff changeset
243 return newObj