def local_holes_transformation(self, mesh_object, nb_holes, hole_size,write_ply = False, path = "base"):
mesh_object = copy.deepcopy(mesh_object)
reject_groups = np.random.randint(mesh_object.points.shape[0], size=nb_holes)
tree = KDTree(mesh_object.points, leaf_size=2)
complete_rejected_indices = []
for reject_index in reject_groups:
indices = tree.query_radius(mesh_object.points[reject_index].reshape(1,-1), r=hole_size)[0]
complete_rejected_indices.extend(indices.tolist())
complete_rejected_indices.append(reject_index.tolist())
delete_index = list(set(complete_rejected_indices))
new_points = np.delete(mesh_object.points, delete_index, axis = 0)
print("After deletion, nb_points remaining : {}".format(new_points.shape))
holes_mesh = base.Meshgrid(new_points)
interest_points_holes = holes_mesh.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
ply.write_ply(path + "_all", holes_mesh.points, ['x','y','z'])
if write_ply:
ply.write_ply(path + "_int", interest_points_holes, ['x','y','z'])
return interest_points_holes
def save(self, mesh, mesh_color, path, red, green, blue):
"""
Home-made function to save a ply file with colors. A bit hacky
"""
#for i in range(self.right_arm.size()):
print(self.right_arm)
mesh.vertices[self.right_arm]=[0,0,0]
to_write = mesh.vertices
b = np.zeros((len(mesh.faces), 4)) + 3
b[:, 1:] = np.array(mesh.faces)
try:
points2write = pd.DataFrame({
'lst0Tite': to_write[:, 0],
'lst1Tite': to_write[:, 1],
'lst2Tite': to_write[:, 2],
'lst3Tite': red,
'lst4Tite': green,
'lst5Tite': blue,
})
ply.write_ply(filename=path, points=points2write, as_text=True, text=False,
faces=pd.DataFrame(b.astype(int)),
color=True)
except:
points2write = pd.DataFrame({
'lst0Tite': to_write[:, 0],
'lst1Tite': to_write[:, 1],
'lst2Tite': to_write[:, 2],
})
ply.write_ply(filename=path, points=points2write, as_text=True, text=False,
faces=pd.DataFrame(b.astype(int)),
color=False)
def npy2ply(all_data, name):
for i in range(all_data.shape[0]):
ply_path = ".\exp"
if not os.path.exists(ply_path):
os.makedirs(ply_path)
cloud_file = os.path.join(ply_path, name + str(i) + '.ply')
cloud_points = np.empty((0, 3), dtype=np.float32)
cloud_colors = np.empty((0, 3), dtype=np.uint8)
cloud_classes = np.empty((0, 1), dtype=np.int32)
object_data = all_data[i]
object_class = all_data[i][:, 6].astype(np.uint8)
cloud_points = np.vstack(
(cloud_points, object_data[:, 0:3].astype(np.float32)))
cloud_colors = np.vstack(
(cloud_colors, colors[object_class].astype(np.uint8)))
cloud_classes = np.vstack(
(cloud_classes, object_class.reshape(object_class.shape[0], 1)))
print(cloud_file)
ply.write_ply(cloud_file, (cloud_points, cloud_colors, cloud_classes),
['x', 'y', 'z', 'red', 'green', 'blue', 'class'])
def predict_point_cloud(self, model, index = 0, epsilon_weights = 3, confidence_threshold = .75, output_path = None):
all_indexes = np.arange(self.n_points_clouds[index])
predictions = np.zeros((self.n_points_clouds[index], self.n_classes))
weights = np.zeros(self.n_points_clouds[index])
t = time.time()
time_delay = 1800
pbar, pbar_value, pbar_update = tqdm_notebook(total=1000), 0, 0
while np.min(weights) < epsilon_weights:
center_points_indexes = all_indexes[weights < epsilon_weights]
cloud, ind, dist = self.sample_point_cloud(index = index, center_points_index = center_points_indexes[np.random.randint(len(center_points_indexes))])
weight = 1. / np.clip(dist, 1, 10.) #np.clip(dist, .1 * np.max(dist), 10.)
prediction = model.predict(np.expand_dims(cloud, axis = 0))[0]
max_prediction = np.max(prediction, axis = -1)
to_zeros = max_prediction < confidence_threshold
#print(max_prediction.shape, weight.shape, np.mean(to_zeros), np.min(max_prediction), np.mean(max_prediction), np.max(max_prediction))
prediction[to_zeros] = 0
weight[to_zeros] = 0
predictions[ind] += (prediction.transpose() * weight).transpose()
weights[ind] += weight
int_pbar_value = int(pbar_value)
pbar_value = 1000 * np.mean(weights > epsilon_weights)
pbar_update = int(pbar_value) - int_pbar_value
for _ in range(pbar_update):
pbar.update()
if time.time() - t > time_delay:
t = time.time()
time_delay /= 2
confidence_threshold = min(.25, confidence_threshold / 2.)
print("confidence_threshold, time_delay ==>", confidence_threshold, time_delay)
pbar.close()
predictions = (predictions.transpose() / weights).transpose()
predictions_int = (np.argmax(predictions, axis = -1) + 1).astype(int)
if(output_path is None):output_path = self.paths[index].split(".")[0] + "_prediction.ply"
savemat(output_path[:-4] + "_probas.mat", {"initial_classif": predictions})
np.save(output_path[:-4] + "_probas.npy", predictions)
if(self.use_normals):
write_ply(output_path,
[np.array(self.trees[index].data), self.normals[index], predictions_int],
['x', 'y', 'z', 'nx', 'ny', 'nz', 'class'])
else:
write_ply(output_path,
[np.array(self.trees[index].data), predictions_int],
['x', 'y', 'z', 'class'])
return predictions, predictions_int
def write_clouds(path, clouds):
import pandas
if os.path.isdir(path):
print(path, "écrasé")
else:
print(path, "écrit")
os.mkdir(path)
path += '/'
for i, c in enumerate(clouds):
ply.write_ply(path + str(i), pandas.DataFrame(c), as_text=True)
def transform(root, filename):
if not os.path.exists('./ply_gen'):
os.makedirs('./ply_gen')
# Example transformation.
# 1. Read the PLY data file.
data = read_ply(os.path.join(root, filename))
# 2. Shift points by the vector (25, -10, 7).
data = translate(data, 25, -10, 7)
# 3. Rotate points 45 degress around the Z-axis.
data = rotate_z(data, 45)
# 4. Write the new points into a new PLY data file.
write_ply(os.path.join('./ply_gen', filename), [data['x'], data['y'], data['z'], data['x_origin'], data['y_origin'], data['z_origin'],
data['GPS_time'], data['reflectance']], ['x', 'y', 'z', 'x_origin', 'y_origin', 'z_origin', 'GPS_time', 'reflectance'])
def grid_subsampling_transformation(self, mesh_object, grid_size,write_ply = False, path = "base"):
mesh_object = copy.deepcopy(mesh_object)
subsampled_points = self.grid_subsampling(mesh_object.points, grid_size)
subsampled_mesh = base.Meshgrid(subsampled_points)
interest_points_original = subsampled_mesh.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
ply.write_ply(path + "_all", subsampled_mesh.points, ['x','y','z'])
if write_ply:
ply.write_ply(path + "_int", interest_points_original, ['x','y','z'])
return interest_points_original
def predict_point_cloud(self,
model,
index=0,
epsilon_weights=.5,
output_path=None):
all_indexes = np.arange(self.n_points_clouds[index])
predictions = np.zeros((self.n_points_clouds[index], self.n_classes))
weights = np.zeros(self.n_points_clouds[index])
pbar, pbar_value, pbar_update = tqdm_notebook(total=100), 0, 0
while np.min(weights) < epsilon_weights:
center_points_indexes = all_indexes[weights < epsilon_weights]
cloud, ind, dist = self.sample_point_cloud(
index=index,
center_points_index=center_points_indexes[np.random.randint(
len(center_points_indexes))])
weight = 1. / np.clip(dist, .1 * np.max(dist), 10.)
prediction = model.predict(np.expand_dims(cloud, axis=0))[0]
predictions[ind] += (prediction.transpose() * weight).transpose()
weights[ind] += weight
int_pbar_value = int(pbar_value)
pbar_value = 100 * np.mean(weights > epsilon_weights)
pbar_update = int(pbar_value) - int_pbar_value
for i in range(pbar_update):
pbar.update()
pbar.close()
predictions = (predictions.transpose() / weights).transpose()
predictions_int = (np.argmax(predictions, axis=-1) + 1).astype(int)
if (output_path is None):
output_path = self.paths[index].split(".")[0] + "_prediction.ply"
if (self.use_normals):
write_ply(output_path, [
np.array(self.trees[index].data), self.normals[index],
predictions_int
], ['x', 'y', 'z', 'nx', 'ny', 'nz', 'class'])
else:
write_ply(output_path,
[np.array(self.trees[index].data), predictions_int],
['x', 'y', 'z', 'class'])
return predictions, predictions_int
def scale_transformation(self, mesh_object, scale_factor,write_ply = False, path = "base"):
#angle = [alpha, beta, gamma]
#Rotation Transformation
#Apply Scaling
#Only work on a copy of the original
mesh_object = copy.deepcopy(mesh_object)
scaled_mesh = base.Meshgrid(scale_factor * mesh_object.points)
interest_points_scaled = scaled_mesh.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
ply.write_ply(path + "_all", scaled_mesh.points, ['x','y','z'])
if write_ply:
ply.write_ply(path + "_int", interest_points_scaled, ['x','y','z'])
interest_points_original = interest_points_scaled / scale_factor
return interest_points_original
def rotation_test(self, write_ply = False, path = "base"):
#Original interest points
interest_points = self.mesh_objects.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
path_all = os.path.join(path, "base_all")
path_tmp = os.path.join(path, "base_int")
ply.write_ply(path_all, self.mesh_objects.points, ['x','y', 'z'])
ply.write_ply(path_tmp, interest_points, ['x','y', 'z'])
#Angle repeatability
path_angle = os.path.join(path,"angle/")
angle_repeat_list_x = []
angles = [[1,0,0],[5,0,0], [10,0,0], [15,0,0],[20,0,0],[30,0,0],[45,0,0],[75,0,0],[90,0,0],[120,0,0],[150,0,0],[180,0,0]]
for angle in angles:
angle = np.array(angle)
path_angle_tmp = path_angle + str(angle[1])
interest_points_rotated = self.roation_transformation(self.mesh_objects, angle,write_ply = write_ply, path=path_angle_tmp)
#Calculate repeatability
repeatability_value = self.calc_repeatability_reverse(interest_points, interest_points_rotated,self.repeatbiliy_thresh)
angle_repeat_list_x.append(repeatability_value)
print("Rep value angle{}".format(repeatability_value))
print("Path angle {}".format(path_angle))
angle_repeat_list_z = []
angles = [[0,0,1],[0,0,5], [0,0,10], [0,0,15],[0,0,20],[0,0,30],[0,0,45],[0,0,75],[0,0,90],[0,0,120],[0,0,150],[0,0,180]]
for angle in angles:
angle = np.array(angle)
path_angle_tmp = path_angle + str(angle[1])
interest_points_rotated = self.roation_transformation(self.mesh_objects, angle,write_ply = write_ply, path=path_angle_tmp)
#Calculate repeatability
repeatability_value = self.calc_repeatability_reverse(interest_points, interest_points_rotated,self.repeatbiliy_thresh)
angle_repeat_list_z.append(repeatability_value)
print("Rep value angle{}".format(repeatability_value))
return_values={"angle_x":angle_repeat_list_x, "angle_y":angle_repeat_list_z}
return return_values
def save_cloud_and_scalar_fields(cloud, scalar_fields, fields_name, save_dir, filename):
"""
Save a cloud into a .ply file to visualize it into CloudCompare for example.
In :
- cloud : shape (number of points, 3) contains the point coordinates
- scalar_field : list of scalar fields (each scalar field is an array of length 'number of points')
- fields_name : list containing the names under which saving the scalar fields (same size as scalar_fields)
- save_dir, filename : filename must end with ".ply"
"""
# Avoid handling bad headers (scalar fields must of float type)
for i in range(len(scalar_fields)):
scalar_fields[i] = scalar_fields[i].astype(float)
# Store results in a .ply file to visualize in CloudCompare
write_ply(save_dir + '/' + filename, [cloud] + scalar_fields,
['x', 'y', 'z'] + fields_name)
print("File saved !\n")
def test_pipeline_move(self, path_file_tmps = [],write_ply = False, keep_diameter=False):
#Used to test the non-rigid transformations
interest_points, _ = self.mesh_objects.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris, index = True)
basename = path_file_tmps[0].split("/")[-1]
basename = os.path.join("visualisation", basename)
print(basename)
if write_ply:
ply.write_ply(basename, interest_points, ['x', 'y', 'z'])
move_list = []
for path_file_tmp in path_file_tmps:
print("Path file tmp {}".format(path_file_tmp))
pos_tmp, G_tmp = self.move_data(path_file_tmp)
if keep_diameter:
mesh_object_tmp =base.Meshgrid(pos_tmp, G_tmp, diameter_set = self.mesh_objects.diameter)
else:
mesh_object_tmp =base.Meshgrid(pos_tmp, G_tmp)
assert not np.all(np.isnan(pos_tmp)) and not np.all(np.isinf(pos_tmp))
ip_moved, interest_points_index = mesh_object_tmp.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris, index = True)
basename = path_file_tmp.split("/")[-1]
basename = os.path.join("visualisation", basename)
if write_ply:
ply.write_ply(basename, ip_moved, ['x', 'y', 'z'])
interest_points_moved = self.mesh_objects.np_position_array[interest_points_index]
rep = self.calc_repeatability_reverse(interest_points, interest_points_moved,self.repeatbiliy_thresh)
print("Rep {}".format(rep))
move_list.append(rep)
return move_list
def roation_transformation(self, mesh_object, angle,write_ply = False, path = "base"):
#angle = [alpha, beta, gamma]
#Rotation Transformation
#Apply Rotation
#Only work on a copy of the original
mesh_object = copy.deepcopy(mesh_object)
rotations = angle
inverse_rotations = [-rotations[2],-rotations[1],-rotations[0]]
r = R.from_euler('xyz',rotations, degrees=True)
r_inverse = R.from_euler('zyx', inverse_rotations, degrees=True)
rotated_mesh = base.Meshgrid(r.apply(mesh_object.points))
interest_points = rotated_mesh.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
ply.write_ply(path + "_all", rotated_mesh.points, ['x','y','z'])
if write_ply:
ply.write_ply(path + "_int", interest_points, ['x','y','z'])
interest_points_original = r_inverse.apply(interest_points)
return interest_points_original
def gaus_noise_transformation(self, mesh_object, noise_level, absolute_noise_factor,write_ply = False, path = "base"):
#noise level: std of the gaussian additive noise
#absolute noise factor --> to make the absolute value of the noise in the range of the size
#--> factor which is multiplied with the diameter of the object
mesh_object = copy.deepcopy(mesh_object)
noise_x = np.random.normal(0,noise_level,mesh_object.points.shape[0]) * mesh_object.width * absolute_noise_factor
noise_y = np.random.normal(0,noise_level,mesh_object.points.shape[0]) * mesh_object.height * absolute_noise_factor
noise_z = np.random.normal(0,noise_level,mesh_object.points.shape[0]) * mesh_object.depth * absolute_noise_factor
noise = np.stack([noise_x,noise_y,noise_z]).T
noised_points = mesh_object.points + noise
if write_ply:
ply.write_ply(path + "_all", noised_points, ['x','y','z'])
noise_mesh = base.Meshgrid(noised_points)
interest_points_original = noise_mesh.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
ply.write_ply(path + "_ip", interest_points_original, ['x','y','z'])
return interest_points_original
def write_ply_with_labels(self, fname, labels=None):
if(labels is None):
labels = self.mem_labels if self.labels is None else self.labels
write_ply(fname, [self.points, labels], ['x', 'y', 'z', 'class'])
def partply(partpath, rot, opath):
T = np.dtype([("n", np.uint8), ("i0", np.int32), ('i1', np.int32),
('i2', np.int32)])
data = read_ply(os.path.join(partpath, 'point_sample', 'ply-10000.ply'))
label = np.loadtxt(os.path.join(partpath, 'point_sample',
'label-10000.txt'),
dtype=np.int32)
plypts = np.array(data['points'])[:, :3]
start = np.min(label)
end = np.max(label)
part_map = json.load(open(os.path.join(partpath, 'result_map.json'), 'r'))
partv_lst = []
partf_lst = []
for i in range(start, end + 1):
num = np.sum(label == i)
if num > 0:
pv = []
pf = []
pvn = 0
for name in part_map['%d' % i]['objs']:
pvi, pfi = read_obj(
os.path.join(partpath, 'objs', name + '.obj'))
pv.append(pvi)
pf.append(pfi + pvn)
pvn += pvi.shape[0]
if len(pv) > 1:
partv_lst.append(np.concatenate(pv, axis=0))
partf_lst.append(np.concatenate(pf, axis=0))
else:
partv_lst.append(pv[0])
partf_lst.append(pf[0])
partpts = np.concatenate(partv_lst, axis=0)
center, scale = tounit_param(partpts)
for parti in range(len(partv_lst)):
partptsi = partv_lst[parti]
partface = partf_lst[parti]
r = R.from_euler('y', 180, degrees=True)
pc = r.apply(partptsi).astype(np.float32)
pc -= center
pc /= scale
r = R.from_euler('y', rot, degrees=True)
pc = r.apply(pc).astype(np.float32)
face = np.zeros(shape=[len(partface)], dtype=T)
for i in range(len(partface)):
face[i] = (3, int(partface[i][0]), int(partface[i][1]),
int(partface[i][2]))
r = R.from_euler('x', 90, degrees=True)
pc = r.apply(pc).astype(np.float32)
rc = pd.DataFrame(
np.repeat(np.array([[255, 0, 0]], dtype=np.uint8),
partptsi.shape[0],
axis=0))
bc = pd.DataFrame(
np.repeat(np.array([[0, 0, 255]], dtype=np.uint8),
partptsi.shape[0],
axis=0))
pc = pd.DataFrame(pc)
partptsia = pd.concat([pc, rc], axis=1, ignore_index=True)
partptsib = pd.concat([pc, bc], axis=1, ignore_index=True)
write_ply(os.path.join(opath, 'p_%d_a.ply' % parti),
points=partptsia,
faces=pd.DataFrame(face),
color=True)
write_ply(os.path.join(opath, 'p_%d_b.ply' % parti),
points=partptsib,
faces=pd.DataFrame(face),
color=True)
return
def test_pipeline(self, write_ply = False, path = "base"):
#Original interest points
interest_points = self.mesh_objects.get_interest_points(sel_mod =self.sel_mod,\
sel_args = self.sel_args,neigh_flag = self.neigh_flag, neigh_args = self.neigh_args,\
k_harris = self.k_harris)
if write_ply:
path_all = os.path.join(path, "base_all")
path_tmp = os.path.join(path, "base_int")
ply.write_ply(path_all, self.mesh_objects.points, ['x','y', 'z'])
ply.write_ply(path_tmp, interest_points, ['x','y', 'z'])
#Angle repeatability
path_angle = os.path.join(path,"angle/")
print("Path angle {}".format(path_angle))
angle_repeat_list = []
angles = [[0,1,0],[0,5,0], [0,10,0], [0,15,0],[0,20,0],[0,30,0],[0,45,0],[0,75,0],[0,90,0],[0,120,0],[0,150,0],[0,180,0]]
for angle in angles:
angle = np.array(angle)
path_angle_tmp = path_angle + str(angle[1])
interest_points_rotated = self.roation_transformation(self.mesh_objects, angle,write_ply = write_ply, path=path_angle_tmp)
#Calculate repeatability
repeatability_value = self.calc_repeatability_reverse(interest_points, interest_points_rotated,self.repeatbiliy_thresh)
angle_repeat_list.append(repeatability_value)
print("Rep value angle{}".format(repeatability_value))
scale_list = []
scale_factors = [0.25, 0.5, 0.75, 0.9, 1.0, 1.1, 1.25, 1.5, 2.0, 3.0, 4.0]
path_scale = os.path.join(path,"scale/")
for scale_factor in scale_factors:
path_scale_tmp = path_scale + str(scale_factor)
interest_points_scales = self.scale_transformation(self.mesh_objects,scale_factor,write_ply = write_ply, path= path_scale_tmp)
repeatability_value_scale = self.calc_repeatability_reverse(interest_points, interest_points_scales,self.repeatbiliy_thresh)
print("Rep value scale{}".format(repeatability_value_scale))
scale_list.append(repeatability_value_scale)
translation = np.array([10.0, 20.0, -15.0])
path_transl = os.path.join(path,"translation/")
repeatability_value_transl= []
interest_points_translated = self.translation_transformation(self.mesh_objects, translation,write_ply= write_ply, path= path_transl)
repeatability_value_transl = self.calc_repeatability_reverse(interest_points, interest_points_translated,self.repeatbiliy_thresh)
print("Rep. value transl {}".format(repeatability_value_transl))
#Subsampling (grid resolution)
try:
grid_resolutions = [0.0001, 0.001,0.0025, 0.005, 0.0075, 0.01,0.05, 0.1]
subsampling_list = []
path_subs = os.path.join(path,"subsampling/")
for grid_resolution in grid_resolutions:
path_subs_tmp = path_subs + str(grid_resolution)
grid_size = self.mesh_objects.diameter * grid_resolution
interest_points_subsampled = self.grid_subsampling_transformation( self.mesh_objects, grid_size, write_ply=write_ply, path=path_subs_tmp)
repeatability_value_subsampled = self.calc_repeatability_reverse(interest_points, interest_points_subsampled, self.repeatbiliy_thresh)
print("Subsampled rep. value {}".format(repeatability_value_subsampled))
subsampling_list.append(repeatability_value_subsampled)
except Exception as e:
print("!!!!!!Subsampling failed!!!!!!!!!!!")
print(e)
#Gaussian noise
try:
noise_levels = [0.1, 0.25, 0.5, 0.75, 1.0,1.25 ,1.5,1.75, 2.0,2.5 ]
noise_list = []
absolute_noise_factor = 0.001
path_noise = os.path.join(path,"noise/")
for noise_level in noise_levels:
path_noise_tmp = path_noise + str(noise_level)
interest_points_noise = self.gaus_noise_transformation(self.mesh_objects, noise_level, absolute_noise_factor,write_ply = write_ply, path = path_noise_tmp)
repeatability_value_noise = self.calc_repeatability_reverse(interest_points, interest_points_noise, self.repeatbiliy_thresh)
noise_list.append(repeatability_value_noise)
print("Subsampled nosie. value {}".format(repeatability_value_noise))
else:
# Load Dragon point cloud
cloud_o_ply = read_ply(dragon_o_path)
cloud_p_ply = read_ply(dragon_p_path)
cloud_o = np.vstack((cloud_o_ply['x'], cloud_o_ply['y'], cloud_o_ply['z']))
cloud_p = np.vstack((cloud_p_ply['x'], cloud_p_ply['y'], cloud_p_ply['z']))
# Random transformation
apply_random_transfo = False
if apply_random_transfo:
np.random.seed(42)
t = np.random.randn(3) * 0.05
thetas = np.pi * np.random.rand(3)
R = RotMatrix(thetas)
cloud_p = t[:, None] + R @ cloud_o
write_ply('../data/test_perturbed', [cloud_p.T], ['x', 'y', 'z'])
# Optimization parameters
max_iter = 50 # Maximum iterations in main loop
dist_threshold = 0.1 # d_max
RMS_threshold = 1e-5 # Convergence threshold
kNeighbors = 20 # To compute PCA
eps = 1e-3 # Covariance matrix parameter (for Generalized-ICP only)
# Transformation estimation
Comparison = 0
if Comparison == 0:
# Run and compare all methods
plt.title("Convergence of the different methods")
algos = [
Algorithm('plane-to-plane'),
def pack(pnpath, partpath, spnobjpath, opath, id, angle):
partp = os.path.join(partpath, 'part_r%d' % angle)
imgp = os.path.dirname(spnobjpath)
data = read_ply(os.path.join(pnpath, 'point_sample', 'ply-10000.ply'))
label = np.loadtxt(os.path.join(pnpath, 'point_sample', 'label-10000.txt'),
dtype=np.int32)
plypts = np.array(data['points'])[:, :3]
start = np.min(label)
end = np.max(label)
cnt = 0
msklst = []
smsklst = []
ps = []
pstouch = []
r1 = R.from_euler('x', -90, degrees=True)
for i in range(start, end + 1):
pv = plypts[label == i, :3].astype(np.float32)
num = pv.shape[0]
if num > 0:
mskp = 'p_%d_b_msk0001.png' % (cnt)
msk = Image.open(os.path.join(partp, 'all_msk', mskp))
msk = np.array(msk).astype(np.float32) / 255.0
msk = msk[:, :, 2]
smsk = Image.open(os.path.join(partp, 'self_msk', mskp))
smsk = np.array(smsk).astype(np.float32) / 255.0
smsk = smsk[:, :, 2]
if np.sum(msk) > 9:
pstouch.append(pv)
cpath = os.path.join(partp, 'p_%d_b.ply' % cnt)
pts = read_ply(cpath)
pvp = np.array(pts['points'])[:, :3].astype(np.float32)
pvp = r1.apply(pvp)
ps.append(pvp)
smsklst.append(smsk)
msklst.append(msk)
cnt += 1
print(len(ps))
num = len(ps)
obblst = []
obbp = []
obbf = []
obbcnt = 0
for pts in ps:
obba = OBB.build_by_trimesh(pts)
obbb = OBB.build_from_points(pts)
if (obba is None) or ((obbb is not None) and
(obba.volume > obbb.volume)):
obbr = obbb
else:
obbr = obba
#print('size:',obba.volume);
obblst.append(obbr)
obbf.append(bf + obbcnt * 8)
obbcnt += 1
#print('obbf:',len(obbf));
#print('obbcnt:',obbcnt);
mm = []
for pi in range(num - 1):
for pj in range(pi + 1, num):
da, db = (
pstouch[pi],
pstouch[pj]) if obblst[pi].volume > obblst[pj].volume else (
pstouch[pj], pstouch[pi])
tree = scipy.spatial.KDTree(da)
dsta, idxa = tree.query(da, k=2)
dstb, idxb = tree.query(db, k=1)
if np.min(dstb) < np.mean(dsta[:, 1]):
mm.append(np.array([pi, pj], dtype=np.int32))
print('mm:', len(mm))
if len(mm) < 1:
return
img = Image.open(os.path.join(imgp, 'model_normalized_r%d_e.png' % angle))
img = np.array(img).astype(np.float32) / 255.0
h5fo = h5py.File(os.path.join(opath, id + '_r%d.h5' % angle), 'w')
h5fo.create_dataset("img",
data=img,
compression="gzip",
compression_opts=9)
packorigin(imgp, angle, h5fo)
h5fo.create_dataset("touch",
data=np.stack(mm, axis=0),
compression="gzip",
compression_opts=9)
msks = np.stack(msklst, axis=0)
h5fo.create_dataset("msk",
data=msks,
compression="gzip",
compression_opts=9)
smsks = np.stack(smsklst, axis=0)
h5fo.create_dataset("smsk",
data=smsks,
compression="gzip",
compression_opts=9)
obbk = []
for obb in obblst:
obbp.append(obb.points)
obbk.append(obb.tov)
obbv = np.concatenate(obbp, axis=0)
h5fo.create_dataset("box",
data=np.stack(obbk, axis=0),
compression="gzip",
compression_opts=9)
fidx = np.concatenate(obbf, axis=0)
T = np.dtype([("n", np.uint8), ("i0", np.int32), ('i1', np.int32),
('i2', np.int32)])
face = np.zeros(shape=[12 * len(obbf)], dtype=T)
for i in range(fidx.shape[0]):
face[i] = (3, fidx[i, 0], fidx[i, 1], fidx[i, 2])
print(mm, file=open(os.path.join(partpath, 'mm_r%d.txt' % angle), 'w'))
obox = os.path.join(partpath, 'box_r%d.ply' % angle)
write_ply(obox,
points=pd.DataFrame(obbv.astype(np.float32)),
faces=pd.DataFrame(face),
as_text=True)
h5fo.close()
prev_data = np.load(dir / pc_name)
if training_data :
np.save(dir / pc_name, np.concatenate((prev_data, np.vstack((points.T, labels[index*bunchsize:])).T)))
else :
np.save(dir / pc_name, np.concatenate((prev_data, points))
# If you are memory rich
else :
try:
points = np.loadtxt(pc_path, dtype=np.float32)
except:
continue
labels_path = RAW_PATH / (name + '.labels')
if labels_path.exists():
labels = np.loadtxt(labels_path, dtype=np.float32)
dir = VAL_PATH if '3' in name else TRAIN_PATH
if output_type=='ply' :
write_ply(dir / pc_name, [points, labels], 'x y z intensity red green blue class'.split(' '))
else :
np.save(dir / pc_name, np.vstack((points.T, labels)).T)
print(f'As {dir / pc_name}...')
else:
if output_type=='ply' :
write_ply(TEST_PATH / pc_name, points, 'x y z intensity red green blue'.split(' '))
else :
np.save(TEST_PATH / pc_name, points)
print('Done.')
import pattern
import ply
import projection
res = (800,600)
# screen upper-left phy coords, in.
s1 = (-0.2,-0.15)
# screen lower-righ phy coords, in.
s2 = (0.2, 0.15)
# screen distance
ds = 0.4
p=pattern.make_depths(res[0],res[1],1.0,3.0)
ply.write_ply('out.ply',p)
p=projection.map_to_screen(p,(0,0),res,s1,s2)
ply.write_ply('outm.ply',p)
p=projection.project_points(p,ds)
ply.write_ply('outp.ply',p)
