Iterative Clostest Point (ICP)

Estimation of transformation parameters without known correspondences. We are given two sets of points left and right and our goal is to estimate:

  • correspondences between the points
  • Tranformation between the sets

This page is available for practice as an interactive jupyter notebook.

Approach

  1. Set approximate values (here: estimate rotation)
  2. Assign points of left and right point cloud to each other
  3. Compute transformation using the correspondences from 2.
  4. Transform left point cloud
  5. Compute mean squared error (MSE)
    • Iteration: Go to line 2. until convergence
  6. Compute final transformation

Set approximate values

Translation and scale a given.

$s_0=1$, $ t_0=\begin{pmatrix} 0.6
1.0
-0.2 \end{pmatrix}$

The initial rotation matrix $R_0$ can be estimated visualy. You may use the provided animation function. Optional you may optimize approximate values by (rndomized) grid search.

# imports

import numpy as np

from tools import rotmat_from_quat
from plyfile import PlyData

from tools import rotmat_from_quat
from sklearn.neighbors import KDTree

visualization = True  # Use this flag to deactive the visualization
if visualization:
    import ipyvolume as ipv

def animate(lefts_array, right):
    lefts_array = np.stack(lefts_array)
    ipv.figure()
   
    if len(lefts_array.shape) == 3 : 
        sl = ipv.scatter(lefts_array[:,:,0], lefts_array[:,:,1], lefts_array[:,:,2], size=1, marker='sphere',color='green', color_selected='white')
    elif len(lefts_array.shape) == 2:
        sl = ipv.scatter(lefts_array[:,0], lefts_array[:,1], lefts_array[:,2], size=1, marker='sphere',color='green', color_selected='white')
    else:
        raise UnkownError("Unsupported state.")
        
    ipv.scatter(right[:,0], right[:,1], right[:,2],  size=1, marker='sphere',color='red')
    ipv.animation_control(sl) # shows controls for animation controls
    ipv.show()


# load data
_data = PlyData.read('tango5cm.ply').elements[0].data
left = np.stack((_data['x'], _data['y'], _data['z']), axis=1)
_data = PlyData.read('riegl5cm.ply').elements[0].data
right = np.stack((_data['x'], _data['y'], _data['z']), axis=1) 

print("Left points count: {}".format(left.shape[0]))
print("Right points count: {}".format(right.shape[0]))

# example visualization
if visualization:
    animate((left), right)
    
    # The first paramter of the animate function can be a list of points list. 
    # In this animation is generated between the point lits. 
    # mv = np.array([4, 4, 4])
    # animate((left, left - mv, left + mv*0.5), right)

Left points count: 8753
Right points count: 9099

Estimate Correspondences

We assume that closest points (euclidean distance) in left and right data set are the corrensponding one. The usage of a KDTree data structure can improve our implementation. Please read following documentation. Feel free to use an other correspndence estimation strategie. Ignore the automated test in that case.

def sort_by_corrspondence(left, right):
    # This method takes two lists of points and return a subset of right list. With following properties
    # return right sublist with same length as left list and the points left[i] and right_sublist[i] are the closest points.
    # YOUR CODE HERE
    raise NotImplementedError()

def test_cor():
    tpl = np.array([[0,0,1],
                    [0,1,0],
                    [1,0,0]])
    tpr = np.array([[0,0.5,-0.5],
                    [0,0,1.5],
                    [0.1,0,0]])
    sr = sort_by_corrspondence(tpl,tpr)
    assert all(tpr[0] == sr[1]) and all(tpr[1] == sr[0]) and all(tpr[2] == sr[2])
test_cor()

Implement ICP

Implement a function for icp which takes the parameters icp(s_0, t_0, R_0, left, right): and returns s, t, R, the estimated transformation and list of meaned square error for each iteration step.


#Use this area to copy paste your code from earlier exercises
# YOUR CODE HERE
#raise NotImplementedError()
def icp(s, t, R, left, right):
    left_states = [left]
    
    left_trans = s*np.dot(R, left.transpose()).transpose() + t
    left_states.append(left_trans) # if you store the intermidiate states of the left list thex will be animated.
    
    max_iter = 20
    # YOUR CODE HERE
    #raise NotImplementedError()
    if visualization:
        animate(left_states, right)
    
    # dont forget to return the values

Application

  • use the given 3D point clouds “tango5cm.ply” (left) and “riegl5cm.ply” (right, reference) as input data for ICP
  • determine the final transformation parameters
  • plot the mean square errors (MSE) of the ICP iterations
# Apply icp
t=np.array([0.6, 1, -0.2])

# wrong rotation. estimate your own rotation.
R = np.array([[0.2, -0.8, 0],
              [-0.3,  0, 0.7],
              [0,  0.2, 0.8]
             ])
# YOUR CODE HERE
#raise NotImplementedError()


icp(1, t, R, left, right)

This how it looks like if everything works well:

With a bad initial rotation icp is useless:

Author: Artem Leichter
Last modified: 2019-05-02