def show_state_distribution(self, model_file, env_file):
self.robot.setupViewer(model_file, env_file)
M = 30.0 * np.identity(2 * self.robot_dof)
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
self.robot_dof = len(active_joints)
#x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
states = []
for z in xrange(2000):
u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
#ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
control_error[:] = ce
x = None
for k in xrange(1):
result = v_double()
self.robot.propagate(current_state, control, control_error,
self.simulation_step_size, self.delta_t,
result)
x = [result[i] for i in xrange(len(result))]
current_state[:] = x
print x
states.append(np.array(x[3:6]))
x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
self.robot.updateViewerValues(cjvals, cjvels,
particle_joint_values,
particle_joint_values)
from plot import plot_3d_points
mins = []
maxs = []
x_min = min([states[i][0] for i in xrange(len(states))])
x_max = max([states[i][0] for i in xrange(len(states))])
y_min = min([states[i][1] for i in xrange(len(states))])
y_max = max([states[i][1] for i in xrange(len(states))])
z_min = min([states[i][2] for i in xrange(len(states))])
z_max = max([states[i][2] for i in xrange(len(states))])
scale = [-0.2, 0.2]
plot_3d_points(np.array(states),
x_scale=[x_min, x_max],
y_scale=[y_min, y_max],
z_scale=[z_min, z_max])
sleep
def plot_current_tooth(self):
self.tooth_number = int(self.e1.get()) - 1
surface_idx = self.total_check_list_entry_short_name.index(
self.e2.get())
dicom_pc_file = 'C:\\tooth segmentation raw\\tooth_' + np.str(
self.tooth_number +
1) + '_surface_' + self.total_check_list_entry_name[
surface_idx] + '.csv'
points = Yomiread.read_csv(dicom_pc_file, 3)
print('points are', points)
fig3 = plt.figure('show current tooth')
ax = plt.axes(projection='3d')
Yomiplot.plot_3d_points(points, ax, axe_option=False)
plt.show()
for i in range(N_REGION):
Yomiwrite.write_csv_matrix(
base + "Results\\" + np.str(N_REGION) + "_region_" +
np.str(RAN_MEAN) + "_random_tolerance_and " + np.str(BIAS_MEAN) +
"_bias_tolerance_value_95_region_" + np.str(i + 1) + ".txt",
total_value_95[i])
Yomiwrite.write_csv_matrix(
base + "Results\\" + np.str(N_REGION) + "_region_" +
np.str(RAN_MEAN) + "_random_tolerance_and " + np.str(BIAS_MEAN) +
"_bias_tolerance_points_region_" + np.str(i + 1) + ".txt",
all_vertex[i])
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
if len(green_point) > 0:
Yomiplot.plot_3d_points(np.asarray(green_point), ax, color='g')
if len(yellow_point) > 0:
Yomiplot.plot_3d_points(np.asarray(yellow_point), ax, color='y')
if len(red_point) > 0:
Yomiplot.plot_3d_points(np.asarray(red_point), ax, color='r')
# Yomiplot.plot_3d_points(all_vertex[3], ax, color = 'g')
plt.show()
exit()
# define number of trials, mean error and stdev
n = 10000
mean = 0
stdev = 0.15
fiducial_pc = []
target_pc = []
#for point in region_candidate.boundary_final:
# HU[point[0], point[1]] = 3000
#plt.imshow(HU)
#plt.show()
test_points = np.zeros(3)
for i in range(len(plot_points)):
test_points = np.vstack((test_points, np.asarray(plot_points[i])))
fig = plt.figure()
ax = plt.axes(projection='3d')
#print('shape of boundary is', np.shape(region_candidate.boundary_final))
#Yomiplot.plot_3d_points(region_candidate.boundary_final, ax, axe_option=False)
print('shape of boundary is', np.shape(test_points))
Yomiplot.plot_3d_points(test_points[1:, :],
ax,
color='green',
alpha=0.3,
axe_option=False)
if dimension == 4:
seed = [323, 360, 285]
dimension = dimension - 1
region_candidate = region(seed, dim=dimension)
srg_3d(region_candidate, scan, dim=dimension)
region_candidate.update_final_boundary(dim=dimension)
fig = plt.figure()
ax = plt.axes(projection='3d')
print('shape of boundary is', np.shape(region_candidate.boundary_final))
Yomiplot.plot_3d_points(region_candidate.boundary_final,
ax,
color='green',
def show_state_distribution(self, model_file, env_file):
self.robot.setupViewer(model_file, env_file)
M = 30.0 * np.identity(2 * self.robot_dof)
active_joints = v_string()
self.robot.getActiveJoints(active_joints)
self.robot_dof = len(active_joints)
#x = [0.0, -np.pi / 2.0, 0.0, 0.0, 0.0, 0.0]
states = []
for z in xrange(2000):
u = [70.0, 70.0, 70.0, 0.0, 0.0, 0.0]
current_state = v_double()
current_state[:] = x
control = v_double()
control[:] = u
control_error = v_double()
ce = self.sample_control_error(M)
#ce = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
control_error[:] = ce
x = None
for k in xrange(1):
result = v_double()
self.robot.propagate(current_state,
control,
control_error,
self.simulation_step_size,
self.delta_t,
result)
x = [result[i] for i in xrange(len(result))]
current_state[:] = x
print x
states.append(np.array(x[3:6]))
x = [0.0, -np.pi/ 2.0, 0.0, 0.0, 0.0, 0.0]
cjvals = v_double()
cjvels = v_double()
cjvals_arr = [x[i] for i in xrange(len(x) / 2)]
cjvels_arr = [x[i] for i in xrange(len(x) / 2, len(x))]
cjvals[:] = cjvals_arr
cjvels[:] = cjvels_arr
particle_joint_values = v2_double()
particle_joint_colors = v2_double()
self.robot.updateViewerValues(cjvals,
cjvels,
particle_joint_values,
particle_joint_values)
from plot import plot_3d_points
mins = []
maxs = []
x_min = min([states[i][0] for i in xrange(len(states))])
x_max = max([states[i][0] for i in xrange(len(states))])
y_min = min([states[i][1] for i in xrange(len(states))])
y_max = max([states[i][1] for i in xrange(len(states))])
z_min = min([states[i][2] for i in xrange(len(states))])
z_max = max([states[i][2] for i in xrange(len(states))])
scale = [-0.2, 0.2]
plot_3d_points(np.array(states),
x_scale = [x_min, x_max],
y_scale = [y_min, y_max],
z_scale= [z_min, z_max])
sleep
surface = np.vstack((surface, front))
surface = surface[1:, :]
volume_centroid = np.sum(surface, axis=0) / np.shape(surface)[0]
occlusal_surface_centroid = np.sum(occlusal, axis=0) / np.shape(occlusal)[0]
buccal_surface_centroid = np.sum(buccal, axis=0) / np.shape(buccal)[0]
lingual_surface_centroid = np.sum(lingual, axis=0) / np.shape(lingual)[0]
front_surface_centroid = np.sum(front, axis=0) / np.shape(front)[0]
surface_centroids = ap.combine_elements_in_list([
buccal_surface_centroid, front_surface_centroid, occlusal_surface_centroid,
lingual_surface_centroid
])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(surface, ax, color='green')
plt.show()
# read IOS points
# p1-p3: buccal side (positive direction: +y(CT) )
# p4-p6: front side (positive direction: +x(CT) )
# p7-p9: occlusal side (positive direction: -z(CT) )
# p10-p12: lingual side
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
surface = np.vstack((surface, front))
surface = surface[1:, :]
volume_centroid = np.sum(surface, axis=0) / np.shape(surface)[0]
occlusal_surface_centroid = np.sum(occlusal, axis=0) / np.shape(occlusal)[0]
buccal_surface_centroid = np.sum(buccal, axis=0) / np.shape(buccal)[0]
lingual_surface_centroid = np.sum(lingual, axis=0) / np.shape(lingual)[0]
front_surface_centroid = np.sum(front, axis=0) / np.shape(front)[0]
surface_centroids = ap.combine_elements_in_list([
buccal_surface_centroid, front_surface_centroid, occlusal_surface_centroid,
lingual_surface_centroid
])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(surface, ax, color='green')
plt.show()
# read IOS points
# p1-p3: buccal side (positive direction: +y(CT) )
# p4-p6: front side (positive direction: +x(CT) )
# p7-p9: occlusal side (positive direction: -z(CT) )
# p10-p12: lingual side
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
# prepare probing markers in ct and fiducial frames
marker_ga = Yomiread.read_YomiSettings(
PROBE_MARKER_GA_FILE, str='probe_positions') * 1000 # convert m to mm
marker_ct = Yomiread.read_csv(PROBE_MARKER_CT_FILE, 4, -1)[:, 1:]
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)
while ask_if_continue is True:
val = int(
input(
"Continue to segment another tooth? \n press '1' to continue, press '0' to stop \n"
))
if val == 1:
print('continue to segment the next tooth')
ask_if_continue = False
elif val == 0:
print('segmentation is finished')
start_tooth_segmentation = False
ask_if_continue = False
else:
print('wrong value was entered \n')
exit()
pc = pcm.preprocess_point_cloud(region_candidate.boundary_final,
voxel_size=4.0)
print('shape of pc is', np.shape(pc))
# source = o3d.PointCloud()
# source.points = o3d.Vector3dVector(region_candidate.boundary_final)
# source.paint_uniform_color([0, 0.651, 0.929]) # yellow
# o3d.draw_geometries([source])
plt.figure()
ax = plt.axes(projection='3d')
print('shape of boundary is', np.shape(region_candidate.boundary_final))
Yomiplot.plot_3d_points(pc, ax, color='green', alpha=0.3, axe_option=False)
plt.show()
newcmp = ListedColormap(map(np.linspace(0.25, 0.75, 256)))
# Set up the axes limits
ax.axes.set_xlim3d(left=-40, right=40)
ax.axes.set_ylim3d(bottom=-40, top=40)
ax.axes.set_zlim3d(bottom=0, top=40)
# Create axes labels
ax.set_xlabel('X(mm)')
ax.set_ylabel('Y(mm)')
ax.set_zlabel('Z(mm)')
# read tooth surface points and plot
tooth_surface_file = "tooth_surface.txt"
tooth_surface_points = Yomiread.read_csv(path + tooth_surface_file, 3, -1)
Yomiplot.plot_3d_points(tooth_surface_points, ax, 'grey', alpha=0.3)
# define targets and plot
base = "G:\\My Drive\\Project\\HW-9232 Registration method for edentulous\\Edentulous registration error analysis\\"
measurement_file = "Raw_data\\target_fiducial_measurements.txt"
points = Yomiread.read_csv(base + measurement_file, 4, 10, flag=0)
targets_original = points[0:4, 1:4]
Yomiplot.plot_3d_points(targets_original, ax, 'purple')
# Select method and plot
N_REGION = 4
tolerance = 0.5
BIAS_MEAN = 1.0
RAN_MEAN = 0
GREEN = 0
YELLOW = 0
marker_ct_offset.append(new_point)
marker_ct_offset_pc = open3d.PointCloud()
marker_ct_offset_pc.points = open3d.Vector3dVector(marker_ct_offset)
open3d.visualization.draw_geometries([marker_ct_offset_pc])
# Perform ICP registration
voxel_size = 1.5
threshold = 1.0
trans_init = np.eye(4)
transform = ICP.registration_simple_pc(voxel_size, threshold, marker_ga, marker_ct_offset, trans_init)
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
#Yomiplot.plot_3d_points(marker_ct, ax, color='g')
Yomiplot.plot_3d_points(marker_ga, ax, color='blue')
plt.show()
exit()
# 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', delim=' ')
landmark_ct = np.array([45.872546347336275, 47.517325926047029, 52.978035379603348])
#landmark_fs = np.matmul(np.linalg.inv(R),(landmark_ct - t))
]
arch1.divide_arch_individual_target_modified_angle_seg(
D_RADIUS,
D_ANGLE,
D_HEIGHT,
individual_defined_range=individual_range_modified_angle_seg,
angle_span=angle_span)
# Remove certain fiducials based on idx
remove_idx_list = [2, 3, 6, 9, 14]
arch1.remove_regions(remove_idx_list)
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(arch1.default_fiducial, ax, color='g')
plt.show()
# Perform Simulation
N = 5000
BIAS_MEAN = 1.0
BIAS_STDEV = 0.15
RAN_MEAN = 0.97
RAN_STDEV = 0.30
#N_REGION = 10
THRESHOLD_0 = 0.7
THRESHOLD_1 = 1
green_point = []
yellow_point = []
red_point = []
max_value_95 = []
source_points = source_points_new
# print('destination_points are', destination_points_new[0,:])
if error < 0.01:
break
print('shape of original robot points is', np.shape(original_points_Probe))
print('shape of final robot points is',
np.shape(ap.combine_elements_in_list(source_points)))
R_final, t_final = registration.point_set_registration(
original_points_Probe, ap.combine_elements_in_list(source_points))
source_points_array = ap.combine_elements_in_list(source_points)
fig3 = plt.figure()
ax = fig3.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(teeth_CT[plot_tooth - 1].surface_all_points,
ax,
color='green',
alpha=0.2)
Yomiplot.plot_3d_points(source_points_array, ax, color='red')
#Yomiplot.plot_3d_points(source_points, ax, color='blue')
plt.show()
FIDUCIAL_ARRAY_FS_FILE = "C:\\Neocis\\FiducialArrays\\FXT-0086-07-LRUL-MFG-Splint.txt"
fiducial_array_fs = Yomiread.read_csv_specific_rows(FIDUCIAL_ARRAY_FS_FILE,
4, [3, -1],
delimiter=' ')[:, 1:]
fiducial_array_ct = []
fiducial_array_ct_2 = []
for point in fiducial_array_fs:
fiducial_array_ct_2.append(np.matmul(R_final, point) + t_final)
fiducial_array_ct.append(
np.matmul(np.linalg.inv(R_final), (point - t_final)))
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)
fig1 = plt.figure()
ax = fig1.add_subplot(111, projection='3d')
Yomiplot.plot_3d_points(original_surface_points, ax, color='g')
Yomiplot.plot_3d_points(turbulent_surface_points, ax, color='blue')
plt.show()