Mercurial > hg > nsaunier > traffic-intelligence

comparison python/objectsmoothing.py @ 641:9fe254f11743

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cleaning under way and renaming
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 09 Apr 2015 16:55:24 +0200
parents dc2d0a0d7fe1
children e54751e71d61
comparison
equal deleted inserted replaced
640:fe34c0f79c32 641:9fe254f11743
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
16 def getFeatures(obj,features,featureID): 16 def getFeatures(obj,features,featureID):
17 #longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
18 t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant() 17 t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant()
19 listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]] 18 listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]]
20 # find the features to fill in the beginning of the object existence 19 # find the features to fill in the beginning of the object existence
21 currentFeature = features[featureID] 20 currentFeature = features[featureID]
22 while t1!=obj.getFirstInstant(): 21 while t1!=obj.getFirstInstant():
62 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x)) 61 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
63 bearing1 = (90 - angle) % 360 62 bearing1 = (90 - angle) % 360
64 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x)) 63 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
65 bearing2 = (90 - angle2) % 360 64 bearing2 = (90 - angle2) % 360
66 dist= moving.Point.distanceNorm2(p1, p2) 65 dist= moving.Point.distanceNorm2(p1, p2)
67 return [dist,bearing1,bearing2,bearing2-bearing1] 66 return [dist,bearing1,bearing2,bearing2-bearing1]
68 67
69 #Quantitative analysis "CSJ" functions 68 #Quantitative analysis "CSJ" functions
70 def computeVelocities (object,smoothing=True,halfWidth=3): #compute velocities from positions 69 def computeVelocities(obj,smoothing=True,halfWidth=3): #compute velocities from positions
71 velocities={} 70 velocities={}
72 for i in list(object.timeInterval)[:-1]: 71 for i in list(obj.timeInterval)[:-1]:
73 p1= object.getPositionAtInstant(i) 72 p1= obj.getPositionAtInstant(i)
74 p2= object.getPositionAtInstant(i+1) 73 p2= obj.getPositionAtInstant(i+1)
75 velocities[i]=p2-p1 74 velocities[i]=p2-p1
76 velocities[object.getLastInstant()]= velocities[object.getLastInstant()-1] # duplicate last point 75 velocities[obj.getLastInstant()]= velocities[obj.getLastInstant()-1] # duplicate last point
77 if smoothing: 76 if smoothing:
78 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())] 77 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
79 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())] 78 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
80 v1= list(utils.filterMovingWindow(velX, halfWidth)) 79 v1= list(utils.filterMovingWindow(velX, halfWidth))
81 v2= list(utils.filterMovingWindow(velY, halfWidth)) 80 v2= list(utils.filterMovingWindow(velY, halfWidth))
83 for t,i in enumerate(sorted(velocities.keys())): 82 for t,i in enumerate(sorted(velocities.keys())):
84 smoothedVelocity[i]=moving.Point(v1[t], v2[t]) 83 smoothedVelocity[i]=moving.Point(v1[t], v2[t])
85 velocities=smoothedVelocity 84 velocities=smoothedVelocity
86 return velocities 85 return velocities
87 86
88 def computeAcceleration (object,fromPosition=True): 87 def computeAcceleration(obj,fromPosition=True):
89 acceleration={} 88 acceleration={}
90 if fromPosition: 89 if fromPosition:
91 velocities=computeVelocities(object,False,1) 90 velocities=computeVelocities(obj,False,1)
92 for i in sorted (velocities.keys()): 91 for i in sorted(velocities.keys()):
93 if i != sorted (velocities.keys())[-1]: 92 if i != sorted(velocities.keys())[-1]:
94 acceleration[i]= velocities[i+1]-velocities[i] 93 acceleration[i]= velocities[i+1]-velocities[i]
95 else: 94 else:
96 for i in list(object.timeInterval)[:-1]: 95 for i in list(obj.timeInterval)[:-1]:
97 v1= object.getVelocityAtInstant(i) 96 v1= obj.getVelocityAtInstant(i)
98 v2= object.getVelocityAtInstant(i+1) 97 v2= obj.getVelocityAtInstant(i+1)
99 acceleration[i]= v2-v1 98 acceleration[i]= v2-v1
100 return acceleration 99 return acceleration
101 100
102 def computeJerk (object,fromPosition=True): 101 def computeJerk(obj,fromPosition=True):
103 jerk={} 102 jerk={}
104 acceleration=computeAcceleration (object,fromPosition=fromPosition) 103 acceleration=computeAcceleration(obj,fromPosition=fromPosition)
105 for i in sorted (acceleration.keys()): 104 for i in sorted(acceleration.keys()):
106 if i != sorted (acceleration.keys())[-1]: 105 if i != sorted(acceleration.keys())[-1]:
107 jerk[i]= (acceleration[i+1]-acceleration[i]).norm2() 106 jerk[i] = (acceleration[i+1]-acceleration[i]).norm2()
108 return jerk 107 return jerk
109 108
110 def sumSquaredJerk (object,fromPosition=True): 109 def sumSquaredJerk(obj,fromPosition=True):
111 jerk= computeJerk (object,fromPosition=fromPosition) 110 jerk= computeJerk(obj,fromPosition=fromPosition)
112 t=0 111 t=0
113 for i in sorted(jerk.keys()): 112 for i in sorted(jerk.keys()):
114 t+= jerk[i]* jerk[i] 113 t+= jerk[i]* jerk[i]
115 return t 114 return t
116 115
117 def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False): 116 def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False):
118 results=[] 117 results=[]
119 bearing={} 118 bearing={}
120 if create: 119 if create:
121 feature= buildFeature(obj,features,featureID,num=1) 120 feature = buildFeature(obj,features,featureID,num=1)
122 else: 121 else:
123 feature=features[featureID] 122 feature = features[featureID]
124 for t in feature.getTimeInterval(): 123 for t in feature.getTimeInterval():
125 p1= feature.getPositionAtInstant(t) 124 p1= feature.getPositionAtInstant(t)
126 p2= obj.getPositionAtInstant(t) 125 p2= obj.getPositionAtInstant(t)
127 if t!=feature.getLastInstant(): 126 if t!=feature.getLastInstant():
128 p3= feature.getPositionAtInstant(t+1) 127 p3= feature.getPositionAtInstant(t+1)
129 else: 128 else:
130 p1= feature.getPositionAtInstant(t-1) 129 p1= feature.getPositionAtInstant(t-1)
131 p3= feature.getPositionAtInstant(t) 130 p3= feature.getPositionAtInstant(t)
132 bearing[t]= getBearing(p1,p2,p3)[1] 131 bearing[t]= getBearing(p1,p2,p3)[1]
133 results.append(getBearing(p1,p2,p3)) 132 results.append(getBearing(p1,p2,p3))
134 133
135
136 medianResults=np.median(results,0) 134 medianResults=np.median(results,0)
137 dist= medianResults[0] 135 dist= medianResults[0]
138 angle= medianResults[3] 136 angle= medianResults[3]
139 137
140 for i in sorted(bearing.keys()): 138 for i in sorted(bearing.keys()):
173 translated.setPosition(-i-1,p2) 171 translated.setPosition(-i-1,p2)
174 172
175 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated) 173 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated)
176 return newObj 174 return newObj
177 175
178 def smoothObjectTrajectory(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3,computeVelocities=True,optimize=True,create=False): 176 def smoothObject(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3, _computeVelocities=True,optimize=True,create=False):
177 '''Computes a smoother trajectory for the object
178 and optionnally smoother velocities
179
180 TODO: check whether features are necessary'''
179 featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()] 181 featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()]
180 if featureList==[]: 182 if featureList==[]:
181 featureList.append(longestFeature(obj)) 183 featureList.append(utils.argmaxDict({f.getNum():f.length() for f in obj.features}))
182 create=True 184 create = True
183 objs=[] 185 newObjects = []
184 for featureID in featureList: 186 for featureID in featureList:
185 objTMP=smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create) 187 newObjects.append(smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create))
186 objs.append(objTMP) 188
187 newTranslated = moving.Trajectory() 189 newTranslated = moving.Trajectory()
188 newInterval=[] 190 newInterval = []
189 for t in obj.timeInterval: 191 for t in obj.getTimeInterval():
190 xCoord=[] 192 xCoord=[]
191 yCoord=[] 193 yCoord=[]
192 for i in objs: 194 for i in newObjects:
193 if i.existsAtInstant(t): 195 if i.existsAtInstant(t):
194 p1= i.getPositionAtInstant(t) 196 p1= i.getPositionAtInstant(t)
195 xCoord.append(p1.x) 197 xCoord.append(p1.x)
196 yCoord.append(p1.y) 198 yCoord.append(p1.y)
197 if xCoord!=[]: 199 if xCoord != []:
198 tmp= moving.Point(np.median(xCoord),np.median(yCoord)) 200 tmp= moving.Point(np.median(xCoord),np.median(yCoord))
199 newInterval.append(t) 201 newInterval.append(t)
200 newTranslated.addPosition(tmp) 202 newTranslated.addPosition(tmp)
201 203
202 newObj= moving.MovingObject(newNum,timeInterval=moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated) 204 newObj= moving.MovingObject(newNum, timeInterval = moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
203 205
204 if computeVelocities: 206 if _computeVelocities:
205 tmpTraj = moving.Trajectory() 207 tmpTraj = moving.Trajectory()
206 velocities= computeVelocities(newObj,True,5) 208 velocities= computeVelocities(newObj,True,5)
207 for i in sorted(velocities.keys()): 209 for i in sorted(velocities.keys()):
208 tmpTraj.addPosition(velocities[i]) 210 tmpTraj.addPosition(velocities[i])
209 newObj.velocities=tmpTraj 211 newObj.velocities=tmpTraj
210 else: 212 else:
211 newObj.velocities=obj.velocities 213 newObj.velocities=obj.velocities
212 214
213 if optimize: 215 if optimize:
214 csj1= sumSquaredJerk (obj,fromPosition=True) 216 csj1= sumSquaredJerk(obj,fromPosition=True)
215 csj2= sumSquaredJerk (newObj,fromPosition=True) 217 csj2= sumSquaredJerk(newObj,fromPosition=True)
216 if csj1<csj2: 218 if csj1<csj2:
217 newObj=obj 219 newObj=obj
218 newObj.velocities=obj.velocities 220 newObj.velocities=obj.velocities
219 if computeVelocities and csj1>=csj2: 221 if _computeVelocities and csj1>=csj2:
220 csj3= sumSquaredJerk (obj,fromPosition=False) 222 csj3= sumSquaredJerk(obj,fromPosition=False)
221 csj4= sumSquaredJerk (newObj,fromPosition=False) 223 csj4= sumSquaredJerk(newObj,fromPosition=False)
222 if csj4<=csj3: 224 if csj4<=csj3:
223 newObj.velocities= obj.velocities 225 newObj.velocities= obj.velocities
226
224 newObj.featureNumbers=obj.featureNumbers 227 newObj.featureNumbers=obj.featureNumbers
225 newObj.features=obj.features 228 newObj.features=obj.features
226 newObj.userType=obj.userType 229 newObj.userType=obj.userType
230
227 if plotResults: 231 if plotResults:
228 plt.figure() 232 plt.figure()
229 plt.title('objects_id = {}'.format(obj.num)) 233 plt.title('objects_id = {}'.format(obj.num))
230 for i in featureList: 234 for i in featureList:
231 features[i].plot('cx-') 235 features[i].plot('cx-')