ICP code

from pickle import TRUE
import sys
sys.coinit_flags = 2
import open3d
import pythoncom
import numpy
import copy
import pyvista as pv
import pcl
import tkinter
from tkinter import filedialog
import json
import os

def showPCD(source, target, transformation, reference_frame):
    temp_source = copy.deepcopy(source)
    temp_target = copy.deepcopy(target)
    if reference_frame == 'camera':
        temp_coordinate = open3d.geometry.TriangleMesh.create_coordinate_frame(size=100)
    else:
        temp_coordinate = copy.deepcopy(reference_frame)
    temp_source.transform(transformation)
    open3d.visualization.draw_geometries([temp_source, temp_target, temp_coordinate],'risultati',2160,1440)
    return None

def setFirstTransform(source, target):

    while True:
        # l'utente inserisce i parametri per la trasformazione successiva
        #x = float(input('inserire x'))
        #y = float(input('inserire y'))
        #z = float(input('inserire z'))
        #tetax = float(input('inserire tetax'))
        #tetay = float(input('inserire tetay'))
        #tetaz = float(input('inserire tetaz'))
        #tetax_rad = tetax * (numpy.pi / 180)
        #tetay_rad = tetay * (numpy.pi / 180)
        #tetaz_rad = tetaz * (numpy.pi / 180)
        #teta_array = numpy.array([tetax_rad, tetay_rad, tetaz_rad])

        # calcolo la matrice di rotazione in base agli angoli dati dall'utente (in terna mobile)
        #rot_matrix = open3d.geometry.get_rotation_matrix_from_xyz(teta_array)

        actual_transform = numpy.array([[0.96,0.04,0.00, -167.54],
                            [0.02,-0.55,-0.80,71.67],
                            [-0.03,0.79,-0.55,-520.23],
                            [0, 0, 0, 1]])


        #answer = input('Press n to finish the process')

        if TRUE: #answer == 'n':
            print(actual_transform)
            return actual_transform
            break
#1
root = tkinter.Tk()
file_path = filedialog.askopenfilename()
root.withdraw()
root.destroy()
stl_source = open3d.io.read_triangle_mesh(file_path)
source = open3d.geometry.PointCloud()
source.points = stl_source.vertices
source.paint_uniform_color([0.945, 0.547, 0.156])
#2
root = tkinter.Tk()
file_path2 = filedialog.askopenfilename()
root.withdraw()
root.destroy()
#pyvista
point_cloud = pv.read(file_path2)
point_cloud.plot()
plotter = pv.Plotter()
plotter.add_points(point_cloud)
points = numpy.array(point_cloud.points)
o3d_point_cloud = open3d.geometry.PointCloud()
o3d_point_cloud.points = open3d.utility.Vector3dVector(points)
o3d_point_cloud.scale(scale=1000, center=[0, 0, 0])
open3d.visualization.draw_geometries([o3d_point_cloud])

showPCD(source, o3d_point_cloud, numpy.identity(4), 'camera')

#FIRST TRANSFORM

first_transform = setFirstTransform(source, o3d_point_cloud)

o3d_point_cloud.estimate_normals(open3d.geometry.KDTreeSearchParamHybrid(radius=10, max_nn=10))
actual_result = open3d.pipelines.registration.registration_icp(source, o3d_point_cloud, 0.1, first_transform,
                                                                       open3d.pipelines.registration.TransformationEstimationPointToPoint(),
                                                                       open3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-10,
                                                                                                                            relative_rmse=1e-10,
                                                                                                                            max_iteration=500))
reference_frame = open3d.geometry.TriangleMesh.create_coordinate_frame(size=100)
matrix_result = actual_result.transformation
print(actual_result)
showPCD(source, o3d_point_cloud, matrix_result, 'camera')