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