Mercurial Hosting > traffic-intelligence

annotate python/objectsmoothing.py @ 612:6ee8765bb8db

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
minor modifications
author MohamedGomaa
date Thu, 04 Dec 2014 18:52:07 -0500
parents 75ad9c0d6cc3
children dc2d0a0d7fe1
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 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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
170 p1= feature.getPositionAt(i)+d1
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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
6ee8765bb8db minor modifications
MohamedGomaa
parents: 606
diff changeset
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