--- a/python/objectsmoothing.py	Sun Dec 07 23:01:02 2014 -0500
+++ b/python/objectsmoothing.py	Wed Dec 10 15:27:08 2014 -0500
@@ -1,131 +1,234 @@
-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
+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,features,featureID):
+    #longestFeature = utils.argmaxDict({f:f.length() for i,f in enumerate(obj.features)})
+    t1,t3 = features[featureID].getFirstInstant(), features[featureID].getLastInstant()
+    listFeatures=[[features[featureID],t1,t3,moving.Point(0,0)]]
+    # find the features to fill in the beginning of the object existence
+    currentFeature = features[featureID]
+    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 = features[featureID]
+    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,features,featureID,num=1):
+    listFeatures= getFeatures(obj,features,featureID)
+    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]    
+
+#Quantitative analysis "CSJ" functions    
+def computeVelocities (object,smoothing=True,halfWidth=3):  #compute velocities from positions
+    velocities={}
+    for i in list(object.timeInterval)[:-1]:
+        p1= object.getPositionAtInstant(i)
+        p2= object.getPositionAtInstant(i+1)
+        velocities[i]=p2-p1        
+    velocities[object.getLastInstant()]= velocities[object.getLastInstant()-1]  # duplicate last point
+    if smoothing:
+        velX= [velocities[y].aslist()[0] for y in sorted(velocities.keys())]
+        velY= [velocities[y].aslist()[1] for y in sorted(velocities.keys())]
+        v1= list(utils.filterMovingWindow(velX, halfWidth))
+        v2= list(utils.filterMovingWindow(velY, halfWidth))
+        smoothedVelocity={}
+        for t,i in enumerate(sorted(velocities.keys())):
+            smoothedVelocity[i]=moving.Point(v1[t], v2[t])
+        velocities=smoothedVelocity
+    return velocities
+    
+def computeAcceleration (object,fromPosition=True):
+    acceleration={}
+    if fromPosition:
+        velocities=computeVelocities(object,False,1)
+        for i in sorted (velocities.keys()):
+            if i != sorted (velocities.keys())[-1]:
+                acceleration[i]= velocities[i+1]-velocities[i]
+    else:
+        for i in list(object.timeInterval)[:-1]:
+            v1= object.getVelocityAtInstant(i)
+            v2= object.getVelocityAtInstant(i+1)
+            acceleration[i]= v2-v1
+    return acceleration
+    
+def computeJerk (object,fromPosition=True):
+    jerk={}
+    acceleration=computeAcceleration (object,fromPosition=fromPosition)
+    for i in sorted (acceleration.keys()):
+        if i != sorted (acceleration.keys())[-1]:
+            jerk[i]= (acceleration[i+1]-acceleration[i]).norm2()
+    return jerk
+    
+def sumSquaredJerk (object,fromPosition=True):
+    jerk= computeJerk (object,fromPosition=fromPosition)
+    t=0
+    for i in sorted(jerk.keys()):
+        t+= jerk[i]* jerk[i]
+    return t
+    
+def smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=False,halfWidth=3,create=False):
+    results=[]    
+    bearing={}
+    if create:
+        feature= buildFeature(obj,features,featureID,num=1)
+    else:
+        feature=features[featureID]
+    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])
+        import utils
+        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)
+    return newObj
+    
+def smoothObjectTrajectory(obj,features,newNum,minLengthParam=0.7,smoothing=False,plotResults=True,halfWidth=3,computeVelocities=True,optimize=True,create=False):
+    featureList=[i.num for i in obj.features if i.length() >= minLengthParam*obj.length()]
+    if featureList==[]:
+        featureList.append(longestFeature(obj))
+        create=True
+    objs=[]
+    for featureID in featureList:
+        objTMP=smoothObjectTrajectory(obj,features,featureID,newNum,smoothing=smoothing,halfWidth=halfWidth,create=create)
+        objs.append(objTMP)
+    newTranslated = moving.Trajectory()
+    newInterval=[]
+    for t in obj.timeInterval:
+        xCoord=[]
+        yCoord=[]
+        for i in objs:
+            if i.existsAtInstant(t):
+                p1= i.getPositionAtInstant(t)
+                xCoord.append(p1.x)
+                yCoord.append(p1.y)
+        if xCoord!=[]:
+            tmp= moving.Point(np.median(xCoord),np.median(yCoord))
+            newInterval.append(t)
+            newTranslated.addPosition(tmp)
+    
+    newObj= moving.MovingObject(newNum,timeInterval=moving.TimeInterval(min(newInterval),max(newInterval)),positions=newTranslated)
+        
+    if computeVelocities:
+        tmpTraj = moving.Trajectory()
+        velocities= computeVelocities(newObj,True,5)
+        for i in sorted(velocities.keys()):
+            tmpTraj.addPosition(velocities[i])
+        newObj.velocities=tmpTraj
+    else:
+        newObj.velocities=obj.velocities
+    
+    if optimize:
+        csj1= sumSquaredJerk (obj,fromPosition=True)
+        csj2= sumSquaredJerk (newObj,fromPosition=True)
+        if csj1<csj2:
+            newObj=obj
+            newObj.velocities=obj.velocities
+        if computeVelocities and csj1>=csj2:
+            csj3= sumSquaredJerk (obj,fromPosition=False)
+            csj4= sumSquaredJerk (newObj,fromPosition=False)
+            if csj4<=csj3:
+                newObj.velocities= obj.velocities
+    newObj.featureNumbers=obj.featureNumbers
+    newObj.features=obj.features
+    newObj.userType=obj.userType
+    if plotResults:
+        plt.figure()
+        plt.title('objects_id = {}'.format(obj.num))
+        for i in featureList:
+            features[i].plot('cx-')
+        obj.plot('rx-')
+        newObj.plot('gx-')        
+    return newObj