Frechet Distance (iterative version)

FrechetDistance.py

"""Original Code from Tim Wylie http://www.cs.montana.edu/~timothy.wylie/frechet/ 
A. Pembroke added modifications to combine frechet distance with closest ordered pairs
and implemented an iterative version of the original recursive discrete frechet calculation, df_iterative.
"""

##############################################################################################################
##############################################################################################################
## File: fpact_frechet.py
## Author: Tim Wylie
## Copyright: Tim Wylie
## Version: 1.0 
##############################################################################################################
##############################################################################################################

import numpy
import array

#
#this class is based off of the 94 paper "Computing discrete Frechet distance"
#the class has two subroutines - one does the recursion, the other is the 
#distance function in case this method changes
#rather than map everything to zero based arrays the arrays are simply assigned 1 greater
class Discrete_frechet_distance():
    #
    #init function(constructor) declares variables and then runs the function
    def __init__(self,p,q):
        self.P = p
        self.Q = q
        self.nP = len(self.P[:,0])
        self.nQ = len(self.Q[:,0])
        self.ca = numpy.ones((self.nP+1,self.nQ+1))*-1.
        #[[-1.0]*(len(self.Q)+1) for x in xrange(len(self.P)+1)] #creates a pxq matrix
        self.dist = numpy.ones((self.nP+1,self.nQ+1))*-1.
        #[[-1.0]*(len(self.Q)+1) for x in xrange(len(self.P)+1)] #creates a pxq matrix
        #actually a p+1 x q+1 and we ignore the first row/column
        self.result = -1.0
        self.first = [-1,-1,-1.]
        self.last = [-1,-1,-1.]
        self.df_connectivity = None
        self.dist_connectivity = None
        self.add_connectivity = None
        self.switch_connectivity = None
        self.closestQ = numpy.arange(self.nP*2).reshape(2,self.nP).T
        self.closestP = numpy.arange(self.nQ*2).reshape(2,self.nQ).T
        self.df_iterative() #avoids recursive call to df_paper_c 
