Mercurial > hg > nsaunier > traffic-intelligence

comparison python/objectsmoothing.py @ 561:ee45c6eb6d49

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added Mohamed Gomaa Mohamed function to smooth object trajectories
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Tue, 15 Jul 2014 16:42:04 -0400
parents
children 3550da215e7a
comparison
equal deleted inserted replaced
560:5b534ad95bfb 561:ee45c6eb6d49
1 import storage, moving, utils
2 from math import * #atan2,asin,degrees,sin,cos,pi
3 import numpy as np
4
5 import matplotlib.pyplot as plt
6
7 def findNearest(feat, featureSet,t,reverse=True):
8 dist={}
9 for f in featureSet:
10 if reverse:
11 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t))
12 else:
13 dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t))
14 return min(dist, key=dist.get) # = utils.argmaxDict(dist)
15
16 def getFeatures(obj):
17 longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
18 t1,t3 = longestFeature.getFirstInstant(), longestFeature.getLastInstant()
19 listFeatures=[[longestFeature,t1,t3,moving.Point(0,0)]]
20 # find the features to fill in the beginning of the object existence
21 currentFeature = longestFeature
22 while t1!=obj.getFirstInstant():
23 delta=listFeatures[-1][3]
24 featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)]
25 feat = findNearest(currentFeature,featureSet,t1-1,reverse=True)
26 if feat.existsAtInstant(t1):
27 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta])
28 else:
29 listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta])
30 currentFeature = feat
31 t1= feat.getFirstInstant()
32 # find the features to fill in the end of the object existence
33 delta=moving.Point(0,0)
34 currentFeature = longestFeature
35 while t3!= obj.getLastInstant():
36 featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)]
37 feat = findNearest(currentFeature,featureSet,t3+1,reverse=False)
38 if feat.existsAtInstant(t3):
39 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta])
40 else:
41 listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta])
42 currentFeature = feat
43 t3= feat.getLastInstant()
44 delta=listFeatures[-1][3]
45 return listFeatures
46
47 def buildFeature(obj,num=1):
48 listFeatures= getFeatures(obj)
49 tmp={}
50 delta={}
51 for i in listFeatures:
52 for t in xrange(i[1],i[2]+1):
53 tmp[t]=[i[0],i[3]]
54 newTraj = moving.Trajectory()
55
56 for instant in obj.getTimeInterval():
57 newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1])
58 newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj)
59 return newFeature
60
61 def getBearing(p1,p2,p3):
62 angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x))
63 bearing1 = (90 - angle) % 360
64 angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x))
65 bearing2 = (90 - angle2) % 360
66 dist= moving.Point.distanceNorm2(p1, p2)
67 return [dist,bearing1,bearing2,bearing2-bearing1]
68
69 def smoothObjectTrajectory(obj,newNum,smoothing=False,plotResults=True,halfWidth=3):
70 results=[]
71 bearing={}
72 feature= buildFeature(obj,1)
73 for t in feature.getTimeInterval():
74 p1= feature.getPositionAtInstant(t)
75 p2= obj.getPositionAtInstant(t)
76 if t!=feature.getLastInstant():
77 p3= feature.getPositionAtInstant(t+1)
78 else:
79 p1= feature.getPositionAtInstant(t-1)
80 p3= feature.getPositionAtInstant(t)
81 bearing[t]= getBearing(p1,p2,p3)[1]
82 results.append(getBearing(p1,p2,p3))
83
84 medianResults=np.median(results,0)
85 dist= medianResults[0]
86 angle= medianResults[3]
87
88 for i in sorted(bearing.keys()):
89 bearing[i]= bearing[i]+angle
90
91 if smoothing:
92 bearingInput=[]
93 for i in sorted(bearing.keys()):
94 bearingInput.append(bearing[i])
95 bearingOut=utils.filterMovingWindow(bearingInput, halfWidth)
96 for t,i in enumerate(sorted(bearing.keys())):
97 bearing[i]=bearingOut[t]
98
99 #solve a smoothing problem in case of big drop in computing bearing (0,360)
100 for t,i in enumerate(sorted(bearing.keys())):
101 if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340:
102 for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1):
103 bearing[x]=bearingInput[t-i+x]
104
105 translated = moving.Trajectory()
106 for t in feature.getTimeInterval():
107 p1= feature.getPositionAtInstant(t)
108 p1.x = p1.x + dist*sin(bearing[t]*pi/180)
109 p1.y = p1.y + dist*cos(bearing[t]*pi/180)
110 translated.addPosition(p1)
111
112 #modify first and last un-smoothed positions (half width)
113 if smoothing:
114 d1= translated[halfWidth]- feature.positions[halfWidth]
115 d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1]
116 for i in xrange(halfWidth):
117 p1 = feature.getPositionAt(i)+d1
118 p2 = feature.getPositionAt(-i-1)+d2
119 translated.setPosition(i,p1)
120 translated.setPosition(-i-1,p2)
121
122 newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated,velocities=obj.getVelocities())
123 newObj.features=obj.features
124 newObj.userType=obj.userType
125 if plotResults:
126 plt.figure()
127 plt.title('objects_id = {}'.format(obj.num))
128 newObj.plot('gx-')
129 feature.plot('cx-')
130 obj.plot('rx-')
131 return newObj