def test_one_case(self, template_data, source_data): # template_data: Input Template Data for network # source_data: Input Source Data for network display_poses_in_itr = False display_ptClouds_in_itr = False template = np.copy(template_data) source = np.copy(source_data) template_check = np.copy(template_data) TRANSFORMATIONS = np.identity(4) # Initialize identity transformation matrix. TRANSFORMATIONS = np.matlib.repmat(TRANSFORMATIONS,BATCH_SIZE,1).reshape(BATCH_SIZE,4,4) TRANSFORMATIONS_check = np.copy(TRANSFORMATIONS) start = time.time() # helper.display_three_clouds(template_data[0],source_data[0],source_data[0],"Iteration: 0") for loop_idx in range(self.MAX_LOOPS-1): # Feed the placeholders of Network19 with template data and source data. feed_dict = {self.ops19['source_pointclouds_pl']: source, self.ops19['template_pointclouds_pl']: template, self.ops19['is_training_pl']: self.is_training} predicted_transformation = self.sess.run([self.ops19['predicted_transformation']], feed_dict=feed_dict) # Ask the network to predict the pose. # Apply the transformation on the template data and multiply it to transformation matrix obtained in previous iteration. TRANSFORMATIONS, template = helper.transformation_quat2mat(predicted_transformation,TRANSFORMATIONS,template) if np.sum(np.abs(template[:,0:100,:] - template_check[:,0:100,:])) < self.ftol: # if np.sum(np.dot(np.linalg.inv(TRANSFORMATIONS_check), TRANSFORMATIONS)) < ftol: break else: # TRANSFORMATIONS_check = np.copy(TRANSFORMATIONS) template_check = np.copy(template) # 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[0]) # transformed_source_data = np.dot(np.linalg.inv(TRANSFORMATIONS[0])[0:3,0:3], source[0].T).T + np.linalg.inv(TRANSFORMATIONS[0])[0:3,3] # helper.display_three_clouds(template_data[0], source[0], transformed_source_data, "Iteration: "+str(loop_idx+1)) # Feed the placeholders of Network_L with source data and template data obtained from N-Iterations. feed_dict = {self.ops_L['source_pointclouds_pl']: source, self.ops_L['template_pointclouds_pl']: template, self.ops_L['is_training_pl']: self.is_training} # Ask the network to predict transformation, calculate loss using distance between actual points. step, predicted_transformation = self.sess.run([self.ops_L['step'], self.ops_L['predicted_transformation']], feed_dict=feed_dict) end = time.time() loss_val = 0 # Apply the final transformation on the template data and multiply it with the transformation matrix obtained from N-Iterations. TRANSFORMATIONS, template = helper.transformation_quat2mat(predicted_transformation, TRANSFORMATIONS, template) final_pose = helper.find_final_pose(TRANSFORMATIONS) transformed_source_data = np.dot(np.linalg.inv(TRANSFORMATIONS[0])[0:3,0:3], source[0].T).T + np.linalg.inv(TRANSFORMATIONS[0])[0:3,3] return final_pose, TRANSFORMATIONS, loss_val, template, transformed_source_data, end-start, (loop_idx+1)
def eval_one_epoch(sess, ops, eval_writer): # Arguments: # sess: Tensorflow session to handle tensors. # ops: 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 test_file_idxs = np.arange(0, len(TEST_FILES)) np.random.shuffle(test_file_idxs) for fn in range(len(TEST_FILES)): log_string('----' + str(fn) + '-----') current_data, current_label = provider.loadDataFile( TEST_FILES[test_file_idxs[fn]]) current_data = current_data[:, 0:NUM_POINT, :] current_data, _, _ = provider.shuffle_data(current_data, np.squeeze(current_label)) file_size = current_data.shape[0] num_batches = file_size // BATCH_SIZE loss_sum = 0 # Total Loss in 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 = current_data[start_idx:end_idx, :, :] # To visualize the source and point clouds: if display_ptClouds: 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']: template_data, ops['is_training_pl']: is_training } # Ask the network to predict transformation, calculate loss using distance between actual points. summary, step, loss_val = sess.run( [ops['merged'], ops['step'], ops['loss']], feed_dict=feed_dict) eval_writer.add_summary( summary, step) # Add all the summary to the tensorboard. # Display Loss Value. print("Batch: {} & Loss: {}\r".format(fn, loss_val)), sys.stdout.flush() # Add loss for each batch. loss_sum += loss_val log_string( 'Eval Mean loss: %f' % (loss_sum / num_batches)) # Store and display mean loss of epoch.
def test_one_epoch(sess, ops, saver, model_path): # Arguments: # sess: Tensorflow session to handle tensors. # ops: 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. current_data, current_label = provider.loadDataFile(TEST_FILES[0]) is_training = False display_ptClouds = False template_idx = 11 current_data = current_data[template_idx * BATCH_SIZE:(template_idx + 1) * BATCH_SIZE] print(current_data.shape) template_data = current_data[:, 0:NUM_POINT, :] batch_euler_pose = np.array( [[0, 0, 0, 90 * (np.pi / 180), 0 * (np.pi / 180), 0 * (np.pi / 180)]]) # template_data = helper.apply_transformation(template_data, batch_euler_pose) batch_euler_pose = np.array( [[0, 0, 0, 0 * (np.pi / 180), 0 * (np.pi / 180), 180 * (np.pi / 180)]]) # template_data = helper.apply_transformation(template_data, batch_euler_pose) # To visualize the source and point clouds: if display_ptClouds: 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']: template_data, ops['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_pointclouds_pl = sess.run( [ops['step'], ops['predicted_pointclouds_pl']], feed_dict=feed_dict) end = time.time() print('Time Elapsed: {}'.format(end - start)) predicted_pointclouds_pl[0, :, 0] = predicted_pointclouds_pl[0, :, 0] + 1 helper.display_two_clouds(template_data[0], predicted_pointclouds_pl[0])
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 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.
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 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.
import provider import helper TRAIN_FILES = provider.getDataFiles( 'data/modelnet40_ply_hdf5_2048/train_files.txt') current_data, current_label = provider.loadDataFile(TRAIN_FILES[0]) idx = int(sys.argv[1]) helper.display_clouds_data(current_data[idx])
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)