def save_point_cloud_from_VREP():
depth_im, rgb_im = save_images_from_VREP("preDiploma_PC/")
depth, rgb = image_processing.calculate_point_cloud(rgb_im, depth_im)
current_object = PointsObject()
current_object.add_points(depth, rgb)
current_object.save_all_points("preDiploma_PC/", "box")
def save_point_cloud_from_images():
rgb_im = image_processing.load_image("preDiploma_PC/", "rgb_box_0.png")
depth_im = image_processing.load_image("preDiploma_PC/", "depth_box_0.png",
"depth")
points, color = image_processing.calculate_point_cloud(
rgb_im / 255, depth_im / 255)
current_object = PointsObject()
current_object.add_points(points, color)
current_object.save_all_points("preDiploma_PC/", "box")
def object_from_point_cloud(path, number_of_points):
xyz_points, rgb_points = download_point_cloud.download_ply(path)
if number_of_points is not None and number_of_points > xyz_points.shape[
0]:
number_of_points = xyz_points.shape[0]
object_points = PointsObject()
object_points.set_points(xyz_points, rgb_points, number_of_points)
object_points.scale(0.5)
# visualization.visualize_object([object_points])
return object_points
def create_movement_path(object, angles_params, shift_params,
observation_time):
angles_params = np.flip(angles_params, axis=1)
shift_params = np.flip(shift_params, axis=1)
number_of_points = observation_time.shape[0]
shifts = generate_func(shift_params, observation_time)
rotations = generate_func(angles_params, observation_time)
points = []
zero_t_points = object.get_points()[0]
for i, t in enumerate(observation_time):
points.append(PointsObject())
points[-1].add_points(zero_t_points)
points[-1].rotate(rotations[i])
points[-1].shift(shifts[i])
return rotations, shifts + object.get_center(), points
def download_to_object(path, number_of_points=None):
"""Downloading .ply file and making an instance of PointsObject
Args:
path (str): Path to the .ply file
number_of_points (int): number of active points which will be added to object
Returns:
object (instance of PointsObject)
"""
from points_object import PointsObject
pcd = open3d.io.read_point_cloud(path)
object = PointsObject(np.asarray(pcd.points),
np.asarray(pcd.colors),
camera_position=np.asarray([0, 0, 0]),
number=number_of_points)
return object
def check_data_generation():
data_generation.save_images_from_VREP()
depth_im = image_processing.load_image("3d_map/", "room_depth0.png",
"depth")
rgb_im = image_processing.load_image("3d_map/", "room_rgb0.png")
xyz, rgb = image_processing.calculate_point_cloud(rgb_im / 255,
depth_im / 255)
temp = PointsObject()
temp.add_points(xyz, rgb)
temp.save_all_points("3d_map/", "room")
visualization.visualize([temp])
def point_cloud_from_VREP():
import vrep_functions
import image_processing
"""Function for checking if vrep_functions and PointsObject are working fine"""
client_id = vrep_functions.vrep_connection()
vrep_functions.vrep_start_sim(client_id)
kinect_rgb_id = vrep_functions.get_object_id(client_id, 'kinect_rgb')
kinect_depth_id = vrep_functions.get_object_id(client_id, 'kinect_depth')
depth_im, rgb_im = vrep_functions.vrep_get_kinect_images(
client_id, kinect_rgb_id, kinect_depth_id)
image_processing.save_image(rgb_im, "preDiploma_PC/", 0, "rgb_box_")
image_processing.save_image(depth_im, "preDiploma_PC/", 0, "depth_box_")
print(depth_im.shape, rgb_im.shape)
vrep_functions.vrep_stop_sim(client_id)
depth, rgb = image_processing.calculate_point_cloud(rgb_im, depth_im)
current_object = PointsObject()
current_object.add_points(depth, rgb)
current_object.save_all_points("preDiploma_PC/", "box")
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 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 save_points_cloud():
points_cloud, points_color = create_points_cloud()
object = PointsObject()
object.set_points(points_cloud, points_color)
object.save_all_points("Test", "ball")
def objects_test_moving_figures_global():
classes = {}
rgb_im = image_processing.load_image("falling balls and cylinder",
"rgb_" + str(0) + ".png")
depth_im = image_processing.load_image("falling balls and cylinder",
"depth_" + str(0) + ".png",
"depth")
mog = RGBD_MoG(rgb_im, depth_im, number_of_gaussians=3)
for number_of_frame in range(1, 5):
color_mask = np.zeros([rgb_im.shape[0], rgb_im.shape[1], 3])
rgb_im = image_processing.load_image(
"falling balls and cylinder",
"rgb_" + str(number_of_frame) + ".png")
depth_im = image_processing.load_image(
"falling balls and cylinder",
"depth_" + str(number_of_frame) + ".png", "depth")
mask = mog.set_mask(rgb_im, depth_im)
depth_im = depth_im * (mask / 255).astype(int)
masks = region_growing(mask / 255,
depth_im / 255,
depth_threshold=0.01,
significant_number_of_points=10)
if len(masks) == 0:
print("No moving objects in the frame")
else:
for mask in masks:
xyz_points, rgb_points = image_processing.calculate_point_cloud(
rgb_im / 255, depth_im * mask / 255)
current_object = PointsObject()
current_object.set_points(xyz_points, rgb_points)
norms = current_object.get_normals()
compared_object_descriptor = GlobalCovarianceDescriptor(
xyz_points,
rgb_points,
norms,
depth_im,
rgb_im,
mask,
use_xyz=True,
use_rgb=True,
use_normals=True)
match_found = False
lengths = np.zeros([len(classes)])
if number_of_frame == 1:
match_found = False
else:
match_found = True
for object_number, object_class in enumerate(classes):
lengths[
object_number] = object_class.compare_descriptors(
compared_object_descriptor.
object_descriptor)
min_arg = np.argmin(lengths)
print(lengths)
for object_number, object_class in enumerate(classes):
if object_number == min_arg:
color_mask[:, :,
0] += mask * classes[object_class][0]
color_mask[:, :,
1] += mask * classes[object_class][1]
color_mask[:, :,
2] += mask * classes[object_class][2]
if not match_found:
classes[compared_object_descriptor] = np.random.rand(3)
color_mask[:, :, 0] += mask * classes[
compared_object_descriptor][0]
color_mask[:, :, 1] += mask * classes[
compared_object_descriptor][1]
color_mask[:, :, 2] += mask * classes[
compared_object_descriptor][2]
image_processing.save_image(color_mask,
"tracking_results",
frame_number=number_of_frame,
image_name="global_two_same")
def objects_test_moving_figures_local():
number_of_comparing_points = 100
classes = {}
rgb_im = image_processing.load_image("falling balls and cylinder",
"rgb_" + str(0) + ".png")
depth_im = image_processing.load_image("falling balls and cylinder",
"depth_" + str(0) + ".png",
"depth")
mog = RGBD_MoG(rgb_im, depth_im, number_of_gaussians=3)
for number_of_frame in range(1, 5):
color_mask = np.zeros([rgb_im.shape[0], rgb_im.shape[1], 3])
rgb_im = image_processing.load_image(
"falling balls and cylinder",
"rgb_" + str(number_of_frame) + ".png")
depth_im = image_processing.load_image(
"falling balls and cylinder",
"depth_" + str(number_of_frame) + ".png", "depth")
mask = mog.set_mask(rgb_im, depth_im)
depth_im = depth_im * (mask / 255).astype(int)
masks = region_growing(mask / 255,
depth_im / 255,
depth_threshold=0.01,
significant_number_of_points=10)
if len(masks) == 0:
print("No moving objects in the frame")
else:
for mask in masks:
xyz_points, rgb_points = image_processing.calculate_point_cloud(
rgb_im / 255, depth_im * mask / 255)
current_object = PointsObject()
current_object.set_points(xyz_points, rgb_points)
norms = current_object.get_normals()
compared_object_descriptor = CovarianceDescriptor(
xyz_points,
rgb_points,
norms,
k_nearest_neighbours=None,
relevant_distance=0.1,
use_alpha=True,
use_beta=True,
use_ro=True,
use_theta=True,
use_psi=True,
use_rgb=True)
match_found = False
lengths = np.zeros([
len(classes),
np.amin(
[number_of_comparing_points, xyz_points.shape[0]])
])
if number_of_frame == 1:
match_found = False
else:
match_found = True
for object_number, object_class in enumerate(classes):
lengths[
object_number] = object_class.compare_descriptors(
compared_object_descriptor.
object_descriptor,
number_of_comparing_points)
print(np.sum(mask))
min_args = np.argmin(
lengths, axis=0)[np.amin(lengths, axis=0) < 0.1]
unique, counts = np.unique(min_args,
return_counts=True)
best_match = unique[np.argmax(counts)]
for object_number, object_class in enumerate(classes):
if object_number == best_match:
color_mask[:, :,
0] += mask * classes[object_class][0]
color_mask[:, :,
1] += mask * classes[object_class][1]
color_mask[:, :,
2] += mask * classes[object_class][2]
if not match_found:
classes[compared_object_descriptor] = np.random.rand(3)
color_mask[:, :, 0] += mask * classes[
compared_object_descriptor][0]
color_mask[:, :, 1] += mask * classes[
compared_object_descriptor][1]
color_mask[:, :, 2] += mask * classes[
compared_object_descriptor][2]
image_processing.save_image(color_mask,
"tracking_results",
frame_number=number_of_frame,
image_name="local_two_same")