English

# ICP algorithm with python, vtk – match two point clouds or surfaces – I’m not good at matching two surface data in the software. “ICP algorithm” makes that process much easier.

ICP(Iterative Closest Point)
is an algorithm to match two point clouds.

## １．Intuition of the ICP algorithm

ICP algorithm moves one point clouds to another step by step.
This algorithm is also used to match two surface data those contain point clouds.

Example of the ICP algorithm for the surface data of two dogs. The matching process is performed by step-by-step. One step is called as “iteration”.
This example is done in 15 steps. The number of the steps is set
like

• Setting a threshold for average surface distance
• Setting maximum iteration (e.g. 20)

A reference of ICP algorithm ーBesl PJ, McKay ND. A method for registration of 3-D shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence. 1992;14:239-256.

## ２．Problem of the “Local Minimums”

ICP can be used for only when

one surface is close to another.

This problem is common for optimization algorithms.
The process is stopped(converged) in a local minimum point.

#### “What is local minimum point?” Let’s think about an example algorithm like

“Release a ball on the slope. And the end point is defined as the minimum value.”

When the start point is close to the minimum value, the end point is the same as the true minimum value.  But when the start is far from the minimum value,
the ball will stop at
“Local Minimum”
which takes minimum value in a partial range.  Let’s go back to the example of the two dogs. before ICP algorithm : start from a distant point after ICP : two surfaces are overlapped, but not matched well.

To avoid this process, the two surfaces should be set close before the ICP algorithm. Matching one or three reference points can avoid this “local minimum problem.”

=> Rough matching using three reference points

## ３．Script for ICP algorithm

Write a script using Python.

Goal : Get rotation matrix and translation vector used to match two surfaces in ICP algorithm beforehand, vtk(the Visualization Toolkit) library is installed.

pip install vtk

Vtk is an useful library. But it is difficult to fully understand the structure.
This script is written for simple usage for surface matching.

icp_python.py

ICP algorithm script using python, vtk
required : numpy, numpy-stl, vtk
reference：example of vtk-python in kitware homepage


import numpy as np
import vtk

from stl import mesh

def main(source_stl, target_stl, max_iteration=20):
"""
input
source_stl:source surface data(.stl), target_stl:target surface data (.stl)
max_iteration : the max number of steps.
output
result_matrix (4x4 matrix for transformation）
the postICP- source file will be saved as -----postICP.stl
"""
# stl data are changed into vtk.polyData for vtk (stl2vtkpoints is defined below)
source_vtk_data = stl2vtkpoints(source_stl)
target_vtk_data = stl2vtkpoints(target_stl)

# ============ run ICP ==============
icp = vtk.vtkIterativeClosestPointTransform()
#set input and output data
icp.SetSource(source_vtk_data)
icp.SetTarget(target_vtk_data)
# other setting commands
icp.GetLandmarkTransform().SetModeToRigidBody()
icp.DebugOn()

icp.SetMaximumNumberOfIterations(max_iteration)
icp.StartByMatchingCentroidsOff()
icp.Modified()
icp.Update()

#output 4x4 matrix from the icp result
icpTransformFilter = vtk.vtkTransformPolyDataFilter()
transform_matrix = icpTransformFilter.GetTransform().GetMatrix()
result_matrix = np.identity(4)
for i in range(4):
for j in range(4):
result_matrix[i][j] = transform_matrix.GetElement(i, j)

#transform the source surface using numpy-stl
source_mesh = mesh.Mesh.from_file(source_stl)
source_mesh.transform(result_matrix)
source_mesh.save(source_stl.replace('.stl','_postICP.stl'))
#saved as ---postICP.stl

return result_matrix
#return transformation matrix

def stl2vtkpoints(stl_file):
# change stl data into vtk polydata
stl_data = mesh.Mesh.from_file(stl_file)
stl_points =stl_data.points.reshape([-1, 3])

vtk_data = vtk.vtkPolyData()
vtk_data_points = vtk.vtkPoints()
vtk_data_vertices = vtk.vtkCellArray()
for point in stl_points:
id = vtk_data_points.InsertNextPoint(point)
vtk_data_vertices.InsertNextCell(1)
vtk_data_vertices.InsertCellPoint(id)
vtk_data.SetPoints(vtk_data_points)
vtk_data.SetVerts(vtk_data_vertices)

return vtk_data



Let’s apply this script for two dogs

dog1.stl, dog2.stl import icp_python
from stl import mesh

matrix = icp_python.main('dog1.stl', 'dog2.stl')
print(matrix)
#[[ 0.9730126  -0.11119069 -0.20219573 -2.19660497]
# [ 0.09364225  0.99112002 -0.09440468 -1.35194807]
# [ 0.21089716  0.07292288  0.97478441  0.53585268]
# [ 0.          0.          0.          1.        ]]



dog1_postICP.stl, dog2.stl  vtk actually contains the methods to translate and save stl data.
But I use numpy-stl because it’s intuitive and easier.