#         self.run() 
        self.df_sequence() #connectivity for shortest leash
        print 'getting dist sequence'
        self.dist_sequence() #connectivity for shortest distance
        self.find_pairs()
        self.closest()
        self.add_sequence()
        self.switch_sequence()
    #
    #finds the distance between two elements and stores it in a distance matrix    
    def distance(self,p,q):
        p1 = self.P[p-1,:] #[Px,Py]
        p2 = self.Q[q-1,:] #[Qx,Qy]
        delta = p1-p2 #[Px-Qx,Py-Qy]
        # ret_dist = abs(p1[1]-p2[1])
        ret_dist = numpy.sqrt(numpy.dot(delta,delta))
        # print '(pi,qj),[delta],dist', (p-1,q-1), delta, ret_dist
        # ret_dist =  numpy.sqrt(pow(p1[0]-p2[0],2) + pow(p1[1]-p2[1],2) + pow(p1[2]-p2[2],2))
        self.dist[p,q] = ret_dist
        return ret_dist
    #    
    #sets the first and second largest frechet pairs for alignment
    def find_pairs(self):
        self.last[2] = self.result
        for i in range(len(self.dist[:,0])):
            for j in range(len(self.dist[0,:])):
                if self.dist[i,j] == self.result:
                    self.first = [i,j,self.dist[i,j]]
                if self.dist[i,j] < self.last[2] and self.dist[i,j]>0.:
                    self.last = [i,j,self.dist[i,j]]


    def df_paper_c(self,i,j):
        if self.ca[i,j] > -1:
            pass    #return self.ca[i][j] #could pass
        elif i == 1 and j == 1:
            self.ca[i,j] = self.distance(1,1)
        elif i > 1 and j == 1:
            self.ca[i,j] = max(self.df_paper_c(i-1,1),self.distance(i,1))
        elif i == 1 and j > 1:
            self.ca[i,j] = max(self.df_paper_c(1,j-1),self.distance(1,j))
        elif i > 1 and j > 1:
            self.ca[i,j] = max(min(self.df_paper_c(i-1,j),self.df_paper_c(i-1,j-1),self.df_paper_c(i,j-1)),self.distance(i,j))
        else:
            self.ca[i,j] = 9999999999  #infinity
        return self.ca[i,j]
        
    #
    #calls the function asking for the optimal result
    def run(self):
        self.result = self.df_paper_c(len(self.P),len(self.Q))
        return self.result


    def df_iterative(self): #Iterative version of df_paper_c
        #assumes ca and dist are arrays of PN+1, QN+1, and [0,:] and ca[:,0] are left empty
        self.ca[1,1] = self.distance(1,1)
        print 'NP,NQ=', (self.nP,self.nQ)
        for i in range(2,self.nP+1):
            self.ca[i,1] = max(self.ca[i-1,1], self.distance(i,1))
        for j in range(2,self.nQ+1):
            self.ca[1,j] = max(self.ca[1,j-1], self.distance(1,j))
        for i in range(2,self.nP+1):
            for j in range(2,self.nQ+1):
                self.ca[i,j] = max(min(self.ca[i-1,j],self.ca[i-1,j-1],self.ca[i,j-1]),self.distance(i,j))
        return self.ca[self.nP,self.nQ]

    def df_sequence(self): 
        """Find connectivity by marching down ca gradient. Note: this sequence is not unique"""
        if self.df_connectivity is None:
            self.df_connectivity = self.gradient_descent(self.ca)
        return self.df_connectivity

    def dist_sequence(self):
        """Find connectivity by marching down distance gradient.
        This could be done without the full distance function"""
        if self.dist_connectivity is None:
            self.dist_connectivity = self.gradient_descent(self.dist)
        return self.dist_connectivity

    def add_sequence(self):
        if self.add_connectivity is None:
            self.add_connectivity = self.gradient_descent(self.dist + self.ca)

    def switch_sequence(self):
        if self.switch_connectivity is None:
            self.switch_connectivity = self.get_switch_sequence()

    def gradient_descent(self,function):
        connections = []
        connections.append([self.nP-1,self.nQ-1])
        (i,j) = (self.nP,self.nQ)
        prev = function[i,j]
        shift = -1
        while i > 1 and j > 1:
            a = [function[i-1,j], function[i-1,j-1], function[i,j-1]]
            next = a.index(min(a))
            # if min(a)
            if next == 0:
                connections.append([i-1+shift,j+shift])
                i = i-1
            elif next == 1:
                connections.append([i-1+shift,j-1+shift])
                i = i-1
                j = j-1
            elif next == 2:
                # print 'j shift', a
                connections.append([i+shift,j-1+shift])
                j = j-1
            else: print 'error! could not shift!'
        while i > 1:
            connections.append([i-1+shift,j+shift])
            i = i - 1
        while j > 1:
            connections.append([i+shift,j-1+shift])
            j = j - 1 
        return numpy.array(connections)

    def get_switch_sequence(self):
        """Follow the distance function. If you get stuck, follow the ca function
        until you are out of the local minimum. Results are [P_i, Q_j]"""
        connections = []
        connections.append([self.nP-1,self.nQ-1])
        (i,j) = (self.nP,self.nQ)
        shift = -1
        while i > 1 and j > 1:
            ca = [self.ca[i-1,j], self.ca[i-1,j-1], self.ca[i,j-1]]
            dist = [self.dist[i-1,j], self.dist[i-1,j-1], self.dist[i,j-1]]
            old_dist = self.dist[i,j]
            if min(dist) < old_dist:
                next = dist.index(min(dist))
            else:
                next = ca.index(min(ca))
            if next == 0:
                connections.append([i-1+shift,j+shift])
                i = i-1
            elif next ==1:
                connections.append([i-1+shift,j-1+shift])
                i = i-1
                j = j-1
            else:
                connections.append([i+shift,j-1+shift])
                j = j-1
        while i > 1:
            connections.append([i-1+shift,j+shift])
            i = i - 1
        while j > 1:
            connections.append([i+shift,j-1+shift])
            j = j - 1 
        return numpy.array(connections)


    def closest(self):
        distList = self.dist[1:,1:].tolist()
        for i,delta in enumerate(distList): #rows
            self.closestQ[i,1] = delta.index(min(delta)) #index of closest point in Q
        distList = self.dist[1:,1:].T.tolist() #now on columns
        for i,delta in enumerate(distList): 
            self.closestP[i,1] = delta.index(min(delta))
            
"""Tests frechet distance function"""
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import time
from math import pi
import numpy.ma as ma

(NP, NQ) = (200,100)
(phi_i, phi_f) = (pi/3, 5*pi/3)
(seg0,seg1) = (9*pi/8, 11*pi/8)

