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