def modified_ICP(source_points, buccal, front, lingual, occlusal):
probing_points_transformed = source_points
buccal_selected_points = select_closest_point(
probing_points_transformed[0:3, :], buccal)
front_selected_points = select_closest_point(
probing_points_transformed[3:6, :], front)
lingual_selected_points = select_closest_point(
probing_points_transformed[9:12, :], lingual)
occlusal_selected_points = select_closest_point(
probing_points_transformed[6:9, :], occlusal)
destination_points = np.ones(3)
destination_points = np.vstack(
(destination_points, buccal_selected_points))
destination_points = np.vstack((destination_points, front_selected_points))
destination_points = np.vstack(
(destination_points, lingual_selected_points))
destination_points = np.vstack(
(destination_points, occlusal_selected_points))
destination_points = np.asarray(destination_points)[1:, :]
#print('ios_points_transformed_2 check are', source_points)
r_, t_ = registration.point_set_registration(source_points,
destination_points)
#print('r is', r_)
#print('t is', t_)
ios_points_transformed_2 = np.ones(3)
for point in source_points:
#print('point is', point)
#print('t_ is', t_)
point_tem_2 = np.matmul(r_, point) + t_
#print('point_tem_2 is', point_tem_2)
ios_points_transformed_2 = np.vstack(
(ios_points_transformed_2, point_tem_2))
ios_points_transformed_2 = ios_points_transformed_2[1:, :]
#fig = plt.figure()
#ax = fig.add_subplot(111, projection='3d')
#Yomiplot.plot_3d_points(ios_points_transformed_2,ax,color='red')
#Yomiplot.plot_3d_points(destination_points, ax, color='green')
#Yomiplot.plot_3d_points(source_points, ax, color='blue')
#Yomiplot.plot_3d_points(occlusal,ax,color='red', alpha=0.2)
#plt.show()
#print('ios_points_transformed_2 are', ios_points_transformed_2)
error = np.linalg.norm(ios_points_transformed_2 - destination_points,
axis=1)
#print('error is', error)
error = np.sqrt(np.sum(error**2) / len(error))
print('error rmse is', error)
return error, ios_points_transformed_2, destination_points
def modified_ICP(Probe_surfaces_list, CT_surfaces_list):
source_points_list = Probe_surfaces_list
destination_points_list = []
for i in range(len(Probe_surfaces_list)):
closet_points = select_closest_point(Probe_surfaces_list[i],
CT_surfaces_list[i])
destination_points_list.append(closet_points)
source_points_array = ap.combine_elements_in_list(source_points_list)
destination_points_array = ap.combine_elements_in_list(
destination_points_list)
r_, t_ = registration.point_set_registration(source_points_array,
destination_points_array)
source_points_transformed_list = []
for list_element in source_points_list:
array_tem = []
for point in list_element:
array_tem.append(np.matmul(r_, point) + t_)
source_points_transformed_list.append(array_tem)
source_points_transformed_array = ap.combine_elements_in_list(
source_points_transformed_list)
#fig = plt.figure()
#ax = fig.add_subplot(111, projection='3d')
#Yomiplot.plot_3d_points(ios_points_transformed_2,ax,color='red')
#Yomiplot.plot_3d_points(destination_points, ax, color='green')
#Yomiplot.plot_3d_points(source_points, ax, color='blue')
#Yomiplot.plot_3d_points(occlusal,ax,color='red', alpha=0.2)
#plt.show()
error = np.linalg.norm(source_points_transformed_array -
destination_points_array,
axis=1)
error = np.sqrt(np.sum(error**2) / len(error))
print('error rmse is', error)
return error, source_points_transformed_list, destination_points_list
def check_tre(targets, fiducials):
# define number of trials, mean error and stdev
tre_total = []
tre_1 = []
tre_2 = []
tre_3 = []
tre_4 = []
original_fiducials = np.copy(fiducials)
#print('original_fiducials are', original_fiducials)
for i in range(N): # for each trial
# generate noise on fiducials
fiducials_new = []
# add biased errors
#print('loop', i)
if BIAS_ERROR_FLAG == 1:
center = np.array([-0.154, 0, 0])
if ORIENTATION == 1:
#print('fiducials before biased errors are', original_fiducials)
fiducials = random_points_3d.biased_3d_points_1(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
#print('fiducials after biased errors are', fiducials)
if ORIENTATION == 2:
fiducials = random_points_3d.biased_3d_points_2(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
if ORIENTATION == 3:
fiducials = random_points_3d.biased_3d_points_3(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
for fiducial in fiducials:
# select element 0 from the random values
# add random errors
# the function random_3d_points only return a matrix, to select a vector, should specify the row number
fiducial_tem = fiducial + random_points_3d.random_3d_points(
1, RAN_MEAN, RAN_STDEV)[0]
fiducials_new.append(fiducial_tem)
# perform registration with perturbed fiducials
#print('fiducials_after random errors are', fiducials_new)
#print('fiducials original check are', original_fiducials)
#print('fiducial error vector is', np.linalg.norm(fiducials_new - original_fiducials, axis=1))
R, t = registration.point_set_registration(fiducials_new,
original_fiducials)
#print('R is', R)
#print('t is', np.linalg.norm(t))
# perform registration on targets
targets_new = []
for target in targets:
target_tem = np.matmul(R, target) + t
targets_new.append(target_tem)
tre = np.asarray(targets_new) - targets
#print('tre vector is', tre)
tre = np.linalg.norm(tre, axis=1)
#print('tre is', tre)
tre_total = np.append(tre_total, tre)
tre_1.append(tre[0])
tre_2.append(tre[1])
tre_3.append(tre[2])
tre_4.append(tre[3])
tre_sep = [tre_1, tre_2, tre_3, tre_4]
value_95 = []
for j in range(len(targets)):
value = sa.fit_models_np_plot(tre_sep[j])
value_95.append(value)
return value_95
tre_1 = []
tre_2 = []
tre_3 = []
tre_4 = []
for i in range(n): # for each trial
# generate noise on fiducials
fiducials_new = []
for fiducial in fiducials_original:
# select element 0 from the random values
# the function random_3d_points only return a matrix, to select a vector, should specify the row number
fiducial_tem = fiducial + random_points_3d.random_3d_points(
1, mean, stdev)[0]
fiducials_new.append(fiducial_tem)
# perform registration with perturbed fiducials
R, t = registration.point_set_registration(fiducials_new,
fiducials_original)
# perform registration on targets
targets_new = []
for target in targets_original:
target_tem = np.matmul(R, target) + t
targets_new.append(target_tem)
tre = np.asarray(targets_new) - targets_original
tre = np.linalg.norm(tre, axis=1)
tre_total = np.append(tre_total, tre)
tre_1.append(tre[0])
tre_2.append(tre[1])
tre_3.append(tre[2])
tre_4.append(tre[3])
ios_points = Yomiread.read_csv(CT_FILE_BASE + "points_raw.txt",
4,
-1,
delimiter=' ')[:, 1:]
buccal_point_center = np.sum(ios_points[0:3, :], axis=0) / 3
front_point_center = np.sum(ios_points[3:6, :], axis=0) / 3
occlusal_point_center = np.sum(ios_points[6:9, :], axis=0) / 3
lingual_point_center = np.sum(ios_points[9:12, :], axis=0) / 3
point_centers = ap.combine_elements_in_list([
buccal_point_center, front_point_center, occlusal_point_center,
lingual_point_center
])
print(ios_points)
# Initial alignment
R, t = registration.point_set_registration(point_centers, surface_centroids)
ios_points_transformed = []
for point in ios_points:
ios_points_transformed.append(np.matmul(R, point) + t)
ios_points_transformed = np.asarray(ios_points_transformed)
fig2 = plt.figure()
ax = fig2.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(buccal, ax, color='green')
Yomiplot.plot_3d_points(ios_points_transformed, ax, color='red')
plt.show()
# Modified ICP
# prepare destination points
probing_points_transformed = ios_points_transformed
buccal_selected_points = select_closest_point(
probing_points_transformed[0:3, :], buccal)
marker_ga_selected = []
marker_ct_selected = []
for idx in selected_marker_number:
marker_ga_selected.append(marker_ga[idx, :])
marker_ct_selected.append(marker_ct[idx, :])
marker_ga_selected = np.asarray(marker_ga_selected)
marker_ct_selected = np.asarray(marker_ct_selected)
print('marker_ga are', marker_ga_selected)
print('marker_ct are', marker_ct_selected)
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(marker_ct_selected, ax, color='g')
Yomiplot.plot_3d_points(marker_ga_selected, ax, color='blue')
plt.show()
# Point-pair registration (ct = R * ga + t)
R, t = registration.point_set_registration(marker_ct_selected,
marker_ga_selected)
print('R is', R)
print('t is', t)
# Generate fiducial array in image space
fiducial_array_fs = Yomiread.read_csv_specific_rows(FIDUCIAL_ARRAY_FS_FILE,
4, [3, -1],
delimiter=' ')[:, 1:]
fiducial_array_ct = []
for point in fiducial_array_fs:
#fiducial_array_ct.append(np.matmul(R, point) + t)
fiducial_array_ct.append(np.matmul(np.linalg.inv(R), (point - t)))
fiducial_array_ct = np.asarray(fiducial_array_ct)
Yomiwrite.write_csv_matrix(FIDUCIAL_ARRAY_CT_FILE,
fiducial_array_ct,
fmt='%.6f',
def check_tre(targets, fiducials):
# define number of trials, mean error and stdev
tre_total = []
tre_ang_total = []
tre_trans_total = []
x_error = []
y_error = []
z_error = []
original_fiducials = np.copy(fiducials)
for i in range(N): # for each trial
# generate noise on fiducials
fiducials_new = []
# add biased errors
if BIAS_ERROR_FLAG == 1:
center = np.array([-0.154, 0, 0])
if ORIENTATION == 1:
fiducials = random_points_3d.biased_3d_points_1(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
if ORIENTATION == 2:
fiducials = random_points_3d.biased_3d_points_2(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
if ORIENTATION == 3:
fiducials = random_points_3d.biased_3d_points_3(
original_fiducials, center, BIAS_MEAN, BIAS_STDEV)
for fiducial in fiducials:
# select element 0 from the random values
# add random errors
# the function random_3d_points only return a matrix, to select a vector, should specify the row number
fiducial_tem = fiducial + random_points_3d.random_3d_points(
1, RAN_MEAN, RAN_STDEV)[0]
fiducials_new.append(fiducial_tem)
# perform registration with perturbed fiducials
R, t = registration.point_set_registration(fiducials_new,
original_fiducials)
# perform registration on targets
targets_new = []
if np.ndim(targets) == 1:
targets_new = np.matmul(R, targets) + t
tre_tem = np.asarray(targets_new) - targets
angle_err = np.matmul(R, [0, 0, 1])
print('angle_err is', angle_err)
# print('tre vector is', tre)
tre = np.linalg.norm(tre_tem)
diff_z = tre_tem[2]
diff_x = tre_tem[0]
diff_y = tre_tem[1]
# print('tre is', tre)
tre_total.append(tre)
x_error.append(diff_x)
y_error.append(diff_y)
z_error.append(diff_z)
else:
tre_sep = []
tre_ang_sep = []
tre_trans_sep = []
x_error_sep = []
y_error_sep = []
z_error_sep = []
for i in range(len(targets)):
target = targets[i, :]
target_tem = np.matmul(R, target) + t
angle_err = compare.comp_rot_misori(R, np.eye(3))
trans_err = np.linalg.norm(t)
tre_tem = target_tem - targets[i, :]
tre = np.linalg.norm(tre_tem)
diff_z = tre_tem[2]
diff_x = tre_tem[0]
diff_y = tre_tem[1]
x_error_sep.append(diff_x)
y_error_sep.append(diff_y)
z_error_sep.append(diff_z)
tre_sep.append(tre)
tre_ang_sep.append(angle_err)
tre_trans_sep.append(trans_err)
tre_total.append(tre_sep)
tre_ang_total.append(tre_ang_sep)
tre_trans_total.append(tre_trans_sep)
x_error.append(x_error_sep)
y_error.append(y_error_sep)
z_error.append(z_error_sep)
tre_total = np.asarray(tre_total)
tre_ang_total = np.asarray(tre_ang_total)
tre_trans_total = np.asarray(tre_trans_total)
x_error = np.array(x_error)
y_error = np.array(y_error)
z_error = np.array(z_error)
if np.ndim(targets) == 1:
value_95, mean, std = sa.fit_models_np_plot_mean_std(tre_total)
print('value 95 is', value_95)
print('mean is', mean)
print('std is', std)
else:
value_95 = []
value_95_ang = []
value_95_trans = []
value_95_x = []
value_95_y = []
value_95_z = []
for j in range(len(targets)):
value = sa.fit_models_np_plot(tre_total[:, j])
value_95.append(value)
value_ang = sa.fit_models_np_plot(tre_ang_total[:, j])
value_95_ang.append(value_ang)
value_trans = sa.fit_models_np_plot(tre_trans_total[:, j])
value_95_trans.append(value_trans)
value_x = sa.fit_models_np_plot_mean_std(x_error[:, j])
value_95_x.append(value_x[0])
value_y = sa.fit_models_np_plot_mean_std(y_error[:, j])
value_95_y.append(value_y[0])
value_z = sa.fit_models_np_plot_mean_std(z_error[:, j])
value_95_z.append(value_z[0])
print('value 95 is', value_95)
print('value 95 angle is', value_95_ang)
print('value 95 trans is', value_95_trans)
print('value 95 x is', value_95_x)
print('value 95 y is', value_95_y)
print('value 95 z is', value_95_z)
return value_95, value_95_x, value_95_y, value_95_z
centers_Probe.append(teeth_Probe[i -
1].surface_centers[surface_idx])
surfaces_Probe.append(teeth_Probe[i - 1].points[surface_idx])
centers_CT = ap.combine_elements_in_list(centers_CT)
#surfaces_CT = ap.combine_elements_in_list(surfaces_CT)
centers_Probe = ap.combine_elements_in_list(centers_Probe)
original_points_Probe = ap.combine_elements_in_list(surfaces_Probe)
print('shape of centers CT is', np.shape(centers_CT))
print('shape of centers Probe is', np.shape(centers_Probe))
print('shape of surfaces CT is', np.shape(surfaces_CT))
print('shape of surfaces Probe is', np.shape(surfaces_Probe))
# Perform initial alignment
R, t = registration.point_set_registration(centers_Probe, centers_CT)
surfaces_Probe_transformed = []
for surface in surfaces_Probe:
points_tem = []
for point in surface:
points_tem.append(np.matmul(R, point) + t)
points_tem = np.asarray(points_tem)
surfaces_Probe_transformed.append(points_tem)
del points_tem
# Perform registration
source_points = surfaces_Probe_transformed
for k in range(50):
error, source_points_new, destination_points_new = modified_ICP(
source_points, surfaces_CT)
del source_points
