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