def wavyCurve(NP,phi_i,phi_f,seg0,seg1, radius, amplitude, cycles):	 
	NP1 = int(NP*(seg0-phi_i)/(phi_f-phi_i))
	NP3 = int(NP*(phi_f-seg1)/(phi_f-phi_i))
	NP2 = NP-NP1-NP3
	print 'NP1, NP2, NP3:', (NP1,NP2,NP3), 'NP=', NP

	Pphi1 = np.linspace(phi_i,seg0,NP1)
	Pr1 = radius*np.ones(NP1)
	Px1 = Pr1*np.cos(Pphi1)
	Py1 = Pr1*np.sin(Pphi1)

	Pphi2 = np.linspace(seg0,seg1, NP2)
	Pr2 = radius*np.ones(NP2) + amplitude*np.sin(2*cycles*pi*(Pphi2-seg0)/(seg1-seg0))
	Px2 = Pr2*np.cos(Pphi2)
	Py2 = Pr2*np.sin(Pphi2)

	Pphi3 = np.linspace(seg1,phi_f,NP3)
	Pr3 = radius*np.ones(NP3)
	Px3 = Pr3*np.cos(Pphi3)
	Py3 = Pr3*np.sin(Pphi3)
	Px = np.concatenate([Px1,Px2,Px3])
	Py = np.concatenate([Py1,Py2,Py3])
	return np.array([Px,Py]).T

P = wavyCurve(NP, pi/3, 5*pi/3, pi/2, 6*pi/4, 1, .1, 4)
Q = wavyCurve(NQ, pi/3, 5*pi/3, pi/2, pi, .6, .1, 3)

t1 = time.clock()
fPQ = Discrete_frechet_distance(P,Q)
t2 = time.clock()
print 'frechet distance:', fPQ.result
print 'time to compute frechet distance:', t2-t1

# figure(num=None, figsize=(8, 6), dpi=80, facecolor='w', edgecolor='k')
plt.figure(1, figsize=(18, 10), dpi=100, facecolor='w', edgecolor='k')

plt.subplot(2,3,1, aspect='equal')
plt.imshow(fPQ.dist[1:,1:], interpolation='nearest')
plt.axis([NQ, 1, 1, NP])
plt.colorbar()
plt.plot(fPQ.dist_connectivity[:,1],fPQ.dist_connectivity[:,0], 'g', linewidth=2)
plt.plot(fPQ.switch_connectivity[:,1],fPQ.switch_connectivity[:,0], 'm', linewidth=2)
# plt.plot(fPQ.closestP[:,0],fPQ.closestP[:,1], 'r')
# plt.plot(fPQ.closestQ[:,1],fPQ.closestQ[:,0], 'b')
# plt.legend(("closest", "closest + frechet"))
plt.title("Closest")
plt.xlabel("Q")
plt.ylabel("P")


plt.subplot(2,3,4, aspect='equal')
plt.imshow(fPQ.ca[1:,1:], interpolation='nearest')
plt.axis([NQ, 1, 1, NP])
plt.colorbar()
# plt.contour(fPQ.ca)
plt.title("Frechet")
plt.plot(fPQ.df_connectivity[:,1],fPQ.df_connectivity[:,0], 'k', linewidth=2)
plt.plot(fPQ.first[1]-1,fPQ.first[0]-1,'mo')
plt.xlabel("Q")
plt.ylabel("P")

def plotConnectivity(Px,Py,Qx,Qy,connectivity,frechetObject, colors):
	plt.plot(Px, Py, colors[0], Qx, Qy,colors[1])
	for i in range(len(connectivity)):
		pi = connectivity[i][0]
		qi = connectivity[i][1]
		# print 'pi,qi =', (pi,qi)
		x = np.array([Px[pi],Qx[qi]])
		y = np.array([Py[pi],Qy[qi]])
		if pi == frechetObject.first[0]-1 and qi == frechetObject.first[1]-1:
			print 'found largest frechet pair',(pi,qi)
			plt.plot(x,y,'--k',linewidth=2)
		else: plt.plot(x,y,colors[2], linewidth=.2)

plt.subplot(2,3,2, aspect='equal') #connectivity for closest points
colors = ['r','b','r']
plotConnectivity(Q[:,0],Q[:,1],P[:,0],P[:,1],fPQ.closestP,fPQ, colors)
colors = ['b','r','b']
plotConnectivity(P[:,0],P[:,1],Q[:,0],Q[:,1],fPQ.closestQ,fPQ, colors)
plt.title("Closest points")

plt.subplot(2,3,3, aspect='equal') #connectivity for sequential pairs
colors = ['b','r','g']
plotConnectivity(P[:,0],P[:,1],Q[:,0],Q[:,1],fPQ.dist_connectivity,fPQ,colors)
plt.title("Closest Sequential Pairs")

plt.subplot(2,3,5, aspect='equal')
colors = ['b','r','k']
plotConnectivity(P[:,0],P[:,1],Q[:,0],Q[:,1],fPQ.df_connectivity,fPQ, colors)
plt.title("Discrete Frechet Pairs")

plt.subplot(2,3,6, aspect='equal')
colors = ['b','r','m']
plotConnectivity(P[:,0],P[:,1],Q[:,0],Q[:,1],fPQ.switch_connectivity,fPQ,colors)
plt.title("Frechet and Closest")



plt.show()