--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/python/objectsmoothing.py	Tue Jul 15 16:42:04 2014 -0400
@@ -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

--- a/python/utils.py	Tue Jul 15 13:59:03 2014 -0400
+++ b/python/utils.py	Tue Jul 15 16:42:04 2014 -0400
@@ -209,7 +209,7 @@
     return median([y for observedx, y in zip(X,Y) if abs(x-observedx)<halfwidth])
 
 def argmaxDict(d):
-    return max(d.iterkeys(), key=(lambda key: d[key]))
+    return max(d, key=d.get)
 
 def framesToTime(nFrames, frameRate, initialTime = time()):
     '''returns a datetime.time for the time in hour, minutes and seconds