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