Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
import cv2
import numpy as np
import matplotlib.pyplot as plt
from sklearn.neighbors import NearestNeighbors
#from scipy.optimize import leastsq
from scipy.optimize import fmin_bfgs
from scipy.optimize import minimize
from scipy.optimize import approx_fprime
def res(p,src,dst):
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
r = np.sum(np.square(d[:,0])+np.square(d[:,1]))
return r
def jac(p,src,dst):
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
#look at square as g(U)=sum U_i^TU_i, U_i=f_i([t_x,t_y,theta]^T)
dUdth_R = np.matrix([[-np.sin(p[2]),-np.cos(p[2])],
[ np.cos(p[2]),-np.sin(p[2])]])
dUdth = (src*dUdth_R.T).A
g = np.array([ np.sum(2*d[:,0]),
np.sum(2*d[:,1]),
np.sum(2*(d[:,0]*dUdth[:,0]+d[:,1]*dUdth[:,1])) ])
return g
def hess(p,src,dst):
n = np.size(src,0)
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]
H = np.zeros([3,3])
dUdth_R = np.matrix([[-np.sin(p[2]),-np.cos(p[2])],
[ np.cos(p[2]),-np.sin(p[2])]])
dUdth = (src*dUdth_R.T).A
H[0,0] = n*2
H[0,1] = 0
H[0,2] = np.sum(2*dUdth[:,0])
H[1,0] = 0
H[1,1] = n*2
H[1,2] = np.sum(2*dUdth[:,1])
H[2,0] = H[0,2]
H[2,1] = H[1,2]
d2Ud2th_R = np.matrix([[-np.cos(p[2]), np.sin(p[2])],
[-np.sin(p[2]),-np.cos(p[2])]])
d2Ud2th = (src*d2Ud2th_R.T).A
H[2,2] = np.sum(2*(np.square(dUdth[:,0])+np.square(dUdth[:,1]) + d[:,0]*d2Ud2th[:,0]+d[:,0]*d2Ud2th[:,0]))
return H
def debug_gradient(p,src,dst):
'''
Compare gradient with numerical approxmimation
'''
r_t_x = r_t_y = 1
g_a = jac(p,src,dst)
g_n = approx_fprime(p,res,[1.0e-10,1.0e-10,1.0e-10],src,dst)
print "g_a:",g_a
print "g_n:",g_n
H_a = hess(p,src,dst)
#element of gradient
def g_p_i(p,src,dst,i):
g = jac(p,src,dst)
return g[i]
#assuming analytical gradient is correct!
H_x_n = approx_fprime(p,g_p_i,1.0e-10,src,dst,0)
H_y_n = approx_fprime(p,g_p_i,1.0e-10,src,dst,1)
H_theta_n = approx_fprime(p,g_p_i,1.0e-10,src,dst,2)
H_n = np.zeros([3,3])
H_n[0,:] = H_x_n
H_n[1,:] = H_y_n
H_n[2,:] = H_theta_n
print "H_a:\n",H_a
print "H_n:\n",H_n
def least_squared_2d_transform(src,dst,p0):
'''
Find the translation and roation (matrix) that
gives a local optima to
sum (T(src[i])-dst[i])^T*(T(src[i])-dst[i])
src: (nx2) [x,y]
dst: (nx2) [x,y]
p0: (3x,) [x,y,theta]
'''
#least squares want's 1d functions
#result = leastsq(res,p0,Dfun=jac,col_deriv=1,full_output=1)
#p_opt = fmin_bfgs(res,p0,fprime=jac,args=(src,dst),disp=1)
result = minimize(res,p0,args=(src,dst),method='Newton-CG',jac=jac,hess=hess)
#print result
p_opt = result.x
T_opt = np.array([[np.cos(p_opt[2]),-np.sin(p_opt[2]),p_opt[0]],
[np.sin(p_opt[2]), np.cos(p_opt[2]),p_opt[1]]])
return p_opt,T_opt
def icp(a, b, init_pose=(0,0,0), no_iterations = 13):
'''
The Iterative Closest Point estimator.
Takes two cloudpoints a[x,y], b[x,y], an initial estimation of
their relative pose and the number of iterations
Returns the affine transform that transforms
the cloudpoint a to the cloudpoint b.
Note:
(1) This method works for cloudpoints with minor
transformations. Thus, the result depents greatly on
the initial pose estimation.
(2) A large number of iterations does not necessarily
ensure convergence. Contrarily, most of the time it
produces worse results.
1. For each point in the source point cloud, find the closest point in the reference point cloud.
2. Estimate the combination of rotation and translation using a mean squared error cost function that will best align each source point to its match found in the previous step.
3. Transform the source points using the obtained transformation.
4. Iterate (re-associate the points, and so on).
'''
print "init_pose:",init_pose
#print "a: ",np.shape(a)
#print "b: ",np.shape(b)
src = np.array([a.T], copy=True).astype(np.float32)
dst = np.array([b.T], copy=True).astype(np.float32)
#print "src1: ",np.shape(src)
#print "dst1: ",np.shape(dst)
#Initialise with the initial pose estimation
Tr = np.array([[np.cos(init_pose[2]),-np.sin(init_pose[2]),init_pose[0]],
[np.sin(init_pose[2]), np.cos(init_pose[2]),init_pose[1]],
[0, 0, 1 ]])
src = cv2.transform(src, Tr[0:2])
#print "src2: ",np.shape(src)
p_opt = np.array(init_pose)
for i in range(no_iterations):
#Find the nearest neighbours between the current source and the
#destination cloudpoint
nbrs = NearestNeighbors(n_neighbors=1, algorithm='auto').fit(dst[0])
distances, indices = nbrs.kneighbors(src[0])
#Compute the transformation between the current source
#and destination cloudpoint
#T = cv2.estimateRigidTransform(src, dst[0, indices.T], False) #this thing can return None for unknown reasons!!!
if i==0:
print "squared error at p0 = " + str(res([0,0,0],src[0],dst[0, indices.T][0]))
#debug_gradient([0,0,0],src[0],dst[0, indices.T][0])
p,T = least_squared_2d_transform(src[0],dst[0, indices.T][0],[0,0,0])
#Transform the previous source and update the
#current source cloudpoint
p_opt[:2] = (p_opt[:2]*np.matrix(T[:2,:2]).T).A
p_opt[0] += p[0]
p_opt[1] += p[1]
p_opt[2] += p[2]
src = cv2.transform(src, T)
#Save the transformation from the actual source cloudpoint
Tr = (np.matrix(np.vstack((T,[0,0,1])))*np.matrix(Tr)).A
p_opt[2] = p_opt[2] % (2*np.pi)
print "squared error at p_opt = " + str(res([0,0,0],src[0],dst[0, indices.T][0]))
print "p_opt:",p_opt
return p_opt,np.matrix(Tr)
if __name__ == "__main__":
import pylab
import numpy.random
fig = pylab.figure(figsize=(10,10))
ax = fig.add_subplot(111,aspect='equal')
#Create the datasets
ang = np.linspace(-np.pi/2, np.pi/2, 520)
a = np.array([ang, np.sin(ang)])
#reference is a rotated by pi/2 and translated [0.2,0.3]
th = np.pi/2
rot = np.array([[np.cos(th), -np.sin(th)],[np.sin(th), np.cos(th)]])
b = np.dot(rot, a) + np.array([[0.2], [0.3]]) #reference
idx = numpy.random.choice(520,size=60,replace=False)
a= a[:,idx]
#plot them
ref_h, = ax.plot(b[0],b[1],'b')
input_h = ax.scatter(a[0],a[1],marker='x',color='r')
#homogeneous coords
a_h = np.ones([3,np.size(a,1)])
a_h[:-1,:] = a
#guess for correct pose
#init_pose=[-1.0,0,0.1]
init_pose=[0.2+5,0.3-7,th+0.7]
#init_pose=[0,0,0]
T_g = np.matrix([[np.cos(init_pose[2]),-np.sin(init_pose[2]),init_pose[0]],
[np.sin(init_pose[2]), np.cos(init_pose[2]),init_pose[1]],
[0, 0, 1 ]])
a_g = T_g*a_h
guess_h = ax.scatter(a_g[0],a_g[1],marker='o',color='g')
#Run the icp
p_opt,T_opt = icp(a, b,init_pose,no_iterations=35)
a_opt = T_opt*a_h
result_h = ax.scatter(a_opt[0],a_opt[1],marker='o',color='k')
ax.legend((ref_h,input_h,guess_h,result_h),('reference','input','guess','result'),scatterpoints=1)
pylab.show()
@rebeen

