An Iterative Closest Point Algorithm

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.


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 )


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:
        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 ) ] 
        err, rotation = rot[0]
        if err < threshold:
        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 ) ]
        err, scaling = scale[0]
        if err < threshold:
        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 )


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:


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 )