def object_from_picture():
mask, depth, rgb = get_moving_mask()
object_mask = get_one_object_mask(mask / 255,
depth / 255,
depth_threshold=0.05,
number_of_object=1)
xyz_points, rgb_points = image_processing.calculate_point_cloud(
rgb / 255, depth * object_mask / 255)
pc = PointsObject()
pc.set_points(xyz_points, rgb_points)
visualization.visualize_object([pc])
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():
ground_truth_vector = [0, 1, 0]
vector_model = PointsObject()
vector_model.add_points(np.asarray([ground_truth_vector]),
np.asarray([[1, 0, 0]]))
vector_model_2 = PointsObject()
vector_model_2.add_points(np.asarray([ground_truth_vector]))
vector_model.rotate([1, 1, 1], math.radians(60))
normal = vector_model.get_points()[0][0]
angle = shape_recognition.angle_between_normals(ground_truth_vector,
normal)
axis = np.cross(ground_truth_vector, normal)
vector_model.rotate(axis, angle)
visualization.visualize_object([vector_model, vector_model_2])
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)