def generate_loss_2Dplots(self, axis , x_axis_param):
# Parameters to deal with:
# axis # This will decide the rotation or translation of point cloud about a particular axis. 'x' or 'y' or 'z'
# x_axis_param # This will decide either to rotate or translate the point cloud 'rotation' or 'translation'.
template_data = self.templates[self.template_idx,:,:].reshape((1,MAX_NUM_POINT,3)) # Extract the template and reshape it.
template_data = template_data[:,0:self.NUM_POINT,:]
loss = [] # Store the losses.
if x_axis_param == 'rotation':
angles = [] # Store the angles.
# Loop to find loss for various angles from -90 to 90.
for i in range(-90,91):
if axis == 'X':
gt_pose = np.array([[0.0, 0.0, 0.0, i*(np.pi/180), 0.0, 0.0]]) # New poses as per each index.
if axis == 'Y':
gt_pose = np.array([[0.0, 0.0, 0.0, 0.0, i*(np.pi/180), 0.0]]) # New poses as per each index.
if axis == 'Z':
gt_pose = np.array([[0.0, 0.0, 0.0, 0.0, 0.0, i*(np.pi/180)]]) # New poses as per each index.
source_data = helper.apply_transformation(template_data,gt_pose) # Generate Source Data.
final_pose, TRANSFORMATIONS, loss_i, predicted_data, transformed_source_data, _, _ = self.test_one_case(template_data, source_data) # Find final transformation by network.
loss.append(loss_i)
angles.append(i)
# helper.display_three_clouds(template_data[0],source_data[0],transformed_source_data,"Results")
plt.plot(angles, loss, linewidth=6)
plt.xlabel('Rotation about '+axis+',Y,Z-axes (in degrees)', fontsize=40)
plt.ylabel('Error in Pose (L2 Norm)', fontsize=40)
plt.ylim((-0.5,2))
plt.tick_params(labelsize=30)
plt.show()
if x_axis_param == 'translation':
position = [] # Store the angles.
# Loop to find loss for various angles from -90 to 90.
for i in range(-10,11):
if axis == 'X':
gt_pose = np.array([[i/10, 0.0, 0.0, 0.0, 0.0, 0.0]]) # New poses as per each index.
if axis == 'Y':
gt_pose = np.array([[0.0, i/10, 0.0, 0.0, 0.0, 0.0]]) # New poses as per each index.
if axis == 'Z':
gt_pose = np.array([[0.0, 0.0, i/10, 0.0, 0.0, 0.0]]) # New poses as per each index.
source_data = helper.apply_transformation(template_data,gt_pose) # Generate Source Data.
final_pose, TRANSFORMATIONS, loss_i, predicted_data, transformed_source_data, _, _ = self.test_one_case(template_data,source_data,MAX_LOOPS) # Find final transformation by network.
loss.append(np.sum(np.square(final_pose[0]-gt_pose[0]))/6) # Calculate L2 Norm between gt pose and predicted pose.
position.append(i/10.0)
# helper.display_three_clouds(template_data[0],source_data[0],transformed_source_data,"Results")
plt.plot(position, loss, linewidth=6)
plt.ylim((-0.5,2))
plt.xlabel('Translations about '+axis+'-axis', fontsize=40)
plt.ylabel('Error in Poses (L2 Norm)', fontsize=40)
plt.tick_params(labelsize=30)
plt.show()
def generate_stat_data(self, filename):
eval_poses = helper.read_poses(FLAGS.data_dict, FLAGS.eval_poses)
template_data = self.templates[self.template_idx,:,:].reshape((1,MAX_NUM_POINT,3))
TIME, ITR, Trans_Err, Rot_Err = [], [], [], []
for pose in eval_poses:
source_data = helper.apply_transformation(self.templates[self.template_idx,:,:],pose.reshape((-1,6)))
final_pose, _, _, _, _, elapsed_time, itr = self.test_one_case(source_data,template_data)
translation_error, rotational_error = self.find_errors(pose.reshape((-1,6)), final_pose)
TIME.append(elapsed_time)
ITR.append(itr)
Trans_Err.append(translation_error)
Rot_Err.append(rotational_error)
TIME_mean, ITR_mean, Trans_Err_mean, Rot_Err_mean = sum(TIME)/len(TIME), sum(ITR)/len(ITR), sum(Trans_Err)/len(Trans_Err), sum(Rot_Err)/len(Rot_Err)
TIME_var, ITR_var, Trans_Err_var, Rot_Err_var = np.var(np.array(TIME)), np.var(np.array(ITR)), np.var(np.array(Trans_Err)), np.var(np.array(Rot_Err))
import csv
with open(filename + '.csv','w') as csvfile:
csvwriter = csv.writer(csvfile)
for i in range(len(TIME)):
csvwriter.writerow([i, TIME[i], ITR[i], Trans_Err[i], Rot_Err[i]])
with open(filename+'.txt','w') as file:
file.write("Mean of Time: {}".format(TIME_mean))
file.write("Mean of Iterations: {}".format(ITR_mean))
file.write("Mean of Translation Error: {}".format(Trans_Err_mean))
file.write("Mean of Rotation Error: {}".format(Rot_Err_mean))
file.write("Variance in Time: {}".format(TIME_var))
file.write("Variance in Iterations: {}".format(ITR_var))
file.write("Variance in Translation Error: {}".format(Trans_Err_var))
file.write("Variance in Rotation Error: {}".format(Rot_Err_var))
def eval_one_epoch(self, model, poses, templates, BATCH_SIZE=32):
num_batches = int(poses.shape[0] / BATCH_SIZE)
total_loss = 0.0
weight = 4
with torch.no_grad():
for batch_idx in range(num_batches):
start_idx = batch_idx * BATCH_SIZE
end_idx = (batch_idx + 1) * BATCH_SIZE
template_data = templates[start_idx:end_idx]
source_data = helper.apply_transformation(
template_data, poses[0:BATCH_SIZE])
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
source_data = source_data - np.mean(
source_data, axis=1, keepdims=True)
source_data = torch.from_numpy(source_data).cuda().double()
template_data = torch.from_numpy(template_data).cuda().double()
loss = model(source_data, template_data, self.maxItr) * weight
total_loss += loss.item()
return total_loss / num_batches
def read_data():
# Output ->
# source: Torch tensor on CPU [Nx3]
# template: Torch tensor on CPU [Nx3]
# rotation_ab: Torch tensor on CPU [Nx3]
template = helper.loadData('train_data')
template = template[0, 0:1024, :].reshape(1, -1, 3)
poses = np.array(
[[0, 0, 0, 30 * (np.pi / 180), 40 * (np.pi / 180), 0 * (np.pi / 180)]])
source = helper.apply_transformation(template, poses)
return torch.from_numpy(source), torch.from_numpy(template)
def generate_icp_results(self, gt_pose): from icp import icp_test from scipy.spatial import KDTree M_given = self.templates[self.template_idx,:,:] S_given = helper.apply_transformation(M_given.reshape((1,-1,3)), gt_pose)[0] # S_given = S_given + np.random.normal(0,1,S_given.shape) # Noisy Data M_given = M_given[0:self.NUM_POINT,:] # template data S_given = S_given[0:self.NUM_POINT,:] # source data tree_M = KDTree(M_given) tree_M_sampled = KDTree(M_given[0:100,:]) final_pose, model_data, sensor_data, predicted_data, title, _, _ = icp_test(S_given[0:100,:], M_given, tree_M, M_given[0:100,:], tree_M_sampled, S_given, gt_pose.reshape((1,6)), self.MAX_LOOPS, self.ftol) self.find_errors(gt_pose, final_pose) helper.display_three_clouds(model_data, sensor_data, predicted_data, title)
def generate_results(self, ftol, gt_pose, swap_case): template_data = self.templates[self.template_idx,:,:].reshape((1,MAX_NUM_POINT,3)) # Extract the template and reshape it. template_data = template_data[:,0:self.NUM_POINT,:] source_data = helper.apply_transformation(template_data,gt_pose) # Generate Source Data. # source_data = source_data + np.random.normal(0,0.001,source_data.shape) # Noisy Data if swap_case: final_pose, TRANSFORMATIONS, loss_i, predicted_data, transformed_source_data, elapsed_time, itr = self.test_one_case(source_data, template_data) # Find final transformation by network. else: final_pose, TRANSFORMATIONS, loss_i, predicted_data, transformed_source_data, elapsed_time, itr = self.test_one_case(template_data, source_data) # Find final transformation by network. if not swap_case: title = "Actual T (Red->Green): " for i in range(len(gt_pose[0])): if i>2: title += str(round(gt_pose[0][i]*(180/np.pi),2)) else: title += str(gt_pose[0][i]) title += ', ' title += "\nPredicted T (Red->Blue): " for i in range(len(final_pose[0])): if i>2: title += str(round(final_pose[0,i]*(180/np.pi),3)) else: title += str(round(final_pose[0,i],3)) title += ', ' else: title = "Predicted Transformation: " for i in range(len(final_pose[0])): if i>2: title += str(round(final_pose[0,i]*(180/np.pi),3)) else: title += str(round(final_pose[0,i],3)) title += ', ' title += '\nElapsed Time: '+str(np.round(elapsed_time*1000,3))+' ms'+' & Iterations: '+str(itr) title += ' & Iterative Network' self.find_errors(gt_pose, final_pose) if swap_case: helper.display_three_clouds(source_data[0], template_data[0], transformed_source_data, title) else: helper.display_three_clouds(template_data[0], source_data[0], transformed_source_data, title)
def split_template_source(template_data,
batch_euler_poses,
NUM_POINT,
centroid_subtraction_switch,
ADD_NOISE,
S_RAND_POINTS,
SPARSE=1):
if np.random.random_sample() < S_RAND_POINTS:
#sparse:
if SPARSE == 1:
template_data = template_data[:, :(2 * NUM_POINT), ...]
elif SPARSE == 2:
template_data = template_data[:, :(4 * NUM_POINT), ...]
template_data = helper.select_random_points(template_data,
(2 * NUM_POINT))
source_data = template_data[:, NUM_POINT:(2 * NUM_POINT), ...]
template_data = template_data[:, :NUM_POINT, ...]
else:
source_data = template_data[:, :(NUM_POINT), ...]
template_data = template_data[:, :NUM_POINT, ...]
source_data = helper.apply_transformation(
source_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
if centroid_subtraction_switch:
source_data = source_data - np.mean(source_data, axis=1, keepdims=True)
# template_data = template_data - np.mean(template_data, axis=1, keepdims=True)
if np.random.random_sample() < ADD_NOISE:
source_data = helper.add_noise(source_data)
# Only chose limited number of points from the source and template data.
source_data = source_data[:, 0:NUM_POINT, :]
template_data = template_data[:, 0:NUM_POINT, :]
# # To visualize the source and point clouds:
# if display_ptClouds:
# helper.display_clouds_data(source_data[0])
# helper.display_clouds_data(template_data[0])
return template_data, source_data
def generate_loss_3Dplots(self, axis, x_axis_param):
# Parameters to deal with:
# axis This will decide the rotation or translation of point cloud about a particular axis. 'x' or 'y' or 'z'
# x_axis_param This will decide either to rotate or translate the point cloud 'rotation' or 'translation'.
template_data = self.templates[self.template_idx,:,:].reshape((1,MAX_NUM_POINT,3)) # Extract the template and reshape it.
template_data = template_data[:,0:self.NUM_POINT,:]
loss = []
angles_x = []
angles_y = [] # Store the losses.
if x_axis_param == 'rotation':
# Loop to find loss for various angles from -90 to 90.
for i in range(-90,91):
print('I: {}'.format(i))
for j in range(-90,91):
if axis == 'XY':
gt_pose = np.array([[0.0, 0.0, 0.0, i*(np.pi/180), j*(np.pi/180), 0.0]]) # New poses as per each index.
if axis == 'YZ':
gt_pose = np.array([[0.0, 0.0, 0.0, 0.0, i*(np.pi/180), j*(np.pi/180)]]) # New poses as per each index.
if axis == 'XZ':
gt_pose = np.array([[0.0, 0.0, 0.0, i*(np.pi/180), 0.0, j*(np.pi/180)]]) # New poses as per each index.
source_data = helper.apply_transformation(template_data,gt_pose) # Generate Source Data.
final_pose, TRANSFORMATIONS, loss_i, predicted_data, transformed_source_data, _, _ = self.test_one_case(template_data, source_data) # Find final transformation by network.
loss.append(loss_i)
angles_x.append(i)
angles_y.append(j)
# helper.display_three_clouds(template_data[0],source_data[0],transformed_source_data,"Results")
fig = plt.figure()
ax = fig.add_subplot(111,projection='3d')
ax.scatter(angles_x,angles_y,loss)
ax.set_xlabel('Rotation Angle about '+axis[0]+'-axis', fontsize=25, labelpad=25)
ax.set_ylabel('Rotation Angle about '+axis[1]+'-axis', fontsize=25, labelpad=25)
ax.set_zlabel('Error in Poses (L2 Norm)', fontsize=25, labelpad=25)
ax.tick_params(labelsize=25)
plt.show()
def eval_network(sess, ops, templates, poses, pairs):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
loss_sum = 0 # Total Loss in each batch.
num_batches = int(poses.shape[0] /
BATCH_SIZE) # Number of batches in an epoch.
print('Number of batches to be executed: {}'.format(num_batches))
# Store time taken, no of iterations, translation error and rotation error for registration.
TIME, ITR, Trans_Err, Rot_Err = [], [], [], []
idxs_5_5, idxs_10_1, idxs_20_2 = [], [], []
if FLAGS.use_noise_data:
print(FLAGS.data_dict)
templates, sources = helper.read_noise_data(FLAGS.data_dict)
print(templates.shape, sources.shape)
for fn in range(num_batches):
start_idx = fn * BATCH_SIZE # Start index of poses.
end_idx = (fn + 1) * BATCH_SIZE # End index of poses.
if FLAGS.use_noise_data:
template_data = np.copy(templates[fn, :, :]).reshape(
1, -1, 3) # As template_data is changing.
source_data = np.copy(sources[fn, :, :]).reshape(1, -1, 3)
batch_euler_poses = poses[
start_idx:end_idx] # Extract poses for batch training.
else:
# template_idx = pairs[fn,1]
template_data = np.copy(templates[0, :, :]).reshape(
1, -1, 3) # As template_data is changing.
batch_euler_poses = poses[
start_idx:end_idx] # Extract poses for batch training.
source_data = helper.apply_transformation(
template_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
template_data = template_data[:, 0:NUM_POINT, :]
source_data = source_data[:, 0:NUM_POINT, :]
# Just to visualize the data.
TEMPLATE_DATA = np.copy(
template_data) # Store the initial template to visualize results.
SOURCE_DATA = np.copy(
source_data) # Store the initial source to visualize results.
# Subtract the Centroids from the Point Clouds.
if centroid_subtraction_switch:
source_data = source_data - np.mean(
source_data, axis=1, keepdims=True)
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
TRANSFORMATIONS = np.identity(
4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS, BATCH_SIZE, 1).reshape(
BATCH_SIZE, 4,
4) # Intialize identity matrices of size equal to batch_size
# previous_pose = np.array([0,0,0,1,0,0,0])
previous_T = np.eye(4)
start = time.time() # Log start time.
# Iterations for pose refinement.
for loop_idx in range(MAX_LOOPS):
for network_itr in range(7):
# Feed the placeholders of Network19 with template data and source data.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training
}
predicted_transformation = sess.run(
[ops['predicted_transformation']], feed_dict=feed_dict
) # Ask the network to predict the pose.
# Apply the transformation on the source data and multiply it to transformation matrix obtained in previous iteration.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
# Display Results after each iteration.
if display_poses_in_itr:
print(predicted_transformation[0, 0:3])
print(predicted_transformation[0, 3:7] * (180 / np.pi))
if display_ptClouds_in_itr:
helper.display_clouds_data(template_data[0])
# Feed the placeholders of Network_L with source data and template data obtained from N-Iterations.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training
}
# Ask the network to predict transformation, calculate loss using distance between actual points.
predicted_transformation = sess.run(
[ops['predicted_transformation']], feed_dict=feed_dict)
# Apply the final transformation on the source data and multiply it with the transformation matrix obtained from N-Iterations.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
if check_convergenceT(previous_T, TRANSFORMATIONS[0]):
break
else:
previous_T = np.copy(TRANSFORMATIONS[0])
end = time.time() # Log end time.
final_pose = helper.find_final_pose_inv(
TRANSFORMATIONS
) # Find the final pose (translation, orientation (euler angles in degrees)) from transformation matrix.
final_pose[0,
0:3] = final_pose[0, 0:3] + np.mean(SOURCE_DATA, axis=1)[0]
translation_error, rotational_error = find_errors(
batch_euler_poses[0], final_pose[0])
TIME.append(end - start)
ITR.append(loop_idx + 1)
Trans_Err.append(translation_error)
Rot_Err.append(rotational_error)
if rotational_error < 20 and translation_error < 0.2:
if rotational_error < 10 and translation_error < 0.1:
if rotational_error < 5 and translation_error < 0.05:
idxs_5_5.append(fn)
idxs_10_1.append(fn)
idxs_20_2.append(fn)
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
# Display Loss Value.
# helper.display_three_clouds(TEMPLATE_DATA[0],SOURCE_DATA[0],template_data[0],"")
print("Batch: {} & time: {}, iteration: {}".format(
fn, end - start, loop_idx + 1))
log = {
'TIME': TIME,
'ITR': ITR,
'Trans_Err': Trans_Err,
'Rot_Err': Rot_Err,
'idxs_5_5': idxs_5_5,
'idxs_10_1': idxs_10_1,
'idxs_20_2': idxs_20_2,
'num_batches': num_batches
}
helper.log_test_results(FLAGS.log_dir, FLAGS.filename, log)
def eval_one_epoch(sess, ops, eval_writer, templates, poses):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
#templates = helper.shuffle_templates(templates)
#poses = helper.shuffle_poses(poses)
loss_sum = 0
#poses = poses[0:4000,:] # Total Loss in each batch.
num_batches = int(poses.shape[0]/BATCH_SIZE) # Number of batches in an epoch.
#num_batches=2
for fn in range(num_batches):
#shuffled_poses = helper.shuffle_poses(poses)
start_idx = fn*BATCH_SIZE # Start index of poses.
end_idx = (fn+1)*BATCH_SIZE # End index of poses.
#template_data = np.copy(templates[start_idx:end_idx])
template_data = np.copy(templates[0,:,:]).reshape(1,-1,3)
template_data = np.tile(template_data, (BATCH_SIZE, 1, 1))
batch_euler_poses = poses[0:BATCH_SIZE,:] # Extract poses for batch training.
source_data = helper.apply_transformation(template_data, batch_euler_poses) # Apply the poses on the templates to get source data.
if centroid_subtraction_switch:
source_data = source_data - np.mean(source_data, axis=1, keepdims=True)
template_data = template_data - np.mean(template_data, axis=1, keepdims=True)
if FLAGS.use_partial_data:
complete_source_data = np.copy(source_data)
source_data = helper.find_partial_data(complete_source_data)
template_data = helper.find_partial_data(template_data)
# Chose Random Points from point clouds for training.
if np.random.random_sample()<0.0:
source_data = helper.select_random_points(source_data, NUM_POINT) # 30% probability that source data has different points than template
else:
source_data = source_data[:,0:NUM_POINT,:]
if np.random.random_sample()<ADD_NOISE:
source_data = helper.add_noise(source_data)
# Only chose limited number of points from the source and template data.
source_data = source_data[:,0:NUM_POINT,:]
template_data = template_data[:,0:NUM_POINT,:]
template_voxel = helper.voxelization(template_data, size=32)
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
TRANSFORMATIONS = np.identity(4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS,BATCH_SIZE,1).reshape(BATCH_SIZE,4,4) # Intialize identity matrices of size equal to batch_size
# Iterations for pose refinement.
for loop_idx in range(MAX_LOOPS-1):
source_voxel = helper.voxelization(source_data, size=32)
# 4a
# Feed the placeholders of Network with template data and source data.
feed_dict = {ops['source_pointclouds_pl']: source_voxel,
ops['template_pointclouds_pl']: template_voxel,
ops['is_training_pl']: is_training}
predicted_transformation = sess.run([ops['predicted_transformation']], feed_dict=feed_dict) # Ask the network to predict the pose.
# 4b,4c
# Apply the transformation on the template data and multiply it to transformation matrix obtained in previous iteration.
if FLAGS.use_partial_data:
TRANSFORMATIONS, complete_source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, complete_source_data)
source_data = helper.find_partial_data(complete_source_data)
else:
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, source_data)
# Display Results after each iteration.
if display_poses_in_itr:
print(predicted_transformation[0,0:3])
print(predicted_transformation[0,3:7]*(180/np.pi))
if display_ptClouds_in_itr:
helper.display_clouds_data(source_data[0])
source_voxel = helper.voxelization(source_data, size=32)
# Feed the placeholders of Network with source data and template data obtained from N-Iterations.
feed_dict = {ops['source_pointclouds_pl']: source_voxel,
ops['template_pointclouds_pl']: template_voxel,
ops['transformation_pl']: TRANSFORMATIONS,
ops['gt_transformation_pl']: helper.pose2mat_inv(batch_euler_poses),
ops['is_training_pl']: is_training}
# Ask the network to predict transformation, calculate loss using distance between actual points.
summary, step, loss_val, predicted_transformation = sess.run([ops['merged'], ops['step'], ops['loss'], ops['predicted_transformation']], feed_dict=feed_dict)
eval_writer.add_summary(summary, step) # Add all the summary to the tensorboard.
# Apply the final transformation on the template data and multiply it with the transformation matrix obtained from N-Iterations.
if FLAGS.use_partial_data:
TRANSFORMATIONS, complete_source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, complete_source_data)
source_data = helper.find_partial_data(complete_source_data)
else:
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, source_data)
final_pose = helper.find_final_pose_inv(TRANSFORMATIONS) # Find the final pose (translation, orientation (euler angles in degrees)) from transformation matrix.
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(batch_euler_poses[0,0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0,0:3].tolist()))
print('Ground Truth Orientation: {}'.format((batch_euler_poses[0,3:6]*(180/np.pi)).tolist()))
print('Predicted Orientation: {}'.format((final_pose[0,3:6]*(180/np.pi)).tolist()))
# Display Loss Value.
print("Batch: {}, Loss: {}\r".format(fn, loss_val),end='')
# Add loss for each batch.
loss_sum += loss_val
print('\n')
log_string('Eval Mean loss: %f' % (loss_sum/num_batches)) # Store and display mean loss of epoch.
def test_one_epoch(sess, ops_L, templates, shuffled_poses, saver, model_path):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops_L: Dictionary for tensors of Network_L
# ops19: Dictionary for tensors of Network19
# templates: Training Point Cloud data.
# poses: Training pose data.
# saver: To restore the weights.
# model_path: Path of log directory.
saver.restore(sess, model_path) # Restore the weights of trained network.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
swap_case = False
templates = helper.process_templates('templates')
template_data = np.zeros((BATCH_SIZE, MAX_NUM_POINT,
3)) # Extract Templates for batch training.
for i in range(BATCH_SIZE):
template_data[i, :, :] = np.copy(templates[1, :, :])
batch_euler_poses = shuffled_poses[0].reshape(
(1, 6)) # Extract poses for batch training.
# Self defined test case.
batch_euler_poses[0] = [
0.5, 0.0, 0.2, 50 * (np.pi / 180), 0 * (np.pi / 180),
10 * (np.pi / 180)
]
source_data = helper.apply_transformation(
template_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
# Only chose limited number of points from the source and template data.
template_data = template_data[:, 0:NUM_POINT, :]
source_data = source_data[:, 0:NUM_POINT, :]
if swap_case:
source_data, template_data = template_data, source_data # Swap the template and source.
transformation_template2source = helper.transformation(
batch_euler_poses)
transformation_source2template = np.linalg.inv(
transformation_template2source[0])
[euler_z, euler_y,
euler_x] = t3d.mat2euler(transformation_source2template[0:3, 0:3],
'szyx')
trans_x = transformation_source2template[0, 3]
trans_y = transformation_source2template[1, 3]
trans_z = transformation_source2template[2, 3]
pose_source2template = [
trans_x, trans_y, trans_z, euler_x * (18 / np.pi),
euler_y * (180 / np.pi), euler_z * (180 / np.pi)
]
batch_euler_poses[0] = pose_source2template
TEMPLATE_DATA = np.copy(
template_data) # Store the initial template to visualize results.
SOURCE_DATA = np.copy(
source_data) # Store the initial source to visualize results.
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
# Subtract the Centroids from the Point Clouds.
if centroid_subtraction_switch:
source_data, template_data, centroid_translation_pose = helper.centroid_subtraction(
source_data, template_data)
TRANSFORMATIONS = np.identity(
4) # Initialize identity transformation matrix.
TRANSFORMATIONS = np.matlib.repmat(TRANSFORMATIONS, BATCH_SIZE, 1).reshape(
BATCH_SIZE, 4,
4) # Intialize identity matrices of size equal to batch_size
# Feed the placeholders of Network_L with source data and template data obtained from N-Iterations.
feed_dict = {
ops_L['source_pointclouds_pl']: source_data,
ops_L['template_pointclouds_pl']: template_data,
ops_L['is_training_pl']: is_training
}
# Ask the network to predict transformation, calculate loss using distance between actual points.
import time
start = time.time()
step, predicted_transformation = sess.run(
[ops_L['step'], ops_L['predicted_transformation']],
feed_dict=feed_dict)
end = time.time()
print(end - start)
# Apply the final transformation on the template data and multiply it with the transformation matrix obtained from N-Iterations.
TRANSFORMATIONS, template_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, template_data)
if centroid_subtraction_switch: # If centroid is subtracted then apply the centorid translation back to point clouds.
TRANSFORMATIONS, template_data = helper.transformation_quat2mat(
centroid_translation_pose, TRANSFORMATIONS, template_data)
final_pose = helper.find_final_pose(TRANSFORMATIONS)
if not swap_case:
title = "Actual T (Red->Green): "
for i in range(len(batch_euler_poses[0])):
if i > 2:
title += str(round(batch_euler_poses[0][i] * (180 / np.pi), 2))
else:
title += str(batch_euler_poses[0][i])
title += ', '
title += "\nPredicted T (Red->Blue): "
for i in range(len(final_pose[0])):
if i > 2:
title += str(round(final_pose[0, i] * (180 / np.pi), 3))
else:
title += str(round(final_pose[0, i], 3))
title += ', '
else:
title = "Predicted T (Red->Blue): "
for i in range(len(final_pose[0])):
if i > 2:
title += str(round(final_pose[0, i] * (180 / np.pi), 3))
else:
title += str(round(final_pose[0, i], 3))
title += ', '
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
helper.display_three_clouds(TEMPLATE_DATA[0], SOURCE_DATA[0],
template_data[0], title)
print("Loss: {}".format(loss_val))
def eval_one_epoch(sess, ops_L, eval_writer, templates, poses):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops_L: Dictionary for tensors of Network_L
# ops19: Dictionary for tensors of Network19
# templates: Training Point Cloud data.
# poses: Training pose data.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
#templates = helper.shuffle_templates(templates)
#poses = helper.shuffle_poses(poses)
loss_sum = 0 # Total Loss in each batch.
num_batches = int(poses.shape[0] /
BATCH_SIZE) # Number of batches in an epoch.
for fn in range(num_batches):
start_idx = fn * BATCH_SIZE # Start index of poses.
end_idx = (fn + 1) * BATCH_SIZE # End index of poses.
template_data = np.copy(templates[2, :, :]).reshape(1, -1, 3)
template_data = np.tile(template_data, (BATCH_SIZE, 1, 1))
batch_euler_poses = poses[start_idx:
end_idx] # Extract poses for batch training.
source_data = helper.apply_transformation(
template_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
# Chose Random Points from point clouds for training.
if np.random.random_sample() < 0:
source_data = helper.select_random_points(
source_data, NUM_POINT
) # 30% probability that source data has different points than template
else:
source_data = source_data[:, 0:NUM_POINT, :]
if np.random.random_sample() < 0:
source_data = helper.add_noise(
source_data) # 50% chance of having noise in training data.
# Only chose limited number of points from the source and template data.
template_data = template_data[:, 0:NUM_POINT, :]
source_data = source_data[:, 0:NUM_POINT, :]
# Subtract the Centroids from the Point Clouds.
if centroid_subtraction_switch:
source_data = source_data - np.mean(
source_data, axis=1, keepdims=True)
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
# Feed the placeholders of Network_L with source data and template data obtained from N-Iterations.
feed_dict = {
ops_L['source_pointclouds_pl']: source_data,
ops_L['template_pointclouds_pl']: template_data,
ops_L['is_training_pl']: is_training
}
# Ask the network to predict transformation, calculate loss using distance between actual points.
summary, step, loss_val, predicted_transformation = sess.run(
[
ops_L['merged'], ops_L['step'], ops_L['loss'],
ops_L['predicted_transformation']
],
feed_dict=feed_dict)
eval_writer.add_summary(
summary, step) # Add all the summary to the tensorboard.
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
# Display Loss Value.
print("Batch: {} & Loss: {}\r".format(fn, loss_val), end='')
# Add loss for each batch.
loss_sum += loss_val
print('\n')
log_string(
'Eval Mean loss: %f' %
(loss_sum / num_batches)) # Store and display mean loss of epoch.
def run():
NUM_POINT = FLAGS.num_point
if not use_noise_data:
templates = helper.loadData(FLAGS.data_dict)
pairs = helper.read_pairs(FLAGS.data_dict, FLAGS.pairs_file)
else:
templates, sources = helper.read_noise_data(FLAGS.data_dict)
# templates = helper.loadData(FLAGS.data_dict)
eval_poses = helper.read_poses(
FLAGS.data_dict,
FLAGS.eval_poses) # Read all the poses data for evaluation.
eval_poses = eval_poses[0:1, :]
num_batches = eval_poses.shape[0]
TIME, ITR, Trans_Err, Rot_Err = [], [], [], []
idxs_5_5, idxs_10_1, idxs_20_2 = [], [], []
counter = 0
for fn, gt_pose in enumerate(eval_poses):
if fn > 0:
break
if not use_noise_data:
# template_idx = pairs[fn,1]
template_idx = 0
M_given = templates[template_idx, :, :]
S_given = helper.apply_transformation(M_given.reshape((1, -1, 3)),
gt_pose.reshape((1, 6)))[0]
else:
M_given = templates[fn, :, :]
S_given = sources[fn, :, :]
# M_given = np.loadtxt('template_car_itr.txt')
# S_given = np.loadtxt('source_car_itr.txt')
# helper.display_clouds_data(M_given)
# helper.display_clouds_data(S_given)
# To generate point cloud for Xueqian: For CAD model figures
# gt_pose = np.array([[0.5,0.2,0.4,40*(np.pi/180),20*(np.pi/180),30*(np.pi/180)]])
# templates = helper.loadData('unseen_data')
# gt_pose = np.array([[-0.3,-0.7,0.4,-34*(np.pi/180),31*(np.pi/180),-27*(np.pi/180)]])
# gt_pose = np.array([[0.5929,-0.0643,-0.961,0.4638,-0.3767,-0.6253]])
# M_given = templates[48,:,:]
# S_given = helper.apply_transformation(M_given.reshape(1,-1,3),gt_pose)
# S_given = helper.add_noise(S_given)
# S_given = S_given[0]
M_given = M_given[0:NUM_POINT, :] # template data
S_given = S_given[0:NUM_POINT, :] # source data
tree_M = KDTree(M_given)
tree_M_sampled = KDTree(M_given[0:100, :])
final_pose, model_data, sensor_data, predicted_data, _, time_elapsed, itr = icp.icp_test(
S_given[0:100, :], M_given,
tree_M, M_given[0:100, :], tree_M_sampled, S_given,
gt_pose.reshape((1, 6)), 100, FLAGS.threshold)
translation_error, rotational_error = find_errors(
gt_pose[0], final_pose[0])
print(translation_error, rotational_error)
TIME.append(time_elapsed)
ITR.append(itr)
Trans_Err.append(translation_error)
Rot_Err.append(rotational_error)
if rotational_error < 20 and translation_error < 0.2:
if rotational_error < 10 and translation_error < 0.1:
if rotational_error < 5 and translation_error < 0.05:
idxs_5_5.append(fn)
idxs_10_1.append(fn)
idxs_20_2.append(fn)
print('Batch: {}, Iterations: {}, Time: {}'.format(
counter, itr, time_elapsed))
# counter += 1
# helper.display_three_clouds(M_given, S_given, predicted_data, "")
# np.savetxt('template_piano.txt',M_given)
# np.savetxt('source_piano.txt',S_given)
# np.savetxt('predicted_piano.txt',predicted_data)
log = {
'TIME': TIME,
'ITR': ITR,
'Trans_Err': Trans_Err,
'Rot_Err': Rot_Err,
'idxs_5_5': idxs_5_5,
'idxs_10_1': idxs_10_1,
'idxs_20_2': idxs_20_2,
'num_batches': num_batches
}
helper.log_test_results(FLAGS.log_dir, FLAGS.filename, log)
def test_one_epoch(sess, ops, templates, poses, saver, model_path):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
# saver: To restore the weights.
# model_path: Path of log directory.
saver.restore(sess, model_path) # Restore the weights of trained network.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
swap_case = False
MAX_LOOPS = 4
template_data = np.zeros((BATCH_SIZE, MAX_NUM_POINT,
3)) # Extract Templates for batch training.
template_data[0] = np.copy(templates[FLAGS.template_idx, :, :])
batch_euler_poses = poses[0].reshape(
(1, 6)) # Extract poses for batch training.
# Define test case.
batch_euler_poses[0] = [
0.4, 0.5, 0.1, 10 * (np.pi / 180), 20 * (np.pi / 180),
20 * (np.pi / 180)
]
source_data = helper.apply_transformation(
template_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
# Chose Random Points from point clouds for training.
if np.random.random_sample() < 0:
source_data = helper.select_random_points(
source_data, NUM_POINT
) # probability that source data has different points than template
else:
source_data = source_data[:, 0:NUM_POINT, :]
# Add noise to source point cloud.
if np.random.random_sample() < 1.0:
source_data = helper.add_noise(source_data)
# Only choose limited number of points from the source and template data.
source_data = source_data[:, 0:NUM_POINT, :]
template_data = template_data[:, 0:NUM_POINT, :]
TEMPLATE_DATA = np.copy(
template_data) # Store the initial template to visualize results.
SOURCE_DATA = np.copy(
source_data) # Store the initial source to visualize results.
# Subtract the Centroids from the Point Clouds.
if centroid_subtraction_switch:
source_data = source_data - np.mean(source_data, axis=1, keepdims=True)
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
TRANSFORMATIONS = np.identity(
4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS, BATCH_SIZE, 1).reshape(
BATCH_SIZE, 4,
4) # Intialize identity matrices of size equal to batch_size
# Store the transformed point clouds after each iteration.
ITR = np.zeros((MAX_LOOPS, template_data.shape[0], template_data.shape[1],
template_data.shape[2]))
# Iterations for pose refinement.
for loop_idx in range(MAX_LOOPS - 1):
# 4a
# Feed the placeholders of Network with template data and source data.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training
}
predicted_transformation = sess.run(
[ops['predicted_transformation']],
feed_dict=feed_dict) # Ask the network to predict the pose.
#print (predicted_transformation[0])
# 4b,4c
# Apply the transformation on the template data and multiply it to transformation matrix obtained in previous iteration.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
# Display Results after each iteration.
if display_poses_in_itr:
print(predicted_transformation[0, 0:3])
print(predicted_transformation[0, 3:7] * (180 / np.pi))
if display_ptClouds_in_itr:
helper.display_clouds_data(source_data[0])
ITR[loop_idx, :, :, :] = source_data
# Feed the placeholders of Network with source data and template data obtained from N-Iterations.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training
}
# Ask the network to predict transformation, calculate loss using distance between actual points.
step, predicted_transformation = sess.run(
[ops['step'], ops['predicted_transformation']], feed_dict=feed_dict)
# Apply the final transformation on the template data and multiply it with the transformation matrix obtained from N-Iterations.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
final_pose = helper.find_final_pose_inv(TRANSFORMATIONS)
final_pose[0, 0:3] = final_pose[0, 0:3] + np.mean(SOURCE_DATA, axis=1)[0]
title = "Actual T (Red->Green): "
for i in range(len(batch_euler_poses[0])):
if i > 2:
title += str(round(batch_euler_poses[0][i] * (180 / np.pi), 2))
else:
title += str(batch_euler_poses[0][i])
title += ', '
title += "\nPredicted T (Red->Blue): "
for i in range(len(final_pose[0])):
if i > 2:
title += str(round(final_pose[0, i] * (180 / np.pi), 3))
else:
title += str(round(final_pose[0, i], 3))
title += ', '
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
helper.display_three_clouds(TEMPLATE_DATA[0], SOURCE_DATA[0],
source_data[0], title)
def train_one_epoch(sess, ops, train_writer, templates, poses):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
print(datetime.now())
is_training = True
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
poses = poses[0:5070, :]
poses = helper.shuffle_poses(poses) # Shuffle Poses.
loss_sum = 0 # Total Loss in each batch.
num_batches = int(templates.shape[0] /
BATCH_SIZE) # Number of batches in an epoch.
# Training for each batch.
for fn in range(num_batches):
start_idx = fn * BATCH_SIZE # Start index of poses.
end_idx = (fn + 1) * BATCH_SIZE # End index of poses.
template_data = np.copy(templates[start_idx:end_idx])
batch_euler_poses = poses[start_idx:
end_idx] # Extract poses for batch training.
if SPARSE_SAMPLING > 0:
template_data, source_data = helper.split_template_source(
template_data,
batch_euler_poses,
NUM_POINT,
centroid_subtraction_switch,
ADD_NOISE,
S_RAND_POINTS,
SPARSE=SPARSE_SAMPLING)
else:
if template_random_pose:
template_data = helper.apply_transformation(
template_data, batch_euler_poses / 2)
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
source_data = helper.apply_transformation(
template_data, batch_euler_poses
) # Apply the poses on the templates to get source data.
if centroid_subtraction_switch:
source_data = source_data - np.mean(
source_data, axis=1, keepdims=True)
# template_data = template_data - np.mean(template_data, axis=1, keepdims=True)
# Chose Random Points from point clouds for training.
if np.random.random_sample() < S_RAND_POINTS:
template_data = helper.select_random_points(
template_data, NUM_POINT)
source_data = helper.select_random_points(
source_data, NUM_POINT
) # 50% probability that source data has different points than template
else:
source_data = source_data[:, 0:NUM_POINT, :]
if np.random.random_sample() < ADD_NOISE:
source_data = helper.add_noise(source_data)
# Only chose limited number of points from the source and template data.
source_data = source_data[:, 0:NUM_POINT, :]
template_data = template_data[:, 0:NUM_POINT, :]
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
if FLAGS.add_occlusions > 0.0:
source_data = helper.add_occlusions(source_data,
FLAGS.add_occlusions)
TRANSFORMATIONS = np.identity(
4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS, BATCH_SIZE, 1).reshape(
BATCH_SIZE, 4,
4) # Intialize identity matrices of size equal to batch_size
# Iterations for pose refinement.
for loop_idx in range(MAX_LOOPS - 1):
# 4a
# Feed the placeholders of Network with template data and source data.
if ADD_NOISE_MODEL:
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training,
ops['is_training_pl_1']: False,
ops['labels12']: np.ones([BATCH_SIZE, NUM_POINT]),
ops['add_noise']: np.zeros([BATCH_SIZE, NUM_POINT, 3])
}
else:
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training,
ops['is_training_pl_1']: False,
ops['labels12']: np.ones([BATCH_SIZE, NUM_POINT])
}
if bool(FLAGS.train_single
): #train every iteration, or only on the Nth iter
summary, step, _, loss_val, predicted_transformation = sess.run(
[
ops['merged'], ops['step'], ops['train_op'],
ops['loss'], ops['predicted_transformation']
],
feed_dict=feed_dict)
else:
predicted_transformation = sess.run(
[ops['predicted_transformation']], feed_dict=feed_dict
) # Ask the network to predict the pose.
# 4b,4c
# Apply the transformation on the template data and multiply it to transformation matrix obtained in previous iteration.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
# Display Results after each iteration.
if display_poses_in_itr:
print(predicted_transformation[0, 0:3])
print(predicted_transformation[0, 3:7] * (180 / np.pi))
if display_ptClouds_in_itr:
helper.display_clouds_data(source_data[0])
# Feed the placeholders of Network with source data and template data obtained from N-Iterations.
if ADD_NOISE_MODEL:
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training,
ops['is_training_pl_1']: False,
ops['labels12']: np.ones([BATCH_SIZE, NUM_POINT]),
ops['add_noise']: np.zeros([BATCH_SIZE, NUM_POINT, 3])
}
else:
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training,
ops['is_training_pl_1']: False,
ops['labels12']: np.ones([BATCH_SIZE, NUM_POINT])
}
# Ask the network to predict transformation, calculate loss using distance between actual points, calculate & apply gradients for Network and copy the weights to Network19.
summary, step, _, loss_val, predicted_transformation = sess.run(
[
ops['merged'], ops['step'], ops['train_op'], ops['loss'],
ops['predicted_transformation']
],
feed_dict=feed_dict)
train_writer.add_summary(
summary, step) # Add all the summary to the tensorboard.
# Apply the final transformation on the template data and multiply it with the transformation matrix obtained from N-Iterations.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
# final_pose = helper.find_final_pose_inv(TRANSFORMATIONS) # Find the final pose (translation, orientation (euler angles in degrees)) from transformation matrix.
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
# print(batch_euler_poses[0,0:3],batch_euler_poses[0,3:6]*(180/np.pi))
# print(final_pose[0,0:3],final_pose[0,3:6]*(180/np.pi))
# Display Loss Value.
print("Batch: {} & Loss: {}\r".format(fn, loss_val), end='')
# Add loss for each batch.
loss_sum += loss_val
print('\n')
log_string(
'Train Mean loss: %f\n' %
(loss_sum / num_batches)) # Store and display mean loss of epoch.
templates = helper.process_templates('multi_model_templates')
template_idx = 201
NUM_POINT = 1024
MAX_LOOPS = 500
M_given = templates[template_idx, :, :]
x_trans = 0.5
y_trans = 0.5
z_trans = 0.5
x_rot = 45 * (np.pi / 180)
y_rot = 45 * (np.pi / 180)
z_rot = 45 * (np.pi / 180)
gt_pose = np.array([[x_trans, y_trans, z_trans, x_rot, y_rot,
z_rot]]) # Pose: Source to Template
S_given = helper.apply_transformation(M_given.reshape((1, -1, 3)),
gt_pose)[0]
S_given = S_given + np.random.normal(0, 1, S_given.shape)
M_given = M_given[0:NUM_POINT, :] # template data
S_given = S_given[0:NUM_POINT, :] # source data
tree_M = KDTree(M_given)
tree_M_sampled = KDTree(M_given[0:100, :])
final_pose, model_data, sensor_data, predicted_data, title = icp_test(
S_given[0:100, :], M_given, tree_M, M_given[0:100, :], tree_M_sampled,
S_given, gt_pose.reshape((1, 6)), MAX_LOOPS, 1e-05)
helper.display_three_clouds(model_data, sensor_data, predicted_data, title)
def eval_network(sess, ops, templates, poses):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
loss_sum = 0 # Total Loss in each batch.
# print(int(poses.shape[0]/BATCH_SIZE))
# print(int(len(templates)/BATCH_SIZE))
# poses = poses[:1000]
num_batches = int(poses.shape[0]/BATCH_SIZE) # Number of batches in an epoch.
indx = np.arange(0,len(templates))
while len(indx)<poses.shape[0]:
indx = np.concatenate([indx, indx], 0)
# num_batches = int(len(templates)/BATCH_SIZE)
# exit()
print('Number of batches to be executed: {}'.format(num_batches))
# Store time taken, no of iterations, translation error and rotation error for registration.
TIME, ITR, Trans_Err, Rot_Err = [], [], [], []
idxs_25_5, idxs_5_5, idxs_10_1, idxs_20_2 = [], [], [], []
if FLAGS.use_noise_data:
print(FLAGS.data_dict)
templates, sources = helper.read_noise_data(FLAGS.data_dict)
print(templates.shape, sources.shape)
TE = np.zeros([MAX_LOOPS+1,num_batches]) #translation error
RE = np.zeros([MAX_LOOPS+1,num_batches]) #rotation error
CE = np.zeros([MAX_LOOPS+1,num_batches]) #convergence error
Failures = []
ques = np.zeros([MAX_LOOPS,7,num_batches])
for fn in range(num_batches):
start_idx = fn*BATCH_SIZE # Start index of poses.
end_idx = (fn+1)*BATCH_SIZE # End index of poses.
if SPARSE_SAMPLING>0:
template_data = np.copy(templates[indx[fn], :, :]).reshape(1, -1, 3)
batch_euler_poses = poses[start_idx:end_idx] # Extract poses for batch training.
template_data,source_data = helper.split_template_source(template_data,batch_euler_poses,NUM_POINT,centroid_subtraction_switch=False,ADD_NOISE=FLAGS.use_noise_data,S_RAND_POINTS=S_RAND_POINTS,SPARSE=SPARSE_SAMPLING)
else:
if FLAGS.use_noise_data:
template_data = np.copy(templates[fn,:,:]).reshape(1,-1,3) # As template_data is changing.
source_data = np.copy(sources[fn,:,:]).reshape(1,-1,3)
batch_euler_poses = poses[start_idx:end_idx] # Extract poses for batch training.
else:
# template_idx = pairs[fn,1]
template_data = np.copy(templates[indx[fn],:,:]).reshape(1,-1,3) # As template_data is changing.
batch_euler_poses = poses[start_idx:end_idx] # Extract poses for batch training.
# print(batch_euler_poses)
if template_random_pose:
template_data = helper.apply_transformation(template_data, batch_euler_poses / 2)
template_data = template_data - np.mean(template_data, axis=1, keepdims=True)
source_data = helper.apply_transformation(template_data, batch_euler_poses) # Apply the poses on the templates to get source data.
# SOURCE_DATA = np.copy(source_data[:,0:NUM_POINT,:]) #movement is calculated from initial pose
if np.random.random_sample()<S_RAND_POINTS:
template_data = helper.select_random_points(template_data, NUM_POINT)
source_data = helper.select_random_points(source_data, NUM_POINT) # 50% probability that source data has different points than template
template_data = template_data[:,0:NUM_POINT,:]
else:
source_data = source_data[:,0:NUM_POINT,:]
template_data = template_data[:,0:NUM_POINT,:]
# Just to visualize the data.
TEMPLATE_DATA = np.copy(template_data) # Store the initial template to visualize results.
SOURCE_DATA = np.copy(source_data) # Store the initial source to visualize results.
# Subtract the Centroids from the Point Clouds.
if centroid_subtraction_switch:
# print(np.mean(source_data, axis=1, keepdims=True))
T = np.mean(source_data, axis=1)
print(T)
print(np.mean(template_data, axis=1))
print(np.mean(source_data, axis=1))
source_data = source_data - np.mean(source_data, axis=1, keepdims=True)
# print(np.mean(template_data, axis=1, keepdims=True))
# template_data = template_data - np.mean(template_data, axis=1, keepdims=True)
else:
T=[[0,0,0]]
if FLAGS.add_occlusions>0.0:
source_data = helper.add_occlusions(source_data,FLAGS.add_occlusions)
# exit()
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
TRANSFORMATIONS = np.identity(4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS,BATCH_SIZE,1).reshape(BATCH_SIZE,4,4) # Intialize identity matrices of size equal to batch_size
# previous_pose = np.array([0,0,0,1,0,0,0])
previous_T = np.eye(4)
start = time.time() # Log start time.
# Iterations for pose refinement.
translation_error, rotational_error, final_pose = get_error(TRANSFORMATIONS, SOURCE_DATA,
batch_euler_poses,T)
TE[0, fn] = translation_error
RE[0, fn] = rotational_error
CE[0, fn] = 1
print(fn)
for loop_idx in range(MAX_LOOPS):
# for network_itr in range(7):
# # Feed the placeholders of Network19 with template data and source data.
# feed_dict = {ops['source_pointclouds_pl']: source_data,
# ops['template_pointclouds_pl']: template_data,
# ops['is_training_pl']: is_training}
# predicted_transformation = sess.run([ops['predicted_transformation']], feed_dict=feed_dict) # Ask the network to predict the pose.
#
# # Apply the transformation on the source data and multiply it to transformation matrix obtained in previous iteration.
# TRANSFORMATIONS, source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, source_data)
#
# # Display Results after each iteration.
# if display_poses_in_itr:
# print(predicted_transformation[0,0:3])
# print(predicted_transformation[0,3:7]*(180/np.pi))
# if display_ptClouds_in_itr:
# helper.display_clouds_data(template_data[0])
# Feed the placeholders of Network_L with source data and template data obtained from N-Iterations.
feed_dict = {ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training}
# Ask the network to predict transformation, calculate loss using distance between actual points.
predicted_transformation = sess.run([ops['predicted_transformation']], feed_dict=feed_dict)
# print(predicted_transformation)
# Apply the final transformation on the source data and multiply it with the transformation matrix obtained from N-Iterations.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, source_data)
translation_error, rotational_error, final_pose = get_error(TRANSFORMATIONS, SOURCE_DATA,
batch_euler_poses,T)
ck_con_T,convergence_error = check_convergenceT(previous_T, TRANSFORMATIONS[0])
ques[loop_idx,:,fn] = np.squeeze(predicted_transformation)
print(ques[loop_idx,:,fn])
TE[loop_idx+1,fn] = translation_error
RE[loop_idx+1,fn] = rotational_error
CE[loop_idx+1,fn] = convergence_error
if ck_con_T:
# break
print('converge iteration:',loop_idx)
else:
previous_T = np.copy(TRANSFORMATIONS[0])
previous_T = np.copy(TRANSFORMATIONS[0])
end = time.time() # Log end time.
# final_pose = helper.find_final_pose_inv(TRANSFORMATIONS) # Find the final pose (translation, orientation (euler angles in degrees)) from transformation matrix.
# final_pose[0,0:3] = final_pose[0,0:3] + np.mean(SOURCE_DATA, axis=1)[0]#\
# #- np.mean(TEMPLATE_DATA, axis=1)[0]
#
# translation_error, rotational_error = find_errors(batch_euler_poses[0], final_pose[0])
translation_error, rotational_error, final_pose = get_error(TRANSFORMATIONS, SOURCE_DATA, batch_euler_poses,T)
TIME.append(end-start)
ITR.append(loop_idx+1)
Trans_Err.append(translation_error)
Rot_Err.append(rotational_error)
if rotational_error<20 and translation_error<0.2:
if rotational_error<10 and translation_error<0.1:
if rotational_error<5 and translation_error<0.05:
if rotational_error < 2.5:
idxs_25_5.append(fn)
idxs_5_5.append(fn)
idxs_10_1.append(fn)
idxs_20_2.append(fn)
# if rotational_error>20:
# Failures.append(fn)
# helper.display_three_clouds(template_data[0],SOURCE_DATA[0],source_data[0], str(fn)+"_"+str(rotational_error))
# print('added failure image')
# else:
# print(rotational_error)
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(batch_euler_poses[0,0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0,0:3].tolist()))
print('Ground Truth Orientation: {}'.format((batch_euler_poses[0,3:6]*(180/np.pi)).tolist()))
print('Predicted Orientation: {}'.format((final_pose[0,3:6]*(180/np.pi)).tolist()))
# Display Loss Value.
# helper.display_three_clouds(TEMPLATE_DATA[0],SOURCE_DATA[0],template_data[0],"")
print("Batch: {} & time: {}, iteration: {}".format(fn, end-start, loop_idx+1))
plot_iter_graph(TE,FLAGS.log_dir,'translation error')
plot_iter_graph(RE,FLAGS.log_dir,'rotation error')
plot_iter_graph(CE,FLAGS.log_dir,'convergence error')
log = {'TIME': TIME, 'ITR':ITR, 'Trans_Err': Trans_Err, 'Rot_Err': Rot_Err,'idxs_25_5':idxs_25_5, 'idxs_5_5': idxs_5_5, 'idxs_10_1': idxs_10_1, 'idxs_20_2': idxs_20_2, 'num_batches': num_batches}
helper.log_test_results(FLAGS.log_dir, FLAGS.filename, log)
hf = h5py.File(os.path.join(FLAGS.log_dir, 'log_data.h5'), 'w')
hf.create_dataset('TE', data=TE)
hf.create_dataset('RE', data=RE)
hf.create_dataset('CE', data=CE)
hf.close()
def eval_one_epoch(sess, ops, eval_writer, poses):
# Arguments:
# sess: Tensorflow session to handle tensors.
# ops: Dictionary for tensors of Network
# templates: Training Point Cloud data.
# poses: Training pose data.
is_training = False
display_ptClouds = False
display_poses = False
display_poses_in_itr = False
display_ptClouds_in_itr = False
loss_sum = 0 # Total Loss in each batch.
templates, sources, poses = helper.read_partial_data(
'train_data', 'partial_data_eval.h5')
num_batches = int(templates.shape[0] /
BATCH_SIZE) # Number of batches in an epoch.
for fn in range(num_batches):
start_idx = fn * BATCH_SIZE # Start index of poses.
end_idx = (fn + 1) * BATCH_SIZE # End index of poses.
template_data = np.copy(templates[start_idx:end_idx])
batch_euler_poses = np.copy(poses[start_idx:end_idx])
# Check for partial source.
if np.random.sample() < ADD_PARTIAL:
source_data = np.copy(sources[start_idx:end_idx])
source_full_data = helper.apply_transformation(
template_data, batch_euler_poses)
else:
source_data = np.copy(templates[start_idx:end_idx, 0:512])
source_data = helper.apply_transformation(source_data,
batch_euler_poses)
source_full_data = helper.apply_transformation(
template_data, batch_euler_poses)
# Check for noise in source.
if np.random.sample() < ADD_NOISE:
source_data = helper.add_noise(source_data)
if centroid_subtraction_switch:
source_data = source_data - np.mean(
source_data, axis=1, keepdims=True)
template_data = template_data - np.mean(
template_data, axis=1, keepdims=True)
source_full_data = source_full_data - np.mean(
source_full_data, axis=1, keepdims=True)
# Only chose limited number of points from the source and template data.
template_data = template_data[:, 0:NUM_POINT, :]
source_full_data = source_full_data[:, 0:NUM_POINT, :]
# To visualize the source and point clouds:
if display_ptClouds:
helper.display_clouds_data(source_data[0])
helper.display_clouds_data(template_data[0])
TRANSFORMATIONS = np.identity(
4) # Initialize identity transformation matrix.
TRANSFORMATIONS = npm.repmat(TRANSFORMATIONS, BATCH_SIZE, 1).reshape(
BATCH_SIZE, 4,
4) # Intialize identity matrices of size equal to batch_size
# Iterations for pose refinement.
for loop_idx in range(MAX_LOOPS - 1):
# 4a
# Feed the placeholders of Network with template data and source data.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['is_training_pl']: is_training
}
predicted_transformation = sess.run(
[ops['predicted_transformation']],
feed_dict=feed_dict) # Ask the network to predict the pose.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
# Display Results after each iteration.
if display_poses_in_itr:
print(predicted_transformation[0, 0:3])
print(predicted_transformation[0, 3:7] * (180 / np.pi))
if display_ptClouds_in_itr:
helper.display_clouds_data(source_data[0])
# Feed the placeholders of Network with source data and template data obtained from N-Iterations.
feed_dict = {
ops['source_pointclouds_pl']: source_data,
ops['template_pointclouds_pl']: template_data,
ops['full_source_pointclouds_pl']: source_full_data,
ops['is_training_pl']: is_training
}
# Ask the network to predict transformation, calculate loss using distance between actual points.
summary, step, loss_val, predicted_transformation = sess.run(
[
ops['merged'], ops['step'], ops['loss'],
ops['predicted_transformation']
],
feed_dict=feed_dict)
eval_writer.add_summary(
summary, step) # Add all the summary to the tensorboard.
TRANSFORMATIONS, source_data = helper.transformation_quat2mat(
predicted_transformation, TRANSFORMATIONS, source_data)
final_pose = helper.find_final_pose_inv(
TRANSFORMATIONS
) # Find the final pose (translation, orientation (euler angles in degrees)) from transformation matrix.
# Display the ground truth pose and predicted pose for first Point Cloud in batch
if display_poses:
print('Ground Truth Position: {}'.format(
batch_euler_poses[0, 0:3].tolist()))
print('Predicted Position: {}'.format(final_pose[0, 0:3].tolist()))
print('Ground Truth Orientation: {}'.format(
(batch_euler_poses[0, 3:6] * (180 / np.pi)).tolist()))
print('Predicted Orientation: {}'.format(
(final_pose[0, 3:6] * (180 / np.pi)).tolist()))
# Display Loss Value.
print("Batch: {}, Loss: {}\r".format(fn, loss_val), end='')
# Add loss for each batch.
loss_sum += loss_val
print('\n')
log_string(
'Eval Mean loss: %f' %
(loss_sum / num_batches)) # Store and display mean loss of epoch.
