import open3d as o3d

import numpy as np

import copy

import time

SOURCE_PCD = "bunny/data/bun000.ply"

TARGET_PCD = "bunny/data/bun045.ply"

def draw_registration_result(source, target, transformation):

o3d.visualization.draw_geometries([source_temp, target_temp])

demo_icp_pcds = o3d.data.DemoICPPointClouds()

source = o3d.io.read_point_cloud(SOURCE_PCD)

target = o3d.io.read_point_cloud(TARGET_PCD)

threshold = 0.004

trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],

[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])

draw_registration_result(source, target, trans_init)

start = time.time()

print("Initial alignment")

evaluation = o3d.pipelines.registration.evaluate_registration(

source, target, threshold, trans_init)

print(evaluation)

print("Apply point-to-point ICP")

reg_p2p = o3d.pipelines.registration.registration_icp(

source, target, threshold, trans_init,

o3d.pipelines.registration.TransformationEstimationPointToPoint())

print(reg_p2p)

print("Transformation is:")

print(reg_p2p.transformation)

reg_p2p = o3d.pipelines.registration.registration_icp(

source, target, threshold, trans_init,

o3d.pipelines.registration.TransformationEstimationPointToPoint(),

o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000))

print("ICP registration took %.3f sec.\n" % (time.time() - start))

print(reg_p2p)

print("Transformation is:")

print(reg_p2p.transformation)

draw_registration_result(source, target, reg_p2p.transformation)