Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/objectsmoothing.py @ 1028:cc5cb04b04b0

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
major update using the trafficintelligence package name and install through pip
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 15 Jun 2018 11:19:10 -0400
parents python/objectsmoothing.py@933670761a57
children
comparison
equal deleted inserted replaced
1027:6129296848d3 1028:cc5cb04b04b0
1 from trafficintelligence import storage, moving, utils
2
3 from math import atan2, degrees, sin, cos, pi
4 from numpy import median
5
6 import matplotlib.pyplot as plt
7
8 def findNearest(feat, featureSet,t,reverse=True):
9 dist={}
10 for f in featureSet:
11 if reverse:
12 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t))
13 else:
14 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t))
15 return min(dist, key=dist.get) # = utils.argmaxDict(dist)
16
17 def getFeatures(obj, featureID):
18 currentFeature = obj.getFeature(featureID)
19 first = currentFeature.getFirstInstant()
20 last = currentFeature.getLastInstant()
21 featureList=[[currentFeature,first,last,moving.Point(0,0)]]
22 # find the features to fill in the beginning of the object existence
23 while first != obj.getFirstInstant():
24 delta=featureList[-1][3]
25 featureSet = [f for f in obj.getFeatures() if f.existsAtInstant(first-1)]
26 feat = findNearest(currentFeature,featureSet,first-1,reverse=True)
27 if feat.existsAtInstant(first):
28 featureList.append([feat,feat.getFirstInstant(),first-1,(currentFeature.getPositionAtInstant(first)-feat.getPositionAtInstant(first))+delta])
29 else:
30 featureList.append([feat,feat.getFirstInstant(),first-1,(currentFeature.getPositionAtInstant(first)-feat.getPositionAtInstant(first-1))+delta])
31 currentFeature = feat
32 first= feat.getFirstInstant()
33 # find the features to fill in the end of the object existence
34 delta=moving.Point(0,0)
35 currentFeature = obj.getFeature(featureID) # need to reinitialize
36 while last!= obj.getLastInstant():
37 featureSet = [f for f in obj.getFeatures() if f.existsAtInstant(last+1)]
38 feat = findNearest(currentFeature,featureSet,last+1,reverse=False)
39 if feat.existsAtInstant(last):
40 featureList.append([feat,last+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(last)-feat.getPositionAtInstant(last))+delta])
41 else:
42 featureList.append([feat,last+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(last)-feat.getPositionAtInstant(last+1))+delta])
43 currentFeature = feat
44 last= feat.getLastInstant()
45 delta=featureList[-1][3]
46 return featureList
47
48 def buildFeature(obj, featureID, num = 1):
49 featureList= getFeatures(obj, featureID)
50 tmp={}
51 delta={}
52 for i in featureList:
53 for t in range(i[1],i[2]+1):
54 tmp[t]=[i[0],i[3]]
55 newTraj = moving.Trajectory()
56
57 for instant in obj.getTimeInterval():
58 newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1])
59 newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj)
60 return newFeature
61
62 def getBearing(p1,p2,p3):
63 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
64 bearing1 = (90 - angle) % 360
65 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
66 bearing2 = (90 - angle2) % 360
67 dist= moving.Point.distanceNorm2(p1, p2)
68 return [dist,bearing1,bearing2,bearing2-bearing1]
69
70 #Quantitative analysis "CSJ" functions
71 def computeVelocities(obj, smoothing=True, halfWidth=3): #compute velocities from positions
72 velocities={}
73 for i in list(obj.timeInterval)[:-1]:
74 p1= obj.getPositionAtInstant(i)
75 p2= obj.getPositionAtInstant(i+1)
76 velocities[i]=p2-p1
77 velocities[obj.getLastInstant()]= velocities[obj.getLastInstant()-1] # duplicate last point
78 if smoothing:
79 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
80 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
81 v1= list(utils.filterMovingWindow(velX, halfWidth))
82 v2= list(utils.filterMovingWindow(velY, halfWidth))
83 smoothedVelocity={}
84 for t,i in enumerate(sorted(velocities.keys())):
85 smoothedVelocity[i]=moving.Point(v1[t], v2[t])
86 velocities=smoothedVelocity
87 return velocities
88
89 def computeAcceleration(obj,fromPosition=True):
90 acceleration={}
91 if fromPosition:
92 velocities=computeVelocities(obj,False,1)
93 for i in sorted(velocities.keys()):
94 if i != sorted(velocities.keys())[-1]:
95 acceleration[i]= velocities[i+1]-velocities[i]
96 else:
97 for i in list(obj.timeInterval)[:-1]:
98 v1= obj.getVelocityAtInstant(i)
99 v2= obj.getVelocityAtInstant(i+1)
100 acceleration[i]= v2-v1
101 return acceleration
102
103 def computeJerk(obj,fromPosition=True):
104 jerk={}
105 acceleration=computeAcceleration(obj,fromPosition=fromPosition)
106 for i in sorted(acceleration.keys()):
107 if i != sorted(acceleration.keys())[-1]:
108 jerk[i] = (acceleration[i+1]-acceleration[i]).norm2()
109 return jerk
110
111 def sumSquaredJerk(obj,fromPosition=True):
112 jerk= computeJerk(obj,fromPosition=fromPosition)
113 t=0
114 for i in sorted(jerk.keys()):
115 t+= jerk[i]* jerk[i]
116 return t
117
118 def smoothObjectTrajectory(obj, featureID,newNum,smoothing=False,halfWidth=3,create=False):
119 results=[]
120 bearing={}
121 if create:
122 feature = buildFeature(obj, featureID , num=1) # why num=1
123 else:
124 feature = obj.getFeature(featureID)
125 for t in feature.getTimeInterval():
126 p1= feature.getPositionAtInstant(t)
127 p2= obj.getPositionAtInstant(t)
128 if t!=feature.getLastInstant():
129 p3= feature.getPositionAtInstant(t+1)
130 else:
131 p1= feature.getPositionAtInstant(t-1)
132 p3= feature.getPositionAtInstant(t)
133 bearing[t]= getBearing(p1,p2,p3)[1]
134 results.append(getBearing(p1,p2,p3))
135
136 medianResults=median(results,0)
137 dist= medianResults[0]
138 angle= medianResults[3]
139
140 for i in sorted(bearing.keys()):
141 bearing[i]= bearing[i]+angle
142
143 if smoothing:
144 bearingInput=[]
145 for i in sorted(bearing.keys()):
146 bearingInput.append(bearing[i])
147 import utils
148 bearingOut=utils.filterMovingWindow(bearingInput, halfWidth)
149 for t,i in enumerate(sorted(bearing.keys())):
150 bearing[i]=bearingOut[t]
151
152 #solve a smoothing problem in case of big drop in computing bearing (0,360)
153 for t,i in enumerate(sorted(bearing.keys())):
154 if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340:
155 for x in range(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1):
156 bearing[x]=bearingInput[t-i+x]
157
158 translated = moving.Trajectory()
159 for t in feature.getTimeInterval():
160 p1= feature.getPositionAtInstant(t)
161 p1.x = p1.x + dist*sin(bearing[t]*pi/180)
162 p1.y = p1.y + dist*cos(bearing[t]*pi/180)
163 translated.addPosition(p1)
164
165 #modify first and last un-smoothed positions (half width)
166 if smoothing:
167 d1= translated[halfWidth]- feature.positions[halfWidth]
168 d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1]
169 for i in range(halfWidth):
170 p1= feature.getPositionAt(i)+d1
171 p2= feature.getPositionAt(-i-1)+d2
172 translated.setPosition(i,p1)
173 translated.setPosition(-i-1,p2)
174
175 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated)
176 return newObj
177
178 def smoothObject(obj, newNum, minLengthParam = 0.7, smoothing = False, plotResults = True, halfWidth = 3, _computeVelocities = True, optimize = True, create = False):
179 '''Computes a smoother trajectory for the object
180 and optionnally smoother velocities
181
182 The object should have its features in obj.features
183 TODO: check whether features are necessary'''
184 if not obj.hasFeatures():
185 print('Object {} has an empty list of features: please load and add them using obj.setFeatures(features)'.format(obj.getNum()))
186 from sys import exit
187 exit()
188
189 featureList=[i for i,f in enumerate(obj.getFeatures()) if f.length() >= minLengthParam*obj.length()]
190 if featureList==[]:
191 featureList.append(utils.argmaxDict({i:f.length() for i,f in enumerate(obj.getFeatures())}))
192 create = True
193 newObjects = []
194 for featureID in featureList: # featureID should be the index in the list of obj.features
195 newObjects.append(smoothObjectTrajectory(obj, featureID, newNum, smoothing = smoothing, halfWidth = halfWidth, create = create))
196
197 newTranslated = moving.Trajectory()
198 newInterval = []
199 for t in obj.getTimeInterval():
200 xCoord=[]
201 yCoord=[]
202 for i in newObjects:
203 if i.existsAtInstant(t):
204 p1= i.getPositionAtInstant(t)
205 xCoord.append(p1.x)
206 yCoord.append(p1.y)
207 if xCoord != []:
208 tmp= moving.Point(median(xCoord), median(yCoord))
209 newInterval.append(t)
210 newTranslated.addPosition(tmp)
211
212 newObj= moving.MovingObject(newNum, timeInterval = moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
213
214 if _computeVelocities:
215 tmpTraj = moving.Trajectory()
216 velocities= computeVelocities(newObj,True,5)
217 for i in sorted(velocities.keys()):
218 tmpTraj.addPosition(velocities[i])
219 newObj.velocities=tmpTraj
220 else:
221 newObj.velocities=obj.velocities
222
223 if optimize:
224 csj1= sumSquaredJerk(obj,fromPosition=True)
225 csj2= sumSquaredJerk(newObj,fromPosition=True)
226 if csj1<csj2:
227 newObj=obj
228 newObj.velocities=obj.velocities
229 if _computeVelocities and csj1>=csj2:
230 csj3= sumSquaredJerk(obj,fromPosition=False)
231 csj4= sumSquaredJerk(newObj,fromPosition=False)
232 if csj4<=csj3:
233 newObj.velocities= obj.velocities
234
235 newObj.featureNumbers=obj.featureNumbers
236 newObj.features=obj.getFeatures()
237 newObj.userType=obj.userType
238
239 if plotResults:
240 plt.figure()
241 plt.title('objects_id = {}'.format(obj.num))
242 for i in featureList:
243 obj.getFeature(i).plot('cx-')
244 obj.plot('rx-')
245 newObj.plot('gx-')
246 return newObj