Exemplo n.º 1
0
Arquivo: lqg.py Projeto: hebinbing/LQG
    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
Exemplo n.º 2
0
 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 = []
Exemplo n.º 4
0
        #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',
Exemplo n.º 5
0
 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
Exemplo n.º 6
0
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
Exemplo n.º 8
0
    # 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)
Exemplo n.º 9
0
        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
Exemplo n.º 11
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 = []
Exemplo n.º 13
0
        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)))
Exemplo n.º 14
0
        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()