Mercurial Hosting > traffic-intelligence
comparison python/objectsmoothing.py @ 614:5e09583275a4
Merged Nicolas/trafficintelligence into default
author | Mohamed Gomaa <eng.m.gom3a@gmail.com> |
---|---|
date | Fri, 05 Dec 2014 12:13:53 -0500 |
parents | ee45c6eb6d49 |
children | 3550da215e7a |
comparison
equal
deleted
inserted
replaced
598:11f96bd08552 | 614:5e09583275a4 |
---|---|
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 |