Mercurial Hosting > traffic-intelligence
diff 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 |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/python/objectsmoothing.py Fri Dec 05 12:13:53 2014 -0500 @@ -0,0 +1,131 @@ +import storage, moving, utils +from math import * #atan2,asin,degrees,sin,cos,pi +import numpy as np + +import matplotlib.pyplot as plt + +def findNearest(feat, featureSet,t,reverse=True): + dist={} + for f in featureSet: + if reverse: + dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t+1),f.getPositionAtInstant(t)) + else: + dist[f]= moving.Point.distanceNorm2(feat.getPositionAtInstant(t-1),f.getPositionAtInstant(t)) + return min(dist, key=dist.get) # = utils.argmaxDict(dist) + +def getFeatures(obj): + longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)}) + t1,t3 = longestFeature.getFirstInstant(), longestFeature.getLastInstant() + listFeatures=[[longestFeature,t1,t3,moving.Point(0,0)]] + # find the features to fill in the beginning of the object existence + currentFeature = longestFeature + while t1!=obj.getFirstInstant(): + delta=listFeatures[-1][3] + featureSet = [f for f in obj.features if f.existsAtInstant(t1-1)] + feat = findNearest(currentFeature,featureSet,t1-1,reverse=True) + if feat.existsAtInstant(t1): + listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1))+delta]) + else: + listFeatures.append([feat,feat.getFirstInstant(),t1-1,(currentFeature.getPositionAtInstant(t1)-feat.getPositionAtInstant(t1-1))+delta]) + currentFeature = feat + t1= feat.getFirstInstant() + # find the features to fill in the end of the object existence + delta=moving.Point(0,0) + currentFeature = longestFeature + while t3!= obj.getLastInstant(): + featureSet = [f for f in obj.features if f.existsAtInstant(t3+1)] + feat = findNearest(currentFeature,featureSet,t3+1,reverse=False) + if feat.existsAtInstant(t3): + listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3))+delta]) + else: + listFeatures.append([feat,t3+1,feat.getLastInstant(),(currentFeature.getPositionAtInstant(t3)-feat.getPositionAtInstant(t3+1))+delta]) + currentFeature = feat + t3= feat.getLastInstant() + delta=listFeatures[-1][3] + return listFeatures + +def buildFeature(obj,num=1): + listFeatures= getFeatures(obj) + tmp={} + delta={} + for i in listFeatures: + for t in xrange(i[1],i[2]+1): + tmp[t]=[i[0],i[3]] + newTraj = moving.Trajectory() + + for instant in obj.getTimeInterval(): + newTraj.addPosition(tmp[instant][0].getPositionAtInstant(instant)+tmp[instant][1]) + newFeature= moving.MovingObject(num,timeInterval=obj.getTimeInterval(),positions=newTraj) + return newFeature + +def getBearing(p1,p2,p3): + angle = degrees(atan2(p3.y -p1.y, p3.x -p1.x)) + bearing1 = (90 - angle) % 360 + angle2 = degrees(atan2(p2.y -p1.y, p2.x -p1.x)) + bearing2 = (90 - angle2) % 360 + dist= moving.Point.distanceNorm2(p1, p2) + return [dist,bearing1,bearing2,bearing2-bearing1] + +def smoothObjectTrajectory(obj,newNum,smoothing=False,plotResults=True,halfWidth=3): + results=[] + bearing={} + feature= buildFeature(obj,1) + for t in feature.getTimeInterval(): + p1= feature.getPositionAtInstant(t) + p2= obj.getPositionAtInstant(t) + if t!=feature.getLastInstant(): + p3= feature.getPositionAtInstant(t+1) + else: + p1= feature.getPositionAtInstant(t-1) + p3= feature.getPositionAtInstant(t) + bearing[t]= getBearing(p1,p2,p3)[1] + results.append(getBearing(p1,p2,p3)) + + medianResults=np.median(results,0) + dist= medianResults[0] + angle= medianResults[3] + + for i in sorted(bearing.keys()): + bearing[i]= bearing[i]+angle + + if smoothing: + bearingInput=[] + for i in sorted(bearing.keys()): + bearingInput.append(bearing[i]) + bearingOut=utils.filterMovingWindow(bearingInput, halfWidth) + for t,i in enumerate(sorted(bearing.keys())): + bearing[i]=bearingOut[t] + + #solve a smoothing problem in case of big drop in computing bearing (0,360) + for t,i in enumerate(sorted(bearing.keys())): + if i!= max(bearing.keys()) and abs(bearingInput[t] - bearingInput[t+1])>=340: + for x in xrange(max(i-halfWidth,min(bearing.keys())),min(i+halfWidth,max(bearing.keys()))+1): + bearing[x]=bearingInput[t-i+x] + + translated = moving.Trajectory() + for t in feature.getTimeInterval(): + p1= feature.getPositionAtInstant(t) + p1.x = p1.x + dist*sin(bearing[t]*pi/180) + p1.y = p1.y + dist*cos(bearing[t]*pi/180) + translated.addPosition(p1) + + #modify first and last un-smoothed positions (half width) + if smoothing: + d1= translated[halfWidth]- feature.positions[halfWidth] + d2= translated[-halfWidth-1]- feature.positions[-halfWidth-1] + for i in xrange(halfWidth): + p1 = feature.getPositionAt(i)+d1 + p2 = feature.getPositionAt(-i-1)+d2 + translated.setPosition(i,p1) + translated.setPosition(-i-1,p2) + + newObj= moving.MovingObject(newNum,timeInterval=feature.getTimeInterval(),positions=translated,velocities=obj.getVelocities()) + newObj.features=obj.features + newObj.userType=obj.userType + if plotResults: + plt.figure() + plt.title('objects_id = {}'.format(obj.num)) + newObj.plot('gx-') + feature.plot('cx-') + obj.plot('rx-') + return newObj