Iterative Closest Point (ICP) implementation on python

Question

I have been searching for an implementation of the ICP algorithm in python lately with no result.

According to wikipedia article http://en.wikipedia.org/wiki/Iterative_closest_point, the algorithm steps are:

• Associate points by the nearest neighbor criteria (for each point in one point cloud find the closest point in the second point cloud).

• Estimate transformation parameters (rotation and translation) using a mean square cost function (the transform would align best each point to its match found in the previous step).

• Transform the points using the estimated parameters.

• Iterate (re-associate the points and so on).

Well, I know that ICP is a very useful algorithm and it is used in a variety of applications. However I could not find any built in solution in Python. Am, I missing anything here?

Answer 1

Finally, I managed to write my own implementation of ICP in Python, using the sklearn and opencv libraries.

The function takes two datasets, an initial relative pose estimation and the desired number of iterations. It returns a transformation matrix that transforms the first dataset to the second.

Enjoy!

 import cv2  import numpy as np  import matplotlib.pyplot as plt  from sklearn.neighbors import NearestNeighbors   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.     '''      src = np.array([a.T], copy=True).astype(np.float32)     dst = np.array([b.T], copy=True).astype(np.float32)      #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])      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',                                 warn_on_equidistant=False).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)         #Transform the previous source and update the         #current source cloudpoint         src = cv2.transform(src, T)         #Save the transformation from the actual source cloudpoint         #to the destination         Tr = np.dot(Tr, np.vstack((T,[0,0,1])))     return Tr[0:2] 

Call it like this:

#Create the datasets ang = np.linspace(-np.pi/2, np.pi/2, 320) a = np.array([ang, np.sin(ang)]) 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]])  #Run the icp M2 = icp(a, b, [0.1,  0.33, np.pi/2.2], 30)  #Plot the result src = np.array([a.T]).astype(np.float32) res = cv2.transform(src, M2) plt.figure() plt.plot(b[0],b[1]) plt.plot(res[0].T[0], res[0].T[1], 'r.') plt.plot(a[0], a[1]) plt.show()