Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/objectsmoothing.py @ 1028:cc5cb04b04b0
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 |