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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
| import numpy as np
import open3d
from open3d import registration_ransac_based_on_feature_matching as RANSAC
from open3d import registration_icp as ICP
from open3d import compute_fpfh_feature as FPFH
from open3d import get_information_matrix_from_point_clouds as GET_GTG
def register(pcd1, pcd2, size):
kdt_n = open3d.KDTreeSearchParamHybrid(radius=size, max_nn=50)
kdt_f = open3d.KDTreeSearchParamHybrid(radius=size * 10, max_nn=50)
pcd1_d = open3d.voxel_down_sample(pcd1, size)
pcd2_d = open3d.voxel_down_sample(pcd2, size)
open3d.estimate_normals(pcd1_d, kdt_n)
open3d.estimate_normals(pcd2_d, kdt_n)
pcd1_f = FPFH(pcd1_d, kdt_f)
pcd2_f = FPFH(pcd2_d, kdt_f)
checker = [open3d.CorrespondenceCheckerBasedOnEdgeLength(0.9),
open3d.CorrespondenceCheckerBasedOnDistance(size * 2)]
est_ptp = open3d.TransformationEstimationPointToPoint()
est_ptpln = open3d.TransformationEstimationPointToPlane()
criteria = open3d.RANSACConvergenceCriteria(max_iteration=400000,
max_validation=500)
result1 = RANSAC(pcd1_d, pcd2_d,
pcd1_f, pcd2_f,
max_correspondence_distance=size * 2,
estimation_method=est_ptp,
ransac_n=4,
checkers=checker,
criteria=criteria)
result2 = ICP(pcd1, pcd2, size, result1.transformation, est_ptpln)
return result2.transformation
def merge(pcds):
all_points = []
for pcd in pcds:
all_points.append(np.asarray(pcd.points))
merged_pcd = open3d.PointCloud()
merged_pcd.points = open3d.Vector3dVector(np.vstack(all_points))
return merged_pcd
def add_color_normal(pcd): # in-place coloring and adding normal
pcd.paint_uniform_color(np.random.rand(3))
size = np.abs((pcd.get_max_bound() - pcd.get_min_bound())).max() / 30
kdt_n = open3d.KDTreeSearchParamHybrid(radius=size, max_nn=50)
open3d.estimate_normals(pcd, kdt_n)
def load_pcds(pcd_files):
pcds = []
for f in pcd_files:
pcd = open3d.read_point_cloud(f)
add_color_normal(pcd)
pcds.append(pcd)
return pcds
def align_pcds(pcds, size):
pose_graph = open3d.PoseGraph()
accum_pose = np.identity(4)
pose_graph.nodes.append(open3d.PoseGraphNode(accum_pose))
n_pcds = len(pcds)
for source_id in range(n_pcds):
for target_id in range(source_id + 1, n_pcds):
source = pcds[source_id]
target = pcds[target_id]
trans = register(source, target, size)
GTG_mat = GET_GTG(source, target, size, trans)
if target_id == source_id + 1:
accum_pose = np.matmul(trans, accum_pose)
pose_graph.nodes.append(open3d.PoseGraphNode(np.linalg.inv(accum_pose)))
pose_graph.edges.append(open3d.PoseGraphEdge(source_id,
target_id,
trans,
GTG_mat,
uncertain=True))
solver = open3d.GlobalOptimizationLevenbergMarquardt()
criteria = open3d.GlobalOptimizationConvergenceCriteria()
option = open3d.GlobalOptimizationOption(
max_correspondence_distance=size / 10,
edge_prune_threshold=size / 10,
reference_node=0)
open3d.global_optimization(pose_graph,
method=solver,
criteria=criteria,
option=option)
for pcd_id in range(n_pcds):
trans = pose_graph.nodes[pcd_id].pose
pcds[pcd_id].transform(trans)
return pcds
def main():
pcds = load_pcds(["data/test/bun270.ply",
"data/test/bun315.ply",
"data/test/chin.ply",
"data/test/bun000.ply",
"data/test/bun045.ply",
"data/test/bun090.ply",
"data/test/bun180.ply"])
open3d.draw_geometries(pcds, "input pcds")
size = np.abs((pcds[0].get_max_bound() - pcds[0].get_min_bound())).max() / 30
pcd_aligned = align_pcds(pcds, size)
open3d.draw_geometries(pcd_aligned, "aligned")
pcd_merge = merge(pcd_aligned)
add_color_normal(pcd_merge)
open3d.draw_geometries([pcd_merge], "merged")
if __name__ == '__main__':
main()
|
