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()