icp.py

Go to the documentation of this file.

# Code example for ICP taking 2 points clouds (2D or 3D) relatively close
# and computing the transformation between them.
#
# This code is more complete than icp_simple. It can load parameter files and
# has more options.
 
import numpy as np
 
from pypointmatcher import pointmatcher as pm
from utils import parse_translation, parse_rotation
 
PM = pm.PointMatcher
DP = PM.DataPoints
 
# Save transformation matrix in three different files:
#     - BASEFILENAME_inti_transfo.txt
#     - BASEFILENAME_icp_transfo.txt
#     - BASEFILENAME_complete_transfo.txt
#       (default: false)
is_transfo_saved = False
 
# Be more verbose (info logging to the console)
is_verbose = True
 
# Load the config from a YAML file (default: default.yaml)
# Leave empty to set the ICP default configuration
config_file = "../data/default.yaml"
 
# Path of output directory (default: tests/icp/)
# The output directory must already exist
# Leave empty to save in the current directory
output_base_directory = "tests/icp/"
 
# Name of output files (default: test)
output_base_file = "test"
 
# Toggle to switch between 2D and 3D point clouds
is_3D = True
 
# Add an initial 3D translation before applying ICP (default: 0,0,0)
# Add an initial 2D translation before applying ICP (default: 0,0)
init_translation = "0,0,0" if is_3D else "0,0"
# Add an initial 3D rotation before applying ICP (default: 1,0,0;0,1,0;0,0,1)
# Add an initial 2D rotation before applying ICP (default: 1,0;0,1)
init_rotation = "1,0,0;0,1,0;0,0,1" if is_3D else "1,0;0,1"
 
if is_3D:
    # Load 3D point clouds
    ref = DP(DP.load('../data/car_cloud400.csv'))
    data = DP(DP.load('../data/car_cloud401.csv'))
    test_base = "3D"
else:
    # Load 2D point clouds
    ref = DP(DP.load('../data/2D_twoBoxes.csv'))
    data = DP(DP.load('../data/2D_oneBox.csv'))
    test_base = "2D"
 
# Create the default ICP algorithm
icp = PM.ICP()
 
if len(config_file) == 0:
    # See the implementation of setDefault() to create a custom ICP algorithm
    icp.setDefault()
else:
    # load YAML config
    icp.loadFromYaml(config_file)
 
cloud_dimension = ref.getEuclideanDim()
 
assert cloud_dimension == 2 or cloud_dimension == 3, "Invalid input point clouds dimension"
 
# Parse the translation and rotation to be used to compute the initial transformation
translation = parse_translation(init_translation, cloud_dimension)
rotation = parse_rotation(init_rotation, cloud_dimension)
 
init_transfo = np.matmul(translation, rotation)
 
rigid_trans = PM.get().TransformationRegistrar.create("RigidTransformation")
 
if not rigid_trans.checkParameters(init_transfo):
    print("Initial transformations is not rigid, identiy will be used")
    init_transfo = np.identity(cloud_dimension + 1)
 
initialized_data = rigid_trans.compute(data, init_transfo)
 
# Compute the transformation to express data in ref
T = icp(initialized_data, ref)
 
if is_verbose:
    print(f"match ratio: {icp.errorMinimizer.getWeightedPointUsedRatio():.6}")
 
# Transform data to express it in ref
data_out = DP(initialized_data)
icp.transformations.apply(data_out, T)
 
# Save files to see the results
ref.save(f"{output_base_directory + test_base}_{output_base_file}_ref.vtk")
data.save(f"{output_base_directory + test_base}_{output_base_file}_data_in.vtk")
data_out.save(f"{output_base_directory + test_base}_{output_base_file}_data_out.vtk")
 
if is_transfo_saved:
    init_file_name = f"{output_base_directory + test_base}_{output_base_file}_init_transfo.txt"
    icp_file_name = f"{output_base_directory + test_base}_{output_base_file}_icp.transfo.txt"
    complete_file_name = f"{output_base_directory + test_base}_{output_base_file}_complete_transfo.txt"
 
    with open(init_file_name, "w") as f:
        f.write(f"{init_transfo}".replace("[", " ").replace("]", " "))
 
    with open(icp_file_name, "w") as f:
        f.write(f"{T}".replace("[", " ").replace("]", " "))
 
    with open(complete_file_name, "w") as f:
        f.write(f"{np.matmul(T, init_transfo)}".replace("[", " ").replace("]", " "))
 
else:
    if is_verbose:
        print(f"{test_base} ICP transformation:\n{T}".replace("[", " ").replace("]", " "))