-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathar_image.py
178 lines (121 loc) · 7.11 KB
/
ar_image.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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
import logging
import numpy as np
import cv2
from functions import icp as cp
from functions import project_and_display as proj
from functions import matrix_operations as tf
from Python_3D_Toolbox_for_Realsense import acquisition_realsense as aq
from Python_3D_Toolbox_for_Realsense import info_realsense as ir
from Python_3D_Toolbox_for_Realsense.functions.utils import array as array
from Python_3D_Toolbox_for_Realsense.functions import processing_ply as ply
from Python_3D_Toolbox_for_Realsense.functions import processing_point_cloud as pc
from Python_3D_Toolbox_for_Realsense.functions import processing_pixel_list as pixels
from Python_3D_Toolbox_for_Realsense.functions import processing_img as img
from Python_3D_Toolbox_for_Realsense.functions import previsualisation_application_function as Tk
############# Mode selection ###############
loading_ply_file = True
name_pc_file = "example/input/point_cloud_test_stomach.ply" # Could be umpty if loading_ply_file == False
name_model_3D = "example/input/stomach_3D_rainbow_colored.ply"
name_for_output = "example/output/test_stomach"
size_acqui = (1280,720)
# Define calibration matrix of the camera used for creating the file
M_in = np.asarray([[640.05206, 0, 639.1219, 0], [0, 640.05206, 361.61005, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) # Could be null if loading_mply_file == False
############### Loading ####################
# Load the 3D model
points_model_3D, colors_model_3D = ply.get_points_and_colors(name_model_3D)
################### Acquisition ###########################
if loading_ply_file:
# Load an existing .ply file
points, colors = ply.get_points_and_colors(name_pc_file)
else:
# Get the calibration matrix (will be helpfull later)
calibration_matrix = ir.get_matrix_calib(size_acqui[0],size_acqui[1])
M_in = np.hstack((calibration_matrix, np.zeros((3, 1))))
M_in = np.vstack((M_in, np.array([0, 0, 0, 1])))
# Get point cloud with the realsense camera
pipeline = aq.init_realsense(size_acqui[0],size_acqui[1])
points, colors = aq.get_points_and_colors_from_realsense(pipeline) # Capture the point cloud
tab_index = np.array([i for i in range(len(points))])
##################### Select Zone ####################
points_crop, colors_crop, tab_index_crop, new_shape = pc.crop_from_zone_selection(
points=points, colors=colors, shape=size_acqui, tab_index=tab_index)
###################### Mask ###########################
# Get the mask
mask_hsv = pixels.get_hsv_mask_with_sliders(colors_crop, new_shape)
# Apply hsv mask
points_filtered_hsv, colors_filtered_hsv, tab_index_hsv = pc.apply_hsv_mask(
points_crop, colors_crop, mask_hsv, new_shape, tab_index_crop)
####################### Remove noisy values #####################
radius = Tk.get_parameter_using_preview(
points_filtered_hsv, pc.filter_with_sphere_on_barycentre, "Radius")
points_filtered_noise, colors_filtered_noise, tab_index_filtered_noise = pc.filter_with_sphere_on_barycentre(
points_filtered_hsv, radius, colors_filtered_hsv, tab_index_hsv)
######################### Resizing of 3D model ##################################
pc_too_big = True
if (len(points_filtered_noise)>2000):
points_for_resize_only, _ = pc.reduce_density(points_filtered_noise,2000/len(points_filtered_noise))
# The resize algorithm can crash if we have too many points.
# We check if it is the case (if yes we reduced by half the number of points in the point cloud)
while pc_too_big:
try:
points_model_3D_resized = pc.resize_point_cloud_to_another_one(
points_model_3D, points_for_resize_only)
pc_too_big = False
except Exception as e:
logging.info(
f"Too many points in the point cloud {name_model_3D} : we reduce the number of points by half")
points_model_3D, colors_model_3D = pc.reduce_density(
points_model_3D, 0.5, colors_model_3D)
################## Repose objects ####################
points_reposed = pc.centers_points_on_geometry(points_filtered_noise)
translation_vector = pc.get_center_geometry(points_filtered_noise)
Mt = tf.translation_matrix(translation_vector)
################ Pre-rotation matrix ###################
M_pre_rot, _ = cp.find_the_best_pre_rotation_to_align_points(points_model_3D_resized, points_reposed, [
0, 0, 10], [0, 0, 10], [-180, 180, 10])
M_pre_rot = np.hstack((M_pre_rot, np.array([[0], [0], [0]])))
M_pre_rot = np.vstack((M_pre_rot, np.array([0, 0, 0, 1])))
M_pre_rot_inv = np.linalg.inv(M_pre_rot)
###################### ICP Matrix #########################
model_3D_points_after_pre_rotation = np.array([(float(x), float(y), float(z)) for (
x, y, z, t) in [M_pre_rot_inv @ p for p in np.column_stack((points_model_3D_resized, np.ones(len(
points_model_3D_resized))))]], dtype=np.float64)
M_icp, _ = cp.find_transform_matrix_to_align_points_using_icp(
model_3D_points_after_pre_rotation, points_reposed)
angles_ICP = tf.transformation_matrix_to_euler_xyz(M_icp)
x = -angles_ICP[0]
y = angles_ICP[1]
z = -angles_ICP[2]
M_icp_inv = np.linalg.inv(tf.matrix_from_angles(x, y, z))
############## Projection using closest points identification ##############
M = Mt @ M_pre_rot_inv @ M_icp_inv
colors_projection_cpi = proj.project_3D_model_on_pc_using_closest_points_identification(
points_model_3D_resized, colors_model_3D, points, colors, M,size_acqui)
img.save(colors_projection_cpi, name_for_output +
"_projection_closest_points.png", size_acqui)
ply.save(name_for_output+"_projection_closest_points.ply", points, colors_projection_cpi)
################# Projection using indices identification #################
colors_projection_ii = proj.project_3D_model_on_pc_using_closest_points_and_indices(
points_model_3D_resized, colors_model_3D, points_filtered_noise, colors, tab_index_filtered_noise, M,size_acqui)
img.save(colors_projection_ii, name_for_output +
"_projection_using_indices.png", size_acqui)
ply.save(name_for_output+"_projection_using_indices.ply", points, colors_projection_ii)
################# Projection using Calibration Matrix #################
# Calibration matrix
projection = M_in @ Mt @ M_pre_rot_inv @ M_icp_inv
if len(colors_model_3D) == 0:
colors_model_3D = np.asarray(
[[0., 0., 255.] for i in range(len(np.asarray(points_model_3D)))])
color_image_proj = proj.project_3D_model_on_pc(colors, points_model_3D_resized, colors_model_3D, projection,size_acqui)
img.save(color_image_proj[:, :, ::-1], name_for_output +"_projection.png")
ply.save(name_for_output + "_projection.ply",points,color_image_proj[:, :, ::-1])
################# Displays #################
while True:
cv2.imshow("Projection using closest points identification",
array.line_to_2Darray(colors_projection_cpi,(size_acqui[1],size_acqui[0])).astype(np.uint8)[:, :, ::-1])
cv2.imshow("projection using indices identification",
array.line_to_2Darray(colors_projection_ii,(size_acqui[1],size_acqui[0])).astype(np.uint8)[:, :, ::-1])
cv2.imshow("projection of 3D model on pc", color_image_proj)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()