Accuracy Measurement about probreg HOT 6 CLOSED

from scipy.spatial import KDTree
target_tree = KDTree(target, leafsize=10)
rmse = math_utils.compute_rmse(source, target_tree)

import copy
import numpy as np
import open3d as o3d
import transforms3d as t3d
from numpy.linalg import norm
from probreg import cpd, filterreg

# load source and target point cloud
source = o3d.io.read_point_cloud("./C.ply")
target = o3d.io.read_point_cloud("./E_cut.ply")
# transform target point cloud
th = np.deg2rad(30.0)
target.transform(np.array([[np.cos(th), -np.sin(th), 0.0, 0.0],
                           [np.sin(th), np.cos(th), 0.0, 0.0],
                           [0.0, 0.0, 1.0, 0.0],
                           [0.0, 0.0, 0.0, 1.0]]))
source = source.voxel_down_sample(14)
target = target.voxel_down_sample(7)

threshold = 0.02
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])





import time
# compute cpd registration
start_time = time.time()
tf_param, _, _ = cpd.registration_cpd(source, target)
result = copy.deepcopy(source)
result.points = tf_param.transform(result.points)
print("--- %s seconds ---" % (time.time() - start_time))



# draw result
source.paint_uniform_color([1, 0, 0])
target.paint_uniform_color([0, 1, 0])
result.paint_uniform_color([0, 0, 1])
#o3d.visualization.draw_geometries( [result,target])




#resultglobaltran = tf_param
#gfit=o3d.pipelines.registration.evaluate_registration(source,target,threshold,tf_param.rot)

#fitness=gfit.fitness
#rmse=gfit.inlier_rmse
#cdots=len(gfit.correspondence_set)

print("result: ", np.rad2deg(t3d.euler.mat2euler(tf_param.rot)),
      tf_param.scale, tf_param.t)

from scipy.spatial import KDTree
target_tree = KDTree(target, leafsize=10)
rmse = math_utils.compute_rmse(source, target_tree)

print("Error (m): ")
print(norm(np.rad2deg(t3d.euler.mat2euler(tf_param.rot) - tf_param.t)))