icp_advance_api.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 math import sqrt
from pypointmatcher import pointmatcher as pm, pointmatchersupport as pms
from utils import parse_translation, parse_rotation
 
PM = pm.PointMatcher
DP = PM.DataPoints
Parameters = pms.Parametrizable.Parameters
 
# 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)
config_file = "../data/default.yaml"
 
# Path of output directory (default: tests/icp_advance_api/)
# The output directory must already exist
# Leave empty to save in the current directory
output_base_directory = "tests/icp_advance_api/"
 
# Name of output files (default: test)
output_base_file = "test"
 
# Toggle to switch between 2D and 3D 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:
    # 3D point clouds
    ref = DP.load('../data/car_cloud400.csv')
    data = DP.load('../data/car_cloud401.csv')
    test_base = "3D"
else:
    # 2D point clouds
    ref = DP.load('../data/2D_twoBoxes.csv')
    data = 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)
match_ratio = icp.errorMinimizer.getWeightedPointUsedRatio()
print(f"match ratio: {match_ratio:.6}")
 
# Transform data to express it in ref
data_out = DP(initialized_data)
icp.transformations.apply(data_out, T)
 
print("\n------------------")
 
#  START demo 1
#  Test for retrieving Haussdorff distance (with outliers). We generate new matching module
#  specifically for this purpose.
#
#  INPUTS:
#  ref: point cloud used as reference
#  data_out: aligned point cloud (using the transformation outputted by icp)
#  icp: icp object used to aligned the point clouds
 
params = Parameters()
 
params["knn"] = "1"  # for Hausdorff distance, we only need the first closest point
params["epsilon"] = "0"
matcher_Hausdorff = PM.get().MatcherRegistrar.create("KDTreeMatcher", params)
 
# max. distance from reading to reference
matcher_Hausdorff.init(ref)
matches = matcher_Hausdorff.findClosests(data_out)
max_dist1 = matches.getDistsQuantile(1.0)
max_dist_robust1 = matches.getDistsQuantile(0.85)
 
# max. distance from reference to reading
matcher_Hausdorff.init(data_out)
matches = matcher_Hausdorff.findClosests(ref)
max_dist2 = matches.getDistsQuantile(1.0)
max_dist_robust2 = matches.getDistsQuantile(0.85)
 
haussdorff_dist = max(max_dist1, max_dist2)
haussdorff_quantile_dist = max(max_dist_robust1, max_dist_robust2)
 
print(f"Haussdorff distance: {sqrt(haussdorff_dist):.6} m")
print(f"Haussdorff quantile distance: {sqrt(haussdorff_quantile_dist):.6} m")
 
#  START demo 2
#  Test for retrieving paired point mean distance without outliers.
#  We reuse the same module used for the icp object.
#
#  INPUTS:
#  ref: point cloud used as reference
#  data_out: aligned point cloud (using the transformation outputted by icp)
#  icp: icp object used to aligned the point clouds
 
# initiate the matching with unfiltered point cloud
icp.matcher.init(ref)
 
# extract closest points
matches = icp.matcher.findClosests(data_out)
 
# weight paired points
outlier_weights = icp.outlierFilters.compute(data_out, ref, matches)
 
# generate tuples of matched points and remove pairs with zero weight
matched_points = PM.ErrorMinimizer.ErrorElements(data_out, ref, outlier_weights, matches)
 
# extract relevant information for convenience
dim = matched_points.reading.getEuclideanDim()
nb_matched_points = matched_points.reading.getNbPoints()
matched_read = matched_points.reading.features[:dim]
matched_ref = matched_points.reference.features[:dim]
 
# compute mean distance
dist = np.linalg.norm(matched_read - matched_ref, axis=0)
mean_dist = dist.sum() / nb_matched_points
print(f"Robust mean distance: {mean_dist:.6} m")
 
# End demo
 
print("------------------\n")
 
# 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("]", " "))