This comment has been minimized.

Copy link

@rebeen rebeen commented Oct 15, 2017

what I understand this cannot be used for different data am I right ? can I used this for align two tsne(2d cloud points data )

@TasnubaS

This comment has been minimized.

Copy link

@TasnubaS TasnubaS commented Jan 29, 2019

With minor adjustments

import cv2
import numpy as np
import matplotlib.pyplot as plt
from sklearn.neighbors import NearestNeighbors
#from scipy.optimize import leastsq
from scipy.optimize import fmin_bfgs
from scipy.optimize import minimize
from scipy.optimize import approx_fprime

def res(p,src,dst):
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]

r = np.sum(np.square(d[:,0])+np.square(d[:,1]))
return r

def jac(p,src,dst):
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]

#look at square as g(U)=sum U_i^TU_i, U_i=f_i([t_x,t_y,theta]^T)
dUdth_R = np.matrix([[-np.sin(p[2]),-np.cos(p[2])],
                    [ np.cos(p[2]),-np.sin(p[2])]])
dUdth = (src*dUdth_R.T).A
g = np.array([  np.sum(2*d[:,0]),
                np.sum(2*d[:,1]),
                np.sum(2*(d[:,0]*dUdth[:,0]+d[:,1]*dUdth[:,1])) ])
