In this post I’ll demonstrate an iterative closest point (ICP) algorithm that works reasonably well. An ICP algorithm seeks to find a transformation between two sets of points that minimizes the error between them, i.e., you are trying to find a transformation that will lay one set of points exactly on top of another.
Data
I have assumed that one set of points is a rotated, shifted and scaled version of the set of target points. I have also assumed that the two sets of points have been shuffled so that the first point of one point set does not necessarily map to the first point of the other data set.
In this first bit of code we will import the relevant modules, generate a target set, X
, and a rotated, shifted and scaled version, Y
.
import numpy as np import scipy.stats from pylab import * import random X = np.matrix( scipy.stats.norm(0,100).rvs((500,2)) ) # rotate by some angle theta theta = scipy.stats.uniform(0.0,2.0*np.pi).rvs() c, s = np.cos( theta ), np.sin( theta ) # rotation matrix T0 = np.matrix( [[c,-s],[s,c]] ) # translation vector T1 = np.matrix( scipy.stats.norm((3,3),1).rvs((2,1)) ) # scaling factor k = scipy.stats.uniform( 1, 5 ).rvs() def T( x, T0, T1, k=1.0 ): # apply an affine transformation to `x` y = x * T0.T y[:,0] += T1[0,0] y[:,1] += T1[1,0] return y*k Y = T( X, T0, T1, k )
For a little more realism, we will assume that the set Y
has been shuffled to produce sY
.
def shuffle( X ): N = X.shape[0] idx = range( N ) np.random.shuffle( idx ) return X[idx] sY = shuffle( Y )
Code
Next we will implement some functions that will randomly transform the point set in hopes of stumbling on the optimal transformation. Each of these functions includes an argument errorfct
that we will implement in the next listing.
def translate( X, Y, errorfct ): # translate to align the centers of mass mx = np.mean( X, axis=0 ).T my = np.mean( Y, axis=0 ).T translation = mx - my I = np.matrix( np.eye( 2 ) ) Yp = T( Y, I, translation ) return errorfct( X, Yp ), translation def randrot( X, Y, errorfct ): # perform a random rotation theta = scipy.stats.uniform( 0.0, 2.0*np.pi ).rvs() c = np.cos( theta ) s = np.sin( theta ) rotation = np.matrix( [[c,-s],[s,c]] ) Z = np.matrix( np.zeros((2,1)) ) Yp = T( Y, rotation, Z ) return errorfct( X, Yp ), rotation def randscale( X, Y, errorfct ): # perform a random scaling k = scipy.stats.uniform( 0.5, 1.0 ).rvs() scaling = k * np.matrix( np.eye(2) ) Z = np.matrix( np.zeros((2,1)) ) Yp = T( Y, scaling, Z ) return errorfct( X, Yp ), scaling
The first function here is the traditional sum squared error. The next two functions implement another distance measure that I’ve termed the nearest sum of squares error (NSSE). This is the sum of the squares of the distances between each point in X
, and the nearest point in Y
. By minimizing this function, then we can lay Y
on top of X
without their points being in the same order.
def SSE( X, Y ): return np.sum( np.array( X - Y )**2.0 ) def ptSSE( pt, X ): ''' Point-wise smallest squared error. This is the distance from the point `pt` to the closest point in `X` ''' difference = pt - X # x and y columns xcol = np.ravel( difference[:,0] ) ycol = np.ravel( difference[:,1] ) # sum of the squared differences btwn `pt` and `X` sqr_difference = xcol**2.0 + ycol**2.0 # nearest squared distance distance = np.min( sqr_difference ) # index of the nearest point to `pt` in `X` nearest_pt = np.argmin( sqr_difference ) return distance def NSSE( X, Y ): ''' Nearest sum squared error. This is the sum of the squares of the nearest differences between the points of `X` and the points of `Y` ''' err = 0.0 for x in X: err += ptSSE( x, Y ) return err
This function fits one point set to another, and then outputs the transformed point set, but it can easily be adjusted to output the transformation matrices instead.
def fit( X, Y, M, N, errorfct, threshold=1e-5 ): T0 = list() T1 = list() errors = list() errors.append( errorfct( X, Y ) ) print errors[-1] Yp = Y.copy() for iter in range( M ): err, translation = translate( X, Yp, errorfct ) if err < threshold: break elif err < errors[-1]: errors.append( err ) print errors[-1] T1.append( translation ) I = np.matrix( np.eye( 2 ) ) Yp = T( Yp, I, T1[-1] ) rot = [ randrot( X, Yp, errorfct ) for i in range( N ) ] rot.sort() err, rotation = rot[0] if err < threshold: break elif err < errors[-1]: errors.append( err ) print errors[-1] T0.append( rotation ) Z = np.matrix( np.zeros((2,1)) ) Yp = T( Yp, T0[-1], Z ) scale = [ randscale( X, Yp, errorfct ) for i in range( N ) ] scale.sort() err, scaling = scale[0] if err < threshold: break elif err < errors[-1]: errors.append( err ) print errors[-1] T0.append( scaling ) Z = np.matrix( np.zeros((2,1)) ) Yp = T( Yp, T0[-1], Z ) return Yp
We can execute the above functions with the following call, specifying the shuffled set sY
, M=30
iterations of the outer loop, N=100
iterations of the smaller inner loops, the nearest sum squared error as the error function, and a threshold of 0.0001.
Yp = fit( X, sY, M=30, N=100, errorfct=NSSE, threshold=1e-3 )
Conclusion
I found that this works pretty decently for 100 data points when scaling is applied, and it works decently for around 1000 data points when there is no scaling. I tried to find some tricks to guess the appropriate scaling factor, but those did not work out. Here is an example of the convergence for a 100 point sample:
193765.764406 156346.678914 88290.4226445 74201.2394595 61194.4112199 59713.4603787 59542.5049091 58225.7140321 49076.1127773 35906.7230167 4106.79582267 957.056808684 59.8736429318 47.8269679602 47.7306388826 3.22586740889 1.31683052262 0.347469043719 0.315176517988 0.314942210877 0.312448131649 0.312443259138
And here is a visualization of the output
scatter( np.ravel(X[:,1]), np.ravel(X[:,0]), marker='x', facecolor='b', s=100 ) scatter( np.ravel(Yp[:,1]), np.ravel(Yp[:,0]), marker='o', edgecolor='r', facecolor='none', s=100 ) grid() ; title('ICP with 100 Points') savefig('icp_100pts.png', fmt='png', dpi=200 )