-
Notifications
You must be signed in to change notification settings - Fork 3
/
ICPEval.py
130 lines (95 loc) · 4.2 KB
/
ICPEval.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
"""
Function evaluate_registration calculates two main metrics.
fitness measures the overlapping area (# of inlier correspondences / # of points in target). Higher the better.
inlier_rmse measures the RMSE of all inlier correspondences. Lower the better.
optimal: fitness > 0.60 and inlier_rmse < 0.01
same: fitness = 1 inlier_rmse = 0
ref: http://www.open3d.org/docs/tutorial/Basic/icp_registration.html
"""
from open3d import *
import copy
# onshow
def paint(input_path):
source = read_point_cloud(input_path)
print(source)
source_temp = copy.deepcopy(source)
source_temp.paint_uniform_color([1, 0.706, 0])
draw_geometries([source_temp])
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
draw_geometries([source_temp, target_temp])
def metrics_eval(source, target, threshold, trans_init):
# draw_registration_result(source, target, trans_init)
print("Initial alignment")
evaluation = evaluate_registration(source, target,
threshold, trans_init)
print(evaluation)
# can ignore by comments
# print("Transformation is:")
# print(evaluation.transformation)
# print("")
def p2p_icp(source, target, threshold, trans_init):
print("Apply point-to-point ICP")
reg_p2p = registration_icp(source, target, threshold, trans_init,
TransformationEstimationPointToPoint(),
ICPConvergenceCriteria(max_iteration = 30)) # default 30 cant delete this line, and max 2000
print(reg_p2p)
# can ignore by comments
print("Transformation is:")
print(reg_p2p.transformation)
print("")
# draw_registration_result(source, target, reg_p2p.transformation)
def p2l_icp(source, target, threshold, trans_init):
# assume no normalized point
estimate_normals(source, search_param = KDTreeSearchParamHybrid(
radius = 0.1, max_nn = 30)) # default
estimate_normals(target, search_param = KDTreeSearchParamHybrid(
radius = 0.1, max_nn = 30))
# draw_geometries([source]) # test on normals
# need normalized point, notion inplace normailization
print("Apply point-to-plane ICP")
reg_p2l = registration_icp(source, target, threshold, trans_init,
TransformationEstimationPointToPlane(),
ICPConvergenceCriteria(max_iteration = 30)) # default 30 cant delete this line, and max 2000 long long
print(reg_p2l)
# can ignore by comments
print("Transformation is:")
print(reg_p2l.transformation)
print("")
# draw_registration_result(source, target, reg_p2l.transformation)
def icp_eval(source, target, threshold, trans_init):
# metrics_eval(source, target, threshold, trans_init) # not icp but eval ok
p2p_icp(source, target, threshold, trans_init)
p2l_icp(source, target, threshold, trans_init)
# input 2 files
def load_xyz(source_file_path, target_file_path):
source = read_point_cloud(source_file_path)
target = read_point_cloud(target_file_path)
return source, target
def init_para(threshold=0.02, 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]])):
return threshold, trans_init
# local demo
def test():
#### CREDIT: testing point cloud files from the IDETC paper ####
source_path = './TestData/15.xyz'
# target_path = './TestData/15_slight.xyz' # delete few head lines
target_path = './TestData/15_reversed.xyz' # reversed completely
# target_path = './TestData/16.xyz' # unmatched dataset try it out
source, target = load_xyz(source_path, target_path)
threshold, trans_init = init_para()
icp_eval(source, target, threshold, trans_init)
if __name__ == "__main__":
# python3 ICPEval.py
test()
"""
quick helper
from ICPEval import *
# from ICPEval import load_xyz, icp_eval, draw_registration_result, paint, metrics_eval, p2p_icp, p2l_icp, init_para
source, target = load_xyz(source_path, target_path) # in xyz format plz
threshold, trans_init = init_para()
icp_eval(source, target, threshold, trans_init)
"""