return g

def hess(p,src,dst):
n = np.size(src,0)
T = np.matrix([[np.cos(p[2]),-np.sin(p[2]),p[0]],
[np.sin(p[2]), np.cos(p[2]),p[1]],
[0 ,0 ,1 ]])
n = np.size(src,0)
xt = np.ones([n,3])
xt[:,:-1] = src
xt = (xt*T.T).A
d = np.zeros(np.shape(src))
d[:,0] = xt[:,0]-dst[:,0]
d[:,1] = xt[:,1]-dst[:,1]

H = np.zeros([3,3])

dUdth_R = np.matrix([[-np.sin(p[2]),-np.cos(p[2])],
                    [ np.cos(p[2]),-np.sin(p[2])]])
dUdth = (src*dUdth_R.T).A

H[0,0] = n*2
H[0,1] = 0
H[0,2] = np.sum(2*dUdth[:,0])

H[1,0] = 0
H[1,1] = n*2
H[1,2] = np.sum(2*dUdth[:,1])

H[2,0] = H[0,2]
H[2,1] = H[1,2]

d2Ud2th_R = np.matrix([[-np.cos(p[2]), np.sin(p[2])],
                       [-np.sin(p[2]),-np.cos(p[2])]])
d2Ud2th = (src*d2Ud2th_R.T).A

H[2,2] = np.sum(2*(np.square(dUdth[:,0])+np.square(dUdth[:,1]) + d[:,0]*d2Ud2th[:,0]+d[:,0]*d2Ud2th[:,0]))
return H

def debug_gradient(p,src,dst):
'''
Compare gradient with numerical approxmimation
'''
r_t_x = r_t_y = 1

g_a = jac(p,src,dst)
g_n = approx_fprime(p,res,[1.0e-10,1.0e-10,1.0e-10],src,dst)

print ("g_a:",g_a)
print ("g_n:",g_n)

H_a = hess(p,src,dst)

#element of gradient
def g_p_i(p,src,dst,i):
    g = jac(p,src,dst)
    return g[i]

#assuming analytical gradient is correct!
H_x_n     = approx_fprime(p,g_p_i,1.0e-10,src,dst,0)
H_y_n     = approx_fprime(p,g_p_i,1.0e-10,src,dst,1)
H_theta_n = approx_fprime(p,g_p_i,1.0e-10,src,dst,2)
H_n = np.zeros([3,3])
H_n[0,:] = H_x_n
H_n[1,:] = H_y_n
H_n[2,:] = H_theta_n

print ("H_a:\n",H_a)
print ("H_n:\n",H_n)

def least_squared_2d_transform(src,dst,p0):
'''
Find the translation and roation (matrix) that
gives a local optima to

sum (T(src[i])-dst[i])^T*(T(src[i])-dst[i])

src: (nx2) [x,y]
dst: (nx2) [x,y]
p0:  (3x,) [x,y,theta]

'''


#least squares want's 1d functions
#result = leastsq(res,p0,Dfun=jac,col_deriv=1,full_output=1)
    
#p_opt  = fmin_bfgs(res,p0,fprime=jac,args=(src,dst),disp=1)        
result  = minimize(res,p0,args=(src,dst),method='Newton-CG',jac=jac,hess=hess)
#print result
p_opt = result.x
T_opt  = np.array([[np.cos(p_opt[2]),-np.sin(p_opt[2]),p_opt[0]],
                   [np.sin(p_opt[2]), np.cos(p_opt[2]),p_opt[1]]])
return p_opt,T_opt

def icp(a, b, init_pose=(0,0,0), no_iterations = 14):
'''
The Iterative Closest Point estimator.
Takes two cloudpoints a[x,y], b[x,y], an initial estimation of
their relative pose and the number of iterations
Returns the affine transform that transforms
the cloudpoint a to the cloudpoint b.
Note:
(1) This method works for cloudpoints with minor
transformations. Thus, the result depents greatly on
the initial pose estimation.
(2) A large number of iterations does not necessarily
ensure convergence. Contrarily, most of the time it
produces worse results.

1. For each point in the source point cloud, find the closest point in the reference point cloud.
2. Estimate the combination of rotation and translation using a mean squared error cost function that will best align each source point to its match found in the previous step.
3. Transform the source points using the obtained transformation.
4. Iterate (re-associate the points, and so on).
'''

