{{ message }}

Instantly share code, notes, and snippets.

# ecward/ICP_matching.py

Created Aug 21, 2015
 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 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 commented Jan 29, 2019 • edited

 `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() ``````