def check_function_calculation(): # parameters shapes = [] # data generation parameters rotation_params = np.asarray([[0, 0], [0, 0], [0, 50]]) moving_params = np.asarray([[0, 0.1, -0.03], [0, -0.5, -0.1], [0, 0.1, 0]]) observation_step_time = 0.2 number_of_observations = 5 observation_moments = np.arange( 0, round(number_of_observations * observation_step_time, 3), observation_step_time) future_time = np.arange( 0, round(number_of_observations * observation_step_time * 6, 3), observation_step_time) # load the models stable_object = download_point_cloud.download_to_object("3d_map/room.pcd") stable_object_points = stable_object.get_points()[0] falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 500) falling_object.scale(0.1) prediction_points = falling_object.get_points()[0] falling_object.shift([0.3, 0.6, 2.2]) # generate observation data rotation_angles_gt, center_position_gt, moving_objects = data_generation.create_movement_path( falling_object, rotation_params, moving_params, observation_moments)
def try_two_objects_interaction(): orange_sphere = download_point_cloud.download_to_object( "models/orange sphere.ply", 1000) orange_sphere.scale(0.3) orange_sphere.shift([0, 0.18, 0]) # visualization.visualize_object([orange_sphere]) # moving_orange_sphere = PotentialFieldObject(orange_sphere) grey_plane = download_point_cloud.download_to_object( "models/grey plane.ply", 6000) grey_plane.scale(0.1) grey_plane.rotate([1, 0, 0], math.radians(90)) # visualization.visualize(objects=[grey_plane, orange_sphere]) # moving_orange_sphere.interaction(grey_plane) # blue_conus = download_point_cloud.download_to_object("models/blue conus.ply", 3000) # blue_conus.scale(0.4) # blue_conus.rotate([1, 0, 0], math.radians(30)) # blue_conus.rotate([0, 1, 0], math.radians(60)) # blue_conus.shift([0, -0.3, 0]) # visualization.visualize(objects=[blue_conus, orange_sphere]) # moving_orange_sphere.interaction(blue_conus) # brown_cylinder = download_point_cloud.download_to_object("models/brown cylinder.ply", 3000) # brown_cylinder.scale(0.4) # brown_cylinder.rotate([1, 0, 0], math.radians(60)) # brown_cylinder.rotate([0, 1, 0], math.radians(30)) # brown_cylinder.shift([-0.3, -0.6, 0]) # visualization.visualize(objects=[brown_cylinder, orange_sphere]) # moving_orange_sphere.interaction(brown_cylinder) violet_thor = download_point_cloud.download_to_object( "models/violet thor.ply") visualization.visualize(objects=[violet_thor])
def load_many_objects(): models_list = [] models_list.append( download_point_cloud.download_to_object("models/blue conus.ply")) models_list.append( download_point_cloud.download_to_object("models/grey plane.ply")) models_list.append( download_point_cloud.download_to_object("models/red cube.ply")) models_list[0].scale(0.1) models_list[0].clear() visualization.visualize(models_list[0].get_points()[0], models_list[0].get_points()[1]) visualization.visualize_object(models_list)
def temp_1(): import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation as R rotation = np.asarray([15, 10, 20]) current_rotation = np.asarray([0, 0, 0]) falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) falling_object.scale(0.3) previous_points = falling_object.get_points()[0] initial_points = np.copy(previous_points) rotation_from_initial = [] rotation_from_previous = [] rotation_quarterions = [] t = np.arange(0, 15, 1) for i in t: current_rotation += rotation falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) falling_object.scale(0.3) falling_object.rotate(current_rotation) current_points = falling_object.get_points()[0] transformation = open3d_icp.get_transformation_matrix_p2p( initial_points, current_points) rotation_from_initial.append( moving_prediction.get_angles_from_transformation( transformation[:3, :3])) transformation = open3d_icp.get_transformation_matrix_p2p( previous_points, current_points) rotation_from_previous.append( moving_prediction.get_angles_from_transformation( transformation[:3, :3])) previous_points = np.copy(current_points) angles = np.cumsum(rotation_from_previous, axis=0) rotation_from_initial = np.asarray(rotation_from_initial) rotation_from_previous = np.asarray(rotation_from_previous) angles = np.asarray(angles) for i in range(3): plt.plot(t, rotation_from_initial[:, i], '-', t, angles[:, 0], '--') plt.show() plt.plot(t, rotation_from_previous[:, i], '-') plt.show()
def temp_2(): import open3d as o3d full_model = download_point_cloud.download_to_object( "models/blue conus.ply", 3000) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(full_model.get_points()[0]) # pcd = o3d.io.read_point_cloud("models/brown cylinder.ply") downpcd = pcd.voxel_down_sample(voxel_size=0.05) o3d.visualization.draw_geometries([downpcd]) downpcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid( radius=0.1, max_nn=30)) o3d.visualization.draw_geometries([downpcd])
def check_normals_estimation(): stable_object = download_point_cloud.download_to_object("3d_map/room.pcd") points = stable_object.get_points()[0] normals = stable_object.get_normals() / 100 normals_object = PointsObject(points + normals) visualization.visualize([stable_object, normals_object]) d_x = 0.1 new_points, new_normals, _ = data_generation.reduce_environment_points( stable_object.get_points()[0], stable_object.get_normals(), d_x) new_points_object = PointsObject(new_points, np.full(new_points.shape, 0.3)) new_normals_object = PointsObject(new_points + new_normals / 100) visualization.visualize([new_points_object, new_normals_object])
def check_RANSAC(): ball = download_point_cloud.download_to_object("preDiploma_PC/box.pcd") full_model = ball found_shapes = shape_recognition.RANSAC(full_model.get_points()[0], full_model.get_normals()) shapes = [full_model] for _, s in enumerate(found_shapes): new_shape = PointsObject() new_shape.add_points( s, np.asarray([[random.random(), random.random(), random.random()]] * s.shape[0])) shapes.append(new_shape) visualization.visualize_object(shapes)
def fill_the_shape_part(): # save_points_cloud() ball = PointsObject() ball = download_point_cloud.download_to_object("preDiploma_PC/box.pcd") # visualization.visualize_object([ball]) full_model = ball # full_model = download_point_cloud.download_to_object("models/blue conus.ply", 3000) # full_model.scale(0.1) # full_model.shift([0.01, 0.05, 0.01]) # full_model.rotate([1, 1, 1], math.radians(35)) # # full_model_2 = download_point_cloud.download_to_object("models/orange sphere.ply", 3000) # full_model_2.scale(0.1) # full_model_2.shift([-0.1, -0.1, 0.1]) # full_model.rotate([1, 1, 1], math.radians(60)) # full_model.add_points(full_model_2.get_points()[0], full_model_2.get_points()[1]) # # full_model_2 = download_point_cloud.download_to_object("models/orange sphere.ply", 3000) # full_model_2.scale(0.1) # full_model_2.shift([-0.01, 0.1, 0.3]) # full_model.rotate([1, 0, 1], math.radians(30)) # full_model.add_points(full_model_2.get_points()[0], full_model_2.get_points()[1]) # visualization.visualize_object([full_model]) # temp() # temp_2() start = time.time() found_shapes = shape_recognition.RANSAC(full_model.get_points()[0], full_model.get_normals()) print(time.time() - start) shapes = [full_model] for _, s in enumerate(found_shapes): new_shape = PointsObject() new_shape.add_points( s, np.asarray([[random.random(), random.random(), random.random()]] * s.shape[0])) shapes.append(new_shape) visualization.visualize_object(shapes)
def observation_momental(): # load the model stable_object = download_point_cloud.download_to_object( "models/grey plane.ply", 3000) stable_object.scale(0.3) stable_object.rotate([90, 0, 0]) falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) falling_object.scale(0.3) falling_object.shift([0, 3, 0]) shapes = [falling_object] center = falling_object.get_center() # temp_1() # generate observation data rotation_params = np.asarray([[0, 70], [0, 50], [0, 80]]) moving_params = np.asarray([[0, 0.1, -0.3], [0, -1.5, 0.1], [0, 1, 0]]) observation_step_time = 0.2 number_of_observations = 5 observation_moments = np.arange( 0, round(number_of_observations * observation_step_time, 3), observation_step_time) rotation_angles_gt, center_position_gt, moving_objects = create_movement_path( falling_object, rotation_params, moving_params, observation_moments) for i, m in enumerate(moving_objects): found_shapes = shape_recognition.RANSAC(m.get_points()[0], m.get_normals()) moving_objects[i].set_points(found_shapes[-1]) found_rotation, found_center_positions = find_observations( moving_objects, falling_object.get_center()) print(center_position_gt) print(found_center_positions) shapes = [] shapes += moving_objects # visualization.visualize(shapes) # find functions for xyz trajectory start = time.time() trajectory_functions_x = moving_prediction.find_functions( observation_moments, found_center_positions[:, 0]) trajectory_functions_y = moving_prediction.find_functions( observation_moments, found_center_positions[:, 1]) trajectory_functions_z = moving_prediction.find_functions( observation_moments, found_center_positions[:, 2]) angle_functions_x = moving_prediction.find_functions( observation_moments, found_rotation[:, 0]) angle_functions_y = moving_prediction.find_functions( observation_moments, found_rotation[:, 1]) angle_functions_z = moving_prediction.find_functions( observation_moments, found_rotation[:, 2]) print(time.time() - start) future_time = np.arange( 0, round(number_of_observations * observation_step_time * 6, 3), observation_step_time) future_angles_gt, future_center_gt, _ = create_movement_path( falling_object, rotation_params, moving_params, future_time) visualization.show_found_functions(trajectory_functions_x, observation_moments, found_center_positions[:, 0], future_time, future_center_gt[:, 0], 't, s', 'x, m', 'x coordinate of center') visualization.show_found_functions(trajectory_functions_y, observation_moments, found_center_positions[:, 1], future_time, future_center_gt[:, 1], 't, s', 'y, m', 'y coordinate of center') visualization.show_found_functions(trajectory_functions_z, observation_moments, found_center_positions[:, 2], future_time, future_center_gt[:, 2], 't, s', 'z, m', 'z coordinate of center') visualization.show_found_functions(angle_functions_x, observation_moments, found_rotation[:, 0], future_time, future_angles_gt[:, 0], 't, s', 'angle, deg', 'x axis angle') visualization.show_found_functions(angle_functions_y, observation_moments, found_rotation[:, 1], future_time, future_angles_gt[:, 1], 't, s', 'angle, deg', 'y axis angle') visualization.show_found_functions(angle_functions_z, observation_moments, found_rotation[:, 2], future_time, future_angles_gt[:, 2], 't, s', 'angle, deg', 'z axis angle') # prediction part time_of_probability = 2. d_x = 0.1 d_angle = 1 threshold_p = 0.5 prob_x, x = moving_prediction.probability_of_being_in_point( trajectory_functions_x, time_of_probability, d_x, True) prob_y, y = moving_prediction.probability_of_being_in_point( trajectory_functions_y, time_of_probability, d_x, True) prob_z, z = moving_prediction.probability_of_being_in_point( trajectory_functions_z, time_of_probability, d_x, True) prob_x_angle, x_angle = moving_prediction.probability_of_being_in_point( angle_functions_x, time_of_probability, d_angle, True) prob_y_angle, y_angle = moving_prediction.probability_of_being_in_point( angle_functions_y, time_of_probability, d_angle, True) prob_z_angle, z_angle = moving_prediction.probability_of_being_in_point( angle_functions_z, time_of_probability, d_angle, True) prediction_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) prediction_object.scale(0.3) prediction_points = prediction_object.get_points()[0] xyz_dict = moving_prediction.get_xyz_probabilities_from_angles_probabilities( prediction_points, x_angle, prob_x_angle, y_angle, prob_y_angle, z_angle, prob_z_angle, d_x, threshold_p) points, probabilities = moving_prediction.probability_of_all_points( xyz_dict, prob_x, x, prob_y, y, prob_z, z, threshold_p) if xyz_dict == -1: print("всё сломалось") sys.exit(0) high_probabilities = np.where(probabilities >= threshold_p, True, False) high_probable_points, high_probable_points_probabilities = points[ high_probabilities], probabilities[high_probabilities] shapes.append( generate_color_shapes(high_probable_points, high_probable_points_probabilities)) # generate ground truth observation_moment = np.asarray([time_of_probability]) _, _, moving_objects = create_movement_path(falling_object, rotation_params, moving_params, observation_moment) points = moving_objects[0].get_points()[0] gt_object = PointsObject() gt_object.add_points(points, falling_object.get_points()[1]) shapes += [gt_object] visualization.visualize_object(shapes) get_histogram(high_probable_points, high_probable_points_probabilities)
def linear_movement(): # load the model stable_object = download_point_cloud.download_to_object( "models/grey plane.ply", 3000) stable_object.scale(0.2) stable_object.rotate([1, 0, 0], math.radians(90)) falling_object = download_point_cloud.download_to_object( "models/orange sphere.ply", 3000) falling_object.scale(0.4) falling_object.shift([0, 2, 0]) shapes = [stable_object] # generating parameters and trajectory number_of_steps = 5 step_time = 0.2 parameters = np.array([[1, -3], [0, -9.8], []]) # training data time_ = np.arange(step_time, (number_of_steps + 1) * step_time, step_time) points_trajectory, center_trajectory = generate_trajectory( falling_object, generate_func, parameters, time_) # data to compare ttime = np.arange(step_time, (number_of_steps + 1) * step_time * 1.5, step_time / 10) _, real_trajectory = generate_trajectory(falling_object, generate_func, parameters, ttime) # add noise center_trajectory += np.random.normal(0, 0.05, center_trajectory.shape) # find functions for xyz trajectory start = time.time() found_functions_x = moving_prediction.find_functions( time_, center_trajectory[:, 0]) found_functions_y = moving_prediction.find_functions( time_, center_trajectory[:, 1]) found_functions_z = moving_prediction.find_functions( time_, center_trajectory[:, 2]) print(time.time() - start) # show prediction results visualization.show_found_functions(found_functions_x, time_, center_trajectory[:, 0], ttime, real_trajectory[:, 0]) visualization.show_found_functions(found_functions_y, time_, center_trajectory[:, 1], ttime, real_trajectory[:, 1]) visualization.show_found_functions(found_functions_z, time_, center_trajectory[:, 2], ttime, real_trajectory[:, 2]) # estimation probability of being in points in time t time_of_probability = 1. d_x = 0.2 # moving_prediction.show_gaussians(found_functions_x, .6, .1) prob_x, x = moving_prediction.probability_of_being_in_point( found_functions_x, time_of_probability, d_x, True) prob_y, y = moving_prediction.probability_of_being_in_point( found_functions_y, time_of_probability, d_x, True) prob_z, z = moving_prediction.probability_of_being_in_point( found_functions_z, time_of_probability, d_x, True) # create points where probability > threshold_p threshold_p = 0.7 prob_x, x = prob_x[prob_x > threshold_p], x[prob_x > threshold_p] prob_y, y = prob_y[prob_y > threshold_p], y[prob_y > threshold_p] prob_z, z = prob_z[prob_z > threshold_p], z[prob_z > threshold_p] if x.shape[0] * y.shape[0] * z.shape[0] > 10000: print("Слишком много точек") else: points = np.array(np.meshgrid(x, y, z)).T.reshape(-1, 3) probabilities = np.array(np.meshgrid(prob_x, prob_y, prob_z)).T.reshape(-1, 3) high_probabilities = np.where( np.prod(probabilities, axis=1) >= threshold_p, True, False) high_probable_points, high_probable_points_probabilities = points[high_probabilities], \ np.prod(probabilities, axis=1)[high_probabilities] # print(high_probable_points, np.prod(high_probable_points_probabilities, axis=1)) # shapes += points_trajectory shapes += generate_found_shapes(falling_object, high_probable_points, high_probable_points_probabilities) time_ = np.asarray([time_of_probability]) shapes += generate_trajectory(falling_object, generate_func, parameters, time_)[0] visualization.visualize_object(shapes)
from points_object import PointsObject import image_processing import visualization import download_point_cloud import shape_recognition import moving_prediction import open3d_icp if __name__ == "__main__": # parameters # load the model stable_object = download_point_cloud.download_to_object( "models/grey plane.ply", 3000) stable_object.scale(0.3) stable_object.rotate([90, 0, 0]) falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) falling_object.scale(0.3) falling_object.shift([0, 3, 0]) shapes = [falling_object]
def check_physical_objects_interaction_to_moment(): # parameters shapes = [] # data generation parameters rotation_params = np.asarray([[0, 0], [0, 0], [0, 50]]) moving_params = np.asarray([[0, 0.1, -0.03], [0, -0.5, -0.1], [0, 0.1, 0]]) observation_step_time = 0.2 number_of_observations = 5 observation_moments = np.arange( 0, round(number_of_observations * observation_step_time, 3), observation_step_time) future_time = np.arange( 0, round(number_of_observations * observation_step_time * 6, 3), observation_step_time) # prediciton parameters time_of_probability = 1.3 d_x = 0.1 d_angle = 1 threshold_p = 0.5 observation_moment = np.asarray([time_of_probability]) # load the models stable_object = download_point_cloud.download_to_object("3d_map/room.pcd") stable_object_points = stable_object.get_points()[0] falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 500) falling_object.scale(0.1) prediction_points = falling_object.get_points()[0] falling_object.shift([0.3, 0.6, 2.2]) environment_xyz, environment_normals, unique_environment_xyz = data_generation.reduce_environment_points( stable_object_points, stable_object.get_normals(), d_x) # generate observation data rotation_angles_gt, center_position_gt, moving_objects = data_generation.create_movement_path( falling_object, rotation_params, moving_params, observation_moments) # for i, m in enumerate(moving_objects): # found_shapes = shape_recognition.RANSAC(m.get_points()[0], m.get_normals(), number_of_points_threshold=200) # moving_objects[i].set_points(found_shapes[-1]) max_radius = moving_prediction.find_max_radius(prediction_points) found_rotation, found_center_positions = moving_prediction.find_observations( moving_objects, falling_object.get_center()) # find functions for xyz trajectory center_functions, angles_functions = moving_prediction.find_center_and_rotation_functions( observation_moments, found_center_positions, found_rotation) center_f = [ MovementFunctions(center_functions[0], number_of_observations - 1), MovementFunctions(center_functions[1], number_of_observations - 1), MovementFunctions(center_functions[2], number_of_observations - 1) ] angles_f = [ MovementFunctions(angles_functions[0], number_of_observations - 1), MovementFunctions(angles_functions[1], number_of_observations - 1), MovementFunctions(angles_functions[2], number_of_observations - 1) ] # find min/max angles min_max_angles = np.zeros((3, 2)) min_max_center = np.zeros((3, 2)) for i in range(3): min_max_every_gaussian = moving_prediction.find_min_max_of_function( angles_f[i], time_of_probability) min_max_angles[i] = np.min(min_max_every_gaussian[0]), np.max( min_max_every_gaussian[1]) min_max_every_gaussian = moving_prediction.find_min_max_of_function( center_f[i], time_of_probability) min_max_center[i] = np.min(min_max_every_gaussian[0]), np.max( min_max_every_gaussian[1]) # find min/max deviation from center min_max_deviation = moving_prediction.find_min_max_deviation( min_max_angles, prediction_points, d_angle) # find parallelepiped of potential position of object potential_interaction_field = np.round( (min_max_center + min_max_deviation) / d_x) * d_x # find environment points in potential parallelepiped potential_environment_idx = moving_prediction.get_points_in_area( environment_xyz, potential_interaction_field) if np.sum(potential_environment_idx) == 0: print("all clear") return p_e_points = environment_xyz[potential_environment_idx] area = [[np.min(p_e_points[:, 0]), np.max(p_e_points[:, 0])], [np.min(p_e_points[:, 1]), np.max(p_e_points[:, 1])], [np.min(p_e_points[:, 2]), np.max(p_e_points[:, 2])]] interactive_points, interactive_probability = moving_prediction.probable_points_in_area( center_f, angles_f, prediction_points, area, time_of_probability, d_x, d_angle, threshold_p) env_idx, points_idx = moving_prediction.find_matches_in_two_arrays( np.around(p_e_points, 1), np.around(interactive_points, 1)) p_e_points = p_e_points[env_idx] start = time.time() points_velocities = moving_prediction.get_particles_velocities( interactive_points[points_idx], center_f, angles_f, observation_moment, max_radius + d_x / 2) print(points_velocities.angle_sd.shape) new_v, new_w, new_v_sd, new_w_sd, new_weight, _, slip_or_not = \ moving_prediction.find_new_velocities(points_velocities, environment_normals[env_idx], interactive_probability[points_idx]) print(points_velocities.angle_sd.shape) moving_prediction.update_gaussians(new_v, new_w, new_v_sd, new_w_sd, new_weight, points_velocities, center_f, angles_f, observation_moment, slip_or_not) print(time.time() - start) import matplotlib.pyplot as plt for c in center_f: number_of_gaussians = c.get_number_of_gaussians() t = np.arange(0, 3, 0.1) for n in range(number_of_gaussians): means, time_row = c.get_gaussian_presentation(n, t) plt.plot(time_row, means, '--') plt.show()
def check_physical_objects_interaction_at_moment(): # parameters shapes = [] # data generation parameters rotation_params = np.asarray([[0, 70], [0, 50], [0, 80]]) moving_params = np.asarray([[0, 0.1, -0.3], [0, -1.5, 0.1], [0, 1, 0]]) observation_step_time = 0.2 number_of_observations = 5 observation_moments = np.arange( 0, round(number_of_observations * observation_step_time, 3), observation_step_time) future_time = np.arange( 0, round(number_of_observations * observation_step_time * 6, 3), observation_step_time) # prediciton parameters time_of_probability = 1.8 d_x = 0.2 d_angle = 1 threshold_p = 0.5 observation_moment = np.asarray([time_of_probability]) # load the models stable_object = download_point_cloud.download_to_object( "models/grey plane.ply", 6000) stable_object.scale(0.6) stable_object.rotate([90, 0, 0]) falling_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) falling_object.scale(0.3) falling_object.shift([0, 3, 0]) prediction_object = download_point_cloud.download_to_object( "models/red cube.ply", 3000) prediction_object.scale(0.3) prediction_points = prediction_object.get_points()[0] shapes += [stable_object, falling_object] # generate observation data rotation_angles_gt, center_position_gt, moving_objects = data_generation.create_movement_path( falling_object, rotation_params, moving_params, observation_moments) found_rotation, found_center_positions = moving_prediction.find_observations( moving_objects, falling_object.get_center()) # find functions for xyz trajectory trajectory_functions_x = moving_prediction.find_functions( observation_moments, found_center_positions[:, 0]) trajectory_functions_y = moving_prediction.find_functions( observation_moments, found_center_positions[:, 1]) trajectory_functions_z = moving_prediction.find_functions( observation_moments, found_center_positions[:, 2]) angle_functions_x = moving_prediction.find_functions( observation_moments, found_rotation[:, 0]) angle_functions_y = moving_prediction.find_functions( observation_moments, found_rotation[:, 1]) angle_functions_z = moving_prediction.find_functions( observation_moments, found_rotation[:, 2]) # generation of future positions future_angles_gt, future_center_gt, _ = data_generation.create_movement_path( falling_object, rotation_params, moving_params, future_time) # prediction part prob_x, x = moving_prediction.probability_of_being_in_point( trajectory_functions_x, time_of_probability, d_x, True) prob_y, y = moving_prediction.probability_of_being_in_point( trajectory_functions_y, time_of_probability, d_x, True) prob_z, z = moving_prediction.probability_of_being_in_point( trajectory_functions_z, time_of_probability, d_x, True) prob_x_angle, x_angle = moving_prediction.probability_of_being_in_point( angle_functions_x, time_of_probability, d_angle, True) prob_y_angle, y_angle = moving_prediction.probability_of_being_in_point( angle_functions_y, time_of_probability, d_angle, True) prob_z_angle, z_angle = moving_prediction.probability_of_being_in_point( angle_functions_z, time_of_probability, d_angle, True) points, probabilities = moving_prediction.get_xyz_probabilities_from_angles_probabilities( prediction_points, x_angle, prob_x_angle, y_angle, prob_y_angle, z_angle, prob_z_angle, d_x, threshold_p) points, probabilities = moving_prediction.probability_of_all_points( points, probabilities, prob_x, x, prob_y, y, prob_z, z, threshold_p) high_probabilities = np.where(probabilities >= threshold_p, True, False) high_probable_points, high_probable_points_probabilities = points[ high_probabilities], probabilities[high_probabilities] shapes.append( data_generation.generate_color_of_probable_shapes( high_probable_points, high_probable_points_probabilities)) # visualization.visualize_object(shapes) new_points, new_probabilities = probablistic_interaction.create_new_probabilistic_position( high_probable_points, high_probable_points_probabilities, stable_object, d_x) shapes = [stable_object] shapes.append( data_generation.generate_color_of_probable_shapes( new_points, new_probabilities))