-
Notifications
You must be signed in to change notification settings - Fork 1
/
my_get_registration_result.py
60 lines (40 loc) · 2.2 KB
/
my_get_registration_result.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
import open3d
import copy
import numpy as np
from scipy.spatial.transform import Rotation
import constant
def fast_global_registration(source_point_cloud, target_point_cloud, source_point_cloud_fpfh, target_point_cloud_fpfh):
distance_threshold = constant.voxel_size * 0.5*10
global_result = open3d.registration.registration_fast_based_on_feature_matching(
source_point_cloud, target_point_cloud, source_point_cloud_fpfh, target_point_cloud_fpfh,
open3d.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
return global_result
def icp_registration(source_point_cloud, target_point_cloud, global_result):
distance_threshold = constant.voxel_size * 0.4*10
icp_result = open3d.registration.registration_icp(
source_point_cloud, target_point_cloud, distance_threshold, global_result.transformation,
open3d.registration.TransformationEstimationPointToPoint(), open3d.registration.ICPConvergenceCriteria(max_iteration=2000))
return icp_result
def get_registration_result(source_point_cloud, target_point_cloud, source_point_cloud_fpfh, target_point_cloud_fpfh):
global_result = fast_global_registration(source_point_cloud, target_point_cloud,
source_point_cloud_fpfh, target_point_cloud_fpfh)
# print(global_result.fitness)
# print(global_result.inlier_rmse)
icp_result = icp_registration(
source_point_cloud, target_point_cloud, global_result)
# print(icp_result.fitness)
# print(icp_result.inlier_rmse)
# rotation = Rotation.from_dcm(icp_result.transformation[:3, :3])
# print(rotation.as_euler('zyx', degrees=True))
# print(icp_result.transformation[:3, -1])
# draw_registration_result(
# source_point_cloud, target_point_cloud, icp_result.transformation)
return icp_result
def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0, 0])
target_temp.paint_uniform_color([0, 1, 0])
source_temp.transform(transformation)
open3d.visualization.draw_geometries([source_temp, target_temp])