Mercurial > hg > nsaunier > traffic-intelligence

comparison python/objectsmoothing.py @ 619:dc2d0a0d7fe1

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 10 Dec 2014 15:27:08 -0500
parents 6ee8765bb8db
children 9fe254f11743
comparison
equal deleted inserted replaced
596:04a8304e13f0 619:dc2d0a0d7fe1
3 import numpy as np 3 import numpy as np
4 4
5 import matplotlib.pyplot as plt 5 import matplotlib.pyplot as plt
6 6
7 def findNearest(feat, featureSet,t,reverse=True): 7 def findNearest(feat, featureSet,t,reverse=True):
8 dist={} 8 dist={}
9 for f in featureSet: 9 for f in featureSet:
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
16 def getFeatures(obj): 16 def getFeatures(obj,features,featureID):
17 longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)}) 17 #longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
18 t1,t3 = longestFeature.getFirstInstant(), longestFeature.getLastInstant() 18 t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant()
19 listFeatures=[[longestFeature,t1,t3,moving.Point(0,0)]] 19 listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]]
20 # find the features to fill in the beginning of the object existence 20 # find the features to fill in the beginning of the object existence
21 currentFeature = longestFeature 21 currentFeature = features[featureID]
22 while t1!=obj.getFirstInstant(): 22 while t1!=obj.getFirstInstant():
23 delta=listFeatures[-1][3] 23 delta=listFeatures[-1][3]
24 featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)] 24 featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)]
25 feat = findNearest(currentFeature,featureSet,t1-1,reverse=True) 25 feat = findNearest(currentFeature,featureSet,t1-1,reverse=True)
26 if feat.existsAtInstant(t1): 26 if feat.existsAtInstant(t1):
27 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta]) 27 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta])
28 else: 28 else:
29 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta]) 29 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta])
30 currentFeature = feat 30 currentFeature = feat
31 t1= feat.getFirstInstant() 31 t1= feat.getFirstInstant()
32 # find the features to fill in the end of the object existence 32 # find the features to fill in the end of the object existence
33 delta=moving.Point(0,0) 33 delta=moving.Point(0,0)
34 currentFeature = longestFeature 34 currentFeature = features[featureID]
35 while t3!= obj.getLastInstant(): 35 while t3!= obj.getLastInstant():
36 featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)] 36 featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)]
37 feat = findNearest(currentFeature,featureSet,t3+1,reverse=False) 37 feat = findNearest(currentFeature,featureSet,t3+1,reverse=False)
38 if feat.existsAtInstant(t3): 38 if feat.existsAtInstant(t3):
39 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta]) 39 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta])
40 else: 40 else:
41 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta]) 41 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta])
42 currentFeature = feat 42 currentFeature = feat
43 t3= feat.getLastInstant() 43 t3= feat.getLastInstant()
44 delta=listFeatures[-1][3] 44 delta=listFeatures[-1][3]
45 return listFeatures 45 return listFeatures
46 46
47 def buildFeature(obj,num=1): 47 def buildFeature(obj,features,featureID,num=1):
48 listFeatures= getFeatures(obj) 48 listFeatures= getFeatures(obj,features,featureID)
49 tmp={} 49 tmp={}
50 delta={} 50 delta={}
51 for i in listFeatures: 51 for i in listFeatures:
52 for t in xrange(i[1],i[2]+1): 52 for t in xrange(i[1],i[2]+1):
53 tmp[t]=[i[0],i[3]] 53 tmp[t]=[i[0],i[3]]
54 newTraj = moving.Trajectory() 54 newTraj = moving.Trajectory()
55 55
56 for instant in obj.getTimeInterval(): 56 for instant in obj.getTimeInterval():
57 newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1]) 57 newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1])
58 newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj) 58 newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj)
59 return newFeature 59 return newFeature
60 60
61 def getBearing(p1,p2,p3): 61 def getBearing(p1,p2,p3):
62 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x)) 62 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
63 bearing1 = (90 - angle) % 360 63 bearing1 = (90 - angle) % 360
64 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x)) 64 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
65 bearing2 = (90 - angle2) % 360 65 bearing2 = (90 - angle2) % 360
66 dist= moving.Point.distanceNorm2(p1, p2) 66 dist= moving.Point.distanceNorm2(p1, p2)
67 return [dist,bearing1,bearing2,bearing2-bearing1] 67 return [dist,bearing1,bearing2,bearing2-bearing1]
68 68
69 def smoothObjectTrajectory(obj,newNum,smoothing=False,plotResults=True,halfWidth=3): 69 #Quantitative analysis "CSJ" functions
70 results=[] 70 def computeVelocities (object,smoothing=True,halfWidth=3): #compute velocities from positions
71 bearing={} 71 velocities={}
72 feature= buildFeature(obj,1) 72 for i in list(object.timeInterval)[:-1]:
73 for t in feature.getTimeInterval(): 73 p1= object.getPositionAtInstant(i)
74 p1= feature.getPositionAtInstant(t) 74 p2= object.getPositionAtInstant(i+1)
75 p2= obj.getPositionAtInstant(t) 75 velocities[i]=p2-p1
76 if t!=feature.getLastInstant(): 76 velocities[object.getLastInstant()]= velocities[object.getLastInstant()-1] # duplicate last point
77 p3= feature.getPositionAtInstant(t+1) 77 if smoothing:
78 else: 78 velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
79 p1= feature.getPositionAtInstant(t-1) 79 velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
80 p3= feature.getPositionAtInstant(t) 80 v1= list(utils.filterMovingWindow(velX, halfWidth))
81 bearing[t]= getBearing(p1,p2,p3)[1] 81 v2= list(utils.filterMovingWindow(velY, halfWidth))
82 results.append(getBearing(p1,p2,p3)) 82 smoothedVelocity={}
83 83 for t,i in enumerate(sorted(velocities.keys())):
84 medianResults=np.median(results,0) 84 smoothedVelocity[i]=moving.Point(v1[t], v2[t])
85 dist= medianResults[0] 85 velocities=smoothedVelocity
86 angle= medianResults[3] 86 return velocities
87 87
88 for i in sorted(bearing.keys()): 88 def computeAcceleration (object,fromPosition=True):
89 bearing[i]= bearing[i]+angle 89 acceleration={}
90 90 if fromPosition:
91 if smoothing: 91 velocities=computeVelocities(object,False,1)
92 bearingInput=[] 92 for i in sorted (velocities.keys()):
93 for i in sorted(bearing.keys()): 93 if i != sorted (velocities.keys())[-1]:
94 bearingInput.append(bearing[i]) 94 acceleration[i]= velocities[i+1]-velocities[i]
95 bearingOut=utils.filterMovingWindow(bearingInput, halfWidth) 95 else:
96 for t,i in enumerate(sorted(bearing.keys())): 96 for i in list(object.timeInterval)[:-1]:
97 bearing[i]=bearingOut[t] 97 v1= object.getVelocityAtInstant(i)
98 98 v2= object.getVelocityAtInstant(i+1)
99 #solve a smoothing problem in case of big drop in computing bearing (0,360) 99 acceleration[i]= v2-v1
100 for t,i in enumerate(sorted(bearing.keys())): 100 return acceleration
101 if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340: 101
102 for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1): 102 def computeJerk (object,fromPosition=True):
103 bearing[x]=bearingInput[t-i+x] 103 jerk={}
104 104 acceleration=computeAcceleration (object,fromPosition=fromPosition)
105 translated = moving.Trajectory() 105 for i in sorted (acceleration.keys()):
106 for t in feature.getTimeInterval(): 106 if i != sorted (acceleration.keys())[-1]:
107 p1= feature.getPositionAtInstant(t) 107 jerk[i]= (acceleration[i+1]-acceleration[i]).norm2()
108 p1.x = p1.x + dist*sin(bearing[t]*pi/180) 108 return jerk
109 p1.y = p1.y + dist*cos(bearing[t]*pi/180) 109
110 translated.addPosition(p1) 110 def sumSquaredJerk (object,fromPosition=True):
111 111 jerk= computeJerk (object,fromPosition=fromPosition)
112 #modify first and last un-smoothed positions (half width) 112 t=0
113 if smoothing: 113 for i in sorted(jerk.keys()):
114 d1= translated[halfWidth]- feature.positions[halfWidth] 114 t+= jerk[i]* jerk[i]
115 d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1] 115 return t
116 for i in xrange(halfWidth): 116
117 p1 = feature.getPositionAt(i)+d1 117 def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False):
118 p2 = feature.getPositionAt(-i-1)+d2 118 results=[]
119 translated.setPosition(i,p1) 119 bearing={}
120 translated.setPosition(-i-1,p2) 120 if create:
121 121 feature= buildFeature(obj,features,featureID,num=1)
122 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated,velocities=obj.getVelocities()) 122 else:
123 newObj.features=obj.features 123 feature=features[featureID]
124 newObj.userType=obj.userType 124 for t in feature.getTimeInterval():
125 if plotResults: 125 p1= feature.getPositionAtInstant(t)
126 plt.figure() 126 p2= obj.getPositionAtInstant(t)
127 plt.title('objects_id = {}'.format(obj.num)) 127 if t!=feature.getLastInstant():
128 newObj.plot('gx-') 128 p3= feature.getPositionAtInstant(t+1)
129 feature.plot('cx-') 129 else:
130 obj.plot('rx-') 130 p1= feature.getPositionAtInstant(t-1)
131 return newObj 131 p3= feature.getPositionAtInstant(t)
132 bearing[t]= getBearing(p1,p2,p3)[1]
133 results.append(getBearing(p1,p2,p3))
134
135
136 medianResults=np.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 xrange(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 xrange(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 smoothObjectTrajectory(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3,computeVelocities=True,optimize=True,create=False):
179 featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()]
180 if featureList==[]:
181 featureList.append(longestFeature(obj))
182 create=True
183 objs=[]
184 for featureID in featureList:
185 objTMP=smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create)
186 objs.append(objTMP)
187 newTranslated = moving.Trajectory()
188 newInterval=[]
189 for t in obj.timeInterval:
190 xCoord=[]
191 yCoord=[]
192 for i in objs:
193 if i.existsAtInstant(t):
194 p1= i.getPositionAtInstant(t)
195 xCoord.append(p1.x)
196 yCoord.append(p1.y)
197 if xCoord!=[]:
198 tmp= moving.Point(np.median(xCoord),np.median(yCoord))
199 newInterval.append(t)
200 newTranslated.addPosition(tmp)
201
202 newObj= moving.MovingObject(newNum,timeInterval=moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
203
204 if computeVelocities:
205 tmpTraj = moving.Trajectory()
206 velocities= computeVelocities(newObj,True,5)
207 for i in sorted(velocities.keys()):
208 tmpTraj.addPosition(velocities[i])
209 newObj.velocities=tmpTraj
210 else:
211 newObj.velocities=obj.velocities
212
213 if optimize:
214 csj1= sumSquaredJerk (obj,fromPosition=True)
215 csj2= sumSquaredJerk (newObj,fromPosition=True)
216 if csj1<csj2:
217 newObj=obj
218 newObj.velocities=obj.velocities
219 if computeVelocities and csj1>=csj2:
220 csj3= sumSquaredJerk (obj,fromPosition=False)
221 csj4= sumSquaredJerk (newObj,fromPosition=False)
222 if csj4<=csj3:
223 newObj.velocities= obj.velocities
224 newObj.featureNumbers=obj.featureNumbers
225 newObj.features=obj.features
226 newObj.userType=obj.userType
227 if plotResults:
228 plt.figure()
229 plt.title('objects_id = {}'.format(obj.num))
230 for i in featureList:
231 features[i].plot('cx-')
232 obj.plot('rx-')
233 newObj.plot('gx-')
234 return newObj