Mercurial > hg > nsaunier > traffic-intelligence

comparison python/objectsmoothing.py @ 605:3550da215e7a

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