print ("init_pose:",init_pose)

#print "a: ",np.shape(a)
#print "b: ",np.shape(b)

src = np.array([a.T], copy=True).astype(np.float32)
dst = np.array([b.T], copy=True).astype(np.float32)

#print "src1: ",np.shape(src)
#print "dst1: ",np.shape(dst)

#Initialise with the initial pose estimation
Tr = np.array([[np.cos(init_pose[2]),-np.sin(init_pose[2]),init_pose[0]],
               [np.sin(init_pose[2]), np.cos(init_pose[2]),init_pose[1]],
               [0,                    0,                   1          ]])

src = cv2.transform(src, Tr[0:2])
#print "src2: ",np.shape(src)

p_opt = np.array(init_pose)
    
for i in range(no_iterations):
    #Find the nearest neighbours between the current source and the
    #destination cloudpoint
    nbrs = NearestNeighbors(n_neighbors=1, algorithm='auto').fit(dst[0])
    distances, indices = nbrs.kneighbors(src[0])

    #Compute the transformation between the current source
    #and destination cloudpoint
    #T = cv2.estimateRigidTransform(src, dst[0, indices.T], False) #this thing can return None for unknown reasons!!!

    if i==0:
        print ("squared error at p0 = " + str(res([0,0,0],src[0],dst[0, indices.T][0])))

        
    #debug_gradient([0,0,0],src[0],dst[0, indices.T][0])
        
    p,T = least_squared_2d_transform(src[0],dst[0, indices.T][0],[0,0,0])
                           
    #Transform the previous source and update the
    #current source cloudpoint
    p_opt[:2]  = (p_opt[:2]*np.matrix(T[:2,:2]).T).A       
    p_opt[0] += p[0]
    p_opt[1] += p[1]
    p_opt[2] += p[2]
    
    src = cv2.transform(src, T)
    #Save the transformation from the actual source cloudpoint        
    Tr = (np.matrix(np.vstack((T,[0,0,1])))*np.matrix(Tr)).A
    
p_opt[2] = p_opt[2] % (2*np.pi)
print ("squared error at p_opt = " + str(res([0,0,0],src[0],dst[0, indices.T][0])))
print ("p_opt:",p_opt)

return p_opt,np.matrix(Tr)

if name == "main":
import pylab
import numpy.random

fig = pylab.figure(figsize=(10,10))
ax  = fig.add_subplot(111,aspect='equal')

#Create the datasets
ang = np.linspace(-np.pi/2, np.pi/2, 520)
a = np.array([ang, np.sin(ang)])

#reference is a rotated by pi/2 and translated [0.2,0.3]
th = np.pi/2
rot = np.array([[np.cos(th), -np.sin(th)],[np.sin(th), np.cos(th)]])
b = np.dot(rot, a) + np.array([[0.2], [0.3]]) #reference

idx = numpy.random.choice(520,size=60,replace=False)
a= a[:,idx]

#plot them
ref_h,   = ax.plot(b[0],b[1],'b')
input_h  = ax.scatter(a[0],a[1],marker='x',color='r')

#homogeneous coords
a_h = np.ones([3,np.size(a,1)])
a_h[:-1,:] = a

#guess for correct pose 
#init_pose=[-1.0,0,0.1]
init_pose=[0.2+5,0.3-7,th+0.7]
#init_pose=[0,0,0]
T_g = np.matrix([[np.cos(init_pose[2]),-np.sin(init_pose[2]),init_pose[0]],
               [np.sin(init_pose[2]), np.cos(init_pose[2]),init_pose[1]],
               [0,                    0,                   1          ]])

a_g = T_g*a_h
a_g0 = np.squeeze(np.asarray(a_g[0]))
a_g1 = np.squeeze(np.asarray(a_g[1]))
guess_h  = ax.scatter(a_g0,a_g1,marker='o',color='g')


#Run the icp
p_opt,T_opt = icp(a, b,init_pose,no_iterations=34)
a_opt = T_opt*a_h
a_opt0 = np.squeeze(np.asarray(a_opt[0]))
a_opt1 = np.squeeze(np.asarray(a_opt[1]))

result_h = ax.scatter(a_opt0,a_opt1,marker='o',color='k')

ax.legend((ref_h,input_h,guess_h,result_h),('reference','input','guess','result'),scatterpoints=1)

pylab.show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.