helper.py

import csv
import matplotlib.pyplot as plt 
from mpl_toolkits.mplot3d import Axes3D
import os
import numpy as np
import transforms3d.euler as t3d
import helper
import tensorflow as tf

###################### Print Operations #########################
def print_(text="Test", color='w', style='no', bg_color=''):
	color_dict = {'b': 30, 'r': 31, 'g': 32, 'y': 33, 'bl': 34, 'p': 35, 'c': 36, 'w': 37}
	style_dict = {'no': 0, 'bold': 1, 'underline': 2, 'neg1': 3, 'neg2': 5}
	bg_color_dict = {'b': 40, 'r': 41, 'g': 42, 'y': 43, 'bl': 44, 'p': 45, 'c': 46, 'w': 47}
	if bg_color is not '':
		print("\033[" + str(style_dict[style]) + ";" + str(color_dict[color]) + ";" + str(bg_color_dict[bg_color]) + "m" + text + "\033[00m")
	else: print("\033["+ str(style_dict[style]) + ";" + str(color_dict[color]) + "m"+ text + "\033[00m")


###################### Data Downloading Operations #########################

def download_data(file):
	print_('################### Downloading Data ###################', color='g', style='bold')
	from google_drive_downloader import GoogleDriveDownloader as gdd

	if file=='train_data':
		file_id = '16YU-tdayVNBwM3XlPDgFrrzlPjhQN3PB'
	elif file=='car_data':
		file_id = '1k9W75uhUFTfA_iK7YePGn5t9f4JhtgSe'

	if not os.path.exists(os.path.join(os.getcwd(),'data',file)):
		gdd.download_file_from_google_drive(file_id=file_id,
										dest_path=os.path.join(os.getcwd(),'data',file+'.zip'),
										showsize=True,
										unzip=True)

		os.remove(os.path.join(os.getcwd(),'data',file+'.zip'))
	return True


###################### Data Handling Operations #########################

# Read the templates from a given file.
def read_templates(file_name,templates_dict):
	with open(os.path.join('data',templates_dict,file_name),'r') as csvfile:
		csvreader = csv.reader(csvfile)
		data = []
		for row in csvreader:
			row = [float(i) for i in row]
			data.append(row)
	return data 										# n2 x 2048 x 3

# Read the file names having templates.
def template_files(templates_dict):
	with open(os.path.join('data',templates_dict,'template_filenames.txt'),'r') as file:
		files = file.readlines()
	files = [x.strip() for x in files]
	print(files)
	return files 										# 1 x n1

# Read the templates from each file.
def templates_data(templates_dict):
	files = template_files(templates_dict)							# Read the available file names.
	data = []
	for i in range(len(files)):
		temp = read_templates(files[i],templates_dict)
		for i in temp:
			data.append(i)
	return np.asarray(data)								# (n1 x n2 x 2048 x 3) & n = n1 x n2

# Preprocess the templates and rearrange them.
def process_templates(templates_dict):
	data = templates_data(templates_dict)								# Read all the templates.
	print(data.shape[0]/2048)
	templates = []
	for i in range(data.shape[0]/2048):
		start_idx = i*2048
		end_idx = (i+1)*2048
		templates.append(data[start_idx:end_idx,:])
	return np.asarray(templates)						# Return all the templates (n x 2048 x 3)

# Read poses from given file.
def read_poses(templates_dict, filename):
	# Arguments:
		# filename:			Read data from a given file (string)
	# Output:
		# poses:			Return array of all the poses in the file (n x 6)

	with open(os.path.join('data',templates_dict,filename),'r') as csvfile:
		csvreader = csv.reader(csvfile)
		poses = []
		for row in csvreader:
			row = [float(i) for i in row]
			poses.append(row)
	return np.asarray(poses)

# Read names of files in given data_dictionary.
def read_files(data_dict):
	with open(os.path.join('data',data_dict,'files.txt')) as file:
		files = file.readlines()
		files = [x.split()[0] for x in files]
	return files[0]

# Read data from h5 file and return as templates.
def read_h5(file_name):
	import h5py
	f = h5py.File(file_name, 'r')
	templates = np.array(f.get('templates'))
	f.close()
	return templates

def read_noise_data(data_dict):
	import h5py
	f = h5py.File(os.path.join('data',data_dict,'noise_data.h5'), 'r')
	templates = np.array(f.get('templates'))
	sources = np.array(f.get('sources'))
	f.close()
	return templates, sources

def read_pairs(data_dict, file_name):
	with open(os.path.join('data', data_dict, file_name), 'r') as csvfile:
		csvreader = csv.reader(csvfile)
		pairs = []
		for row in csvreader:
			row = [int(x) for x in row]
			pairs.append(row)
	return np.asarray(pairs)

# Main function to load data and return as templates array.
def loadData(data_dict):
	files = read_files(data_dict)	# Read file names.
	print(files)
	templates = read_h5(files)		# Read templates from h5 file using given file_name.
	return templates

###################### Transformation Operations #########################

def rotate_point_cloud_by_angle_y(batch_data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  BxNx3 array, original batch of point clouds
		Return:
		  BxNx3 array, rotated batch of point clouds
	"""
	cosval = np.cos(rotation_angle)
	sinval = np.sin(rotation_angle)
	rotation_matrix = np.array([[cosval, 0, sinval],
								[0, 1, 0],
								[-sinval, 0, cosval]])
	return np.dot(rotation_matrix, batch_data.reshape((-1,3)).T).T
	#rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
	#for k in range(batch_data.shape[0]):
	#	#rotation_angle = np.random.uniform() * 2 * np.pi
	#	cosval = np.cos(rotation_angle)
	#	sinval = np.sin(rotation_angle)
	#	rotation_matrix = np.array([[cosval, 0, sinval],
	#								[0, 1, 0],
	#								[-sinval, 0, cosval]])
	#	shape_pc = batch_data[k, ...]
	#	# rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
	#	rotated_data[k, ...] = np.dot(rotation_matrix, shape_pc.reshape((-1, 3)).T).T 		# Pre-Multiplication (changes done)
	#return rotated_data

def rotate_point_cloud_by_angle_x(batch_data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  BxNx3 array, original batch of point clouds
		Return:
		  BxNx3 array, rotated batch of point clouds
	"""
	cosval = np.cos(rotation_angle)
	sinval = np.sin(rotation_angle)
	rotation_matrix = np.array([[1, 0, 0],
								[0, cosval, -sinval],
								[0, sinval, cosval]])
	return np.dot(rotation_matrix, batch_data.reshape((-1,3)).T).T
	#rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
	#for k in range(batch_data.shape[0]):
	#	#rotation_angle = np.random.uniform() * 2 * np.pi
	#	cosval = np.cos(rotation_angle)
	#	sinval = np.sin(rotation_angle)
	#	rotation_matrix = np.array([[1, 0, 0],
	#								[0, cosval, -sinval],
	#								[0, sinval, cosval]])
	#	shape_pc = batch_data[k, ...]
	#	# rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
	#	rotated_data[k, ...] = np.dot(rotation_matrix, shape_pc.reshape((-1, 3)).T).T 		# Pre-Multiplication (changes done)
	#return rotated_data

def rotate_point_cloud_by_angle_z(batch_data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  BxNx3 array, original batch of point clouds
		Return:
		  BxNx3 array, rotated batch of point clouds
	"""
	cosval = np.cos(rotation_angle)
	sinval = np.sin(rotation_angle)
	rotation_matrix = np.array([[cosval, -sinval, 0],
								[sinval, cosval, 0],
								[0, 0, 1]])
	return np.dot(rotation_matrix, batch_data.reshape((-1,3)).T).T
	#rotated_data = np.zeros(batch_data.shape, dtype=np.float32)
	#for k in range(batch_data.shape[0]):
	#	#rotation_angle = np.random.uniform() * 2 * np.pi
	#	cosval = np.cos(rotation_angle)
	#	sinval = np.sin(rotation_angle)
	#	rotation_matrix = np.array([[cosval, -sinval, 0],
	#								[sinval, cosval, 0],
	#								[0, 0, 1]])
	#	shape_pc = batch_data[k, ...]
	#	# rotated_data[k, ...] = np.dot(shape_pc.reshape((-1, 3)), rotation_matrix)
	#	rotated_data[k, ...] = np.dot(rotation_matrix, shape_pc.reshape((-1, 3)).T).T 		# Pre-Multiplication (changes done)
	#return rotated_data

# Translate the data as per given translation vector.
def translate(data,shift):
	# Arguments:
		# data:					Point Cloud data (1 x num_points x 3)
		# shift:				Translation vector (1 x 3)

	try:
		data = np.asarray(data)
	except:
		pass
	return data+shift

# Apply the given transformation to given point cloud data.
def apply_transformation(datas,poses):			# Transformation function for (2 & 4c, loss 8b)
	# Arguments:
		# datas: 			Point Clouds (batch_size x num_points x 3)
		# poses: 			translation+euler (Batch_size x 6)
	# Output:
		# transformed_data: Transformed Point Clouds by given poses (batch_size x num_points x 3)
	transformed_data = np.copy(datas)
	for i in range(datas.shape[0]):
		transformed_data[i,:,:] = rotate_point_cloud_by_angle_z(transformed_data[i,:,:],poses[i,5])
		transformed_data[i,:,:] = rotate_point_cloud_by_angle_y(transformed_data[i,:,:],poses[i,4])
		transformed_data[i,:,:] = rotate_point_cloud_by_angle_x(transformed_data[i,:,:],poses[i,3])
		transformed_data[i,:,:] = translate(transformed_data[i,:,:],[poses[i,0],poses[i,1],poses[i,2]])
	return transformed_data

# Convert poses from 6D to 7D. 			# For loss function ( 8a )
def poses_euler2quat(poses):
	# Arguments:
		# poses: 			6D pose (translation + euler) (batch_size x 6)
	# Output: 
		# new_poses: 		7D pose (translation + quaternions) (batch_size x 7)

	new_poses = []					# Store 7D poses
	for i in range(poses.shape[0]):
		temp = t3d.euler2quat(poses[i,3],poses[i,4],poses[i,5])						# Convert from euler to quaternion. (1x4)
		temp1 = [poses[i,0],poses[i,1],poses[i,2],temp[0],temp[1],temp[2],temp[3]]		# Add translation & Quaternion (1x7)
		new_poses.append(temp1)												
	return np.asarray(new_poses)									

# Geenerate random poses equal to batch_size.
def generate_poses(batch_size):
	# Arguments:
		# batch_size:		No of 6D poses required.
	# Output:
		# poses:			Array of poses with translation and rotation (euler angles in radians) (batch_size x 6)

	poses = []					# List to store the 6D poses.
	for i in range(batch_size):
		# Generate random translations.
		x = np.round(2*np.random.random_sample()-1,2)
		y = np.round(2*np.random.random_sample()-1,2)
		z = np.round(2*np.random.random_sample()-1,2)
		# Generate random rotations.
		x_rot = np.round(np.pi*np.random.random_sample()-(np.pi/2),3)
		y_rot = np.round(np.pi*np.random.random_sample()-(np.pi/2),3)
		z_rot = np.round(np.pi*np.random.random_sample()-(np.pi/2),3)
		poses.append([x,y,z,x_rot,y_rot,z_rot])
	return np.array(poses).reshape((batch_size,6))

# Convert 6D poses to transformation matrix.	# (for 4b)
def transformation(poses):		
	# Arguments:
		# poses: 					6D (x,y,z,euler_x,euler_y,euler_z) (in radians)
	# Output
		# transformation_matrix: 	batch_size x 4 x 4

	transformation_matrix = np.zeros((poses.shape[0],4,4))		
	transformation_matrix[:,3,3] = 1
	for i in range(poses.shape[0]):
		rot = t3d.euler2mat(poses[i,5],poses[i,4],poses[i,3],'szyx')	# Calculate rotation matrix using transforms3d
		transformation_matrix[i,0:3,0:3]=rot 							# Store rotation matrix in transformation matrix.
		transformation_matrix[i,0:3,3]=poses[i,0:3]						# Store translations in transformation matrix.
	return transformation_matrix

# Convert poses (quaternions) to transformation matrix and apply on point cloud.
def transformation_quat2mat(poses,TRANSFORMATIONS,templates_data):		# (for 4b)
	# Arguments:
		# poses: 					7D (x,y,z,quat_q0,quat_q1,quat_q2,quat_q3) (in radians) (batch_size x 7)
		# TRANSFORMATIONS: 			Overall tranformation matrix.
		# template_data: 			Point Cloud (batch_size x num_points x 3)
	# Output
		# TRANSFORMATIONS: 			Batch_size x 4 x 4
		# templates_data:			Transformed template data (batch_size x num_points x 3)
	
	poses = np.array(poses)												# Convert poses to array.
	poses = poses.reshape(poses.shape[-2],poses.shape[-1])
	for i in range(poses.shape[0]):
		transformation_matrix = np.zeros((4,4))
		transformation_matrix[3,3] = 1
		rot = t3d.quat2mat([poses[i,3],poses[i,4],poses[i,5],poses[i,6]])	# Calculate rotation matrix using transforms3d
		transformation_matrix[0:3,0:3]=rot 									# Store rotation matrix in transformation matrix.
		transformation_matrix[0:3,3]=poses[i,0:3]							# Store translations in transformation matrix.
		TRANSFORMATIONS[i,:,:] = np.dot(transformation_matrix,TRANSFORMATIONS[i,:,:])		# 4b (Multiply tranfromation matrix to Initial Transfromation Matrix)
		templates_data[i,:,:]=np.dot(rot,templates_data[i,:,:].T).T 		# Apply Rotation to Template Data
		templates_data[i,:,:]=templates_data[i,:,:]+poses[i,0:3]			# Apply translation to template data
	return TRANSFORMATIONS,templates_data

# Convert the Final Transformation Matrix to Translation + Orientation (Euler Angles in Degrees)
def find_final_pose(TRANSFORMATIONS):
	# Arguments:
		# TRANSFORMATIONS: 			transformation matrix (batch_size x 4 x 4)
	# Output:
		# final_pose:				final pose predicted by network (batch_size x 6)

	final_pose = np.zeros((TRANSFORMATIONS.shape[0],6))		# Array to store the poses.
	for i in range(TRANSFORMATIONS.shape[0]):				
		rot = TRANSFORMATIONS[i,0:3,0:3]					# Extract rotation matrix.
		euler = t3d.mat2euler(rot,'szyx')					# Convert rotation matrix to euler angles. (Pre-multiplication)
		final_pose[i,3:6]=[euler[2],euler[1],euler[0]]		# Store the translation
		final_pose[i,0:3]=TRANSFORMATIONS[i,0:3,3].T 		# Store the euler angles.
	return final_pose

# Convert the Final Transformation Matrix to Translation + Orientation (Euler Angles in Degrees)
def find_final_pose_inv(TRANSFORMATIONS_ip):
	# Arguments:
		# TRANSFORMATIONS: 			transformation matrix (batch_size x 4 x 4)
	# Output:
		# final_pose:				final pose predicted by network (batch_size x 6)

	TRANSFORMATIONS = np.copy(TRANSFORMATIONS_ip)
	final_pose = np.zeros((TRANSFORMATIONS.shape[0],6))		# Array to store the poses.
	for i in range(TRANSFORMATIONS.shape[0]):
		TRANSFORMATIONS[i] = np.linalg.inv(TRANSFORMATIONS[i])				
		rot = TRANSFORMATIONS[i,0:3,0:3]					# Extract rotation matrix.
		euler = t3d.mat2euler(rot,'szyx')					# Convert rotation matrix to euler angles. (Pre-multiplication)
		final_pose[i,3:6]=[euler[2],euler[1],euler[0]]		# Store the translation
		final_pose[i,0:3]=TRANSFORMATIONS[i,0:3,3].T 		# Store the euler angles.
	return final_pose

# Subtract the centroids from source and template (Like ICP) and then find the pose.
def centroid_subtraction(source_data, template_data):
	# Arguments:
		# source_data:			Source Point Clouds (batch_size x num_points x 3)
		# template_data:		Template Point Clouds (batch_size x num_points x 3)
	# Output:
		# source_data:					Centroid subtracted from source point cloud (batch_size x num_points x 3)
		# template_data:				Centroid subtracted from template point cloud (batch_size x num_points x 3)
		# centroid_translation_pose:	Apply this pose after final iteration. (batch_size x 7)

	centroid_translation_pose = np.zeros((source_data.shape[0],7))
	for i in range(source_data.shape[0]):
		source_centroid = np.mean(source_data[i],axis=0)
		template_centroid = np.mean(template_data[i],axis=0)
		source_data[i] = source_data[i] - source_centroid
		template_data[i] = template_data[i] - template_centroid
		centroid_translation = source_centroid - template_centroid
		centroid_translation_pose[i] = np.array([centroid_translation[0],centroid_translation[1],centroid_translation[2],1,0,0,0])
	return source_data, template_data, centroid_translation_pose

def inverse_pose(pose):
	transformation_pose = np.zeros((4,4))
	transformation_pose[3,3]=1
	transformation_pose[0:3,0:3] = t3d.euler2mat(pose[5], pose[4], pose[3], 'szyx')
	transformation_pose[0,3] = pose[0]
	transformation_pose[1,3] = pose[1]
	transformation_pose[2,3] = pose[2]
	transformation_pose = np.linalg.inv(transformation_pose)
	pose_inv = np.zeros((1,6))[0]
	pose_inv[0] = transformation_pose[0,3]
	pose_inv[1] = transformation_pose[1,3]
	pose_inv[2] = transformation_pose[2,3]
	orient_inv = t3d.mat2euler(transformation_pose[0:3,0:3], 'szyx')
	pose_inv[3] = orient_inv[2]
	pose_inv[4] = orient_inv[1]
	pose_inv[5] = orient_inv[0]
	return pose_inv


###################### Shuffling Operations #########################

# Randomly shuffle given array of poses for training procedure.
def shuffle_templates(templates):
	# Arguments:
		# templates:			Input array of templates to get randomly shuffled (batch_size x num_points x 3)
	# Output:
		# shuffled_templates:	Randomly ordered poses (batch_size x num_points x 3)

	shuffled_templates = np.zeros(templates.shape)						# Array to store shuffled templates.
	templates_idxs = np.arange(0,templates.shape[0])
	np.random.shuffle(templates_idxs)									# Randomly shuffle template indices.
	for i in range(templates.shape[0]):
		shuffled_templates[i,:,:]=templates[templates_idxs[i],:,:]		# Rearrange them as per shuffled indices.
	return shuffled_templates

# Randomly shuffle given array of poses for training procedure.
def shuffle_poses(poses):
	# Arguments:
		# poses:			Input array of poses to get randomly shuffled (batch_size x n)
	# Output:
		# shuffled_poses:	Randomly ordered poses (batch_size x n)

	shuffled_poses = np.zeros(poses.shape)				# Array to store shuffled poses.
	poses_idxs = np.arange(0,poses.shape[0])			
	np.random.shuffle(poses_idxs)						# Shuffle the indexes of poses.
	for i in range(poses.shape[0]):
		shuffled_poses[i,:]=poses[poses_idxs[i],:]		# Rearrange them as per shuffled indexes.
	return shuffled_poses

# Generate random transformation/pose for data augmentation.
def random_trans():
	# Output:
		# 6D pose with first 3 translation values and last 3 euler angles in radian about x,y,z-axes. (1x6)

	# Generate random translations.
	x_trans, y_trans, z_trans = 0.4*np.random.uniform()-0.2, 0.4*np.random.uniform()-0.2, 0.4*np.random.uniform()-0.2	
	# Generate random rotation angles.
	x_rot, y_rot, z_rot = (np.pi/9)*np.random.uniform()-(np.pi/18), (np.pi/9)*np.random.uniform()-(np.pi/18), (np.pi/9)*np.random.uniform()-(np.pi/18)
	return [x_trans,y_trans,z_trans,x_rot,y_rot,z_rot]

# Generate random poses for each batch to train the network.
def generate_random_poses(batch_size):
	# Arguments:
		# Batch_size:		No of poses in the output
	# Output:
		# poses:			Randomly generated poses (batch_size x 6)

	poses = []
	for i in range(batch_size):
		x_trans, y_trans, z_trans = 2*np.random.uniform()-1, 2*np.random.uniform()-1, 2*np.random.uniform()-1										# Generate random translation
		x_rot, y_rot, z_rot = (np.pi)*np.random.uniform()-(np.pi/2), (np.pi)*np.random.uniform()-(np.pi/2), (np.pi)*np.random.uniform()-(np.pi/2)	# Generate random orientation
		poses.append([np.round(x_trans,4), np.round(y_trans,4), np.round(z_trans,4), np.round(x_rot,4), np.round(y_rot,4), np.round(z_rot,4)])		# round upto 4 decimal digits
	return np.array(poses)

def select_random_points(source_data, num_point):
	random_source_data = np.copy(source_data)
	idx = np.arange(random_source_data.shape[1])			# Find indexes of source data.
	np.random.shuffle(idx)									# Shuffle indexes.
	random_source_data = random_source_data[:,idx,:]		# Shuffle data as per shuffled indexes.
	return random_source_data[:,0:num_point,:]

def add_noise(source_data):
	for i in range(source_data.shape[0]):
		mean = 0
		for j in range(source_data.shape[1]):
			sigma = 0.04*np.random.random_sample()				# Generate random variance value b/w 0 to 0.1
			source_data[i,j,:] = source_data[i,j,:] + np.random.normal(mean, sigma, source_data[i,j].shape)	# Add gaussian noise.
	return source_data

###################### Tensor Operations #########################

def rotate_point_cloud_by_angle_y_tensor(data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  Nx3 array, original batch of point clouds
		Return:
		  Nx3 array, rotated batch of point clouds
	"""
	cosval = tf.cos(rotation_angle)
	sinval = tf.sin(rotation_angle)
	rotation_matrix = tf.reshape([[cosval, 0, sinval],[0, 1, 0],[-sinval, 0, cosval]], [3,3])
	data = tf.reshape(data, [-1, 3])
	rotated_data = tf.transpose(tf.tensordot(rotation_matrix, tf.transpose(data), [1,0]))
	return rotated_data

def rotate_point_cloud_by_angle_x_tensor(data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  Nx3 array, original batch of point clouds
		Return:
		  Nx3 array, rotated batch of point clouds
	"""
	cosval = tf.cos(rotation_angle)
	sinval = tf.sin(rotation_angle)
	rotation_matrix = tf.reshape([[1, 0, 0],[0, cosval, -sinval],[0, sinval, cosval]], [3,3])
	data = tf.reshape(data, [-1, 3])
	rotated_data = tf.transpose(tf.tensordot(rotation_matrix, tf.transpose(data), [1,0]))
	return rotated_data

def rotate_point_cloud_by_angle_z_tensor(data, rotation_angle):
	""" Rotate the point cloud along up direction with certain angle.
		Input:
		  Nx3 array, original batch of point clouds
		Return:
		  Nx3 array, rotated batch of point clouds
	"""
	cosval = tf.cos(rotation_angle)
	sinval = tf.sin(rotation_angle)
	rotation_matrix = tf.reshape([[cosval, -sinval, 0],[sinval, cosval, 0],[0, 0, 1]], [3,3])
	data = tf.reshape(data, [-1, 3])
	rotated_data = tf.transpose(tf.tensordot(rotation_matrix, tf.transpose(data), [1,0]))
	return rotated_data

def translate_tensor(data,shift):
	# Add the translation vector to given tensor. (num_point x 3)
	return tf.add(data,shift)	

# Tranform the data as per given poses with orientation as euler in degrees.
def transformation_tensor(datas,poses):
	# Arguments:
		# datas: 				Tensor of Point Cloud (batch_size x num_points x 3)
		# poses: 				Tensor of Poses (translation + euler angles in degrees) (batch_size x num_points x 3)
	# Ouput:
		# transformed_data:		Tensor of transformed point cloud (batch_size x num_points x 3)

	transformed_data = tf.zeros([datas.shape[1], datas.shape[2]])		# Tensor to store the transformed point clouds as tensor.
	for i in range(datas.shape[0]):
		transformed_data_t = rotate_point_cloud_by_angle_x_tensor(datas[i,...],poses[i,3])				# Rotate about x-axis
		transformed_data_t = rotate_point_cloud_by_angle_y_tensor(transformed_data_t,poses[i,4])		# Rotate about y-axis
		transformed_data_t = rotate_point_cloud_by_angle_z_tensor(transformed_data_t,poses[i,5])		# Rotate about z-axis
		transformed_data_t = translate_tensor(transformed_data_t,[poses[i,0],poses[i,1],poses[i,2]])	# Translate by given vector.
		transformed_data = tf.concat([transformed_data, transformed_data_t], 0)							# Append the transformed tensor point cloud.
	transformed_data = tf.reshape(transformed_data, [-1, datas.shape[1], datas.shape[2]])[1:]			# Reshape the transformed tensor and remove first one. (batch_size x num_point x 3)
	return transformed_data

# Tranform the data as per given poses with orientation as quaternion.
def transformation_quat_tensor(data,quat,translation):
	# Arguments:
		# data:					Tensor of Point Cloud. (batch_size x num_point x 3)
		# quat:					Quaternion tensor to generate rotation matrix.	(batch_size x 4)
		# translation:			Translation tensor to translate the point cloud. (batch_size x 3)
	# Outputs:
		# transformed_data: 	Tensor of Rotated and Translated Point Cloud Data. (batch_size x num_points x 3)

	transformed_data = tf.zeros([data.shape[1],3])		# Tensor to store transformed data.
	for i in range(quat.shape[0]):
		# Seperate each quaternion value.
		q0 = tf.slice(quat,[i,0],[1,1])
		q1 = tf.slice(quat,[i,1],[1,1])
		q2 = tf.slice(quat,[i,2],[1,1])
		q3 = tf.slice(quat,[i,3],[1,1])
		# Convert quaternion to rotation matrix.
		# Ref: 	http://www-evasion.inrialpes.fr/people/Franck.Hetroy/Teaching/ProjetsImage/2007/Bib/besl_mckay-pami1992.pdf
			  # A method for Registration of 3D shapes paper by Paul J. Besl and Neil D McKay.
		R = [[q0*q0+q1*q1-q2*q2-q3*q3, 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)],
			 [2*(q1*q2+q0*q3), q0*q0+q2*q2-q1*q1-q3*q3, 2*(q2*q3-q0*q1)],
			 [2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), q0*q0+q3*q3-q1*q1-q2*q2]]

		R = tf.reshape(R,[3,3]) 			# Convert R into a single tensor of shape 3x3.
		# tf.tensordot: Arg: tensor1, tensor2, axes
		# axes defined for tensor1 & tensor2 should be of same size.
		# axis 1 of R is of size 3 and axis 0 of data (3xnum_points) is of size 3.

		temp_rotated_data = tf.transpose(tf.tensordot(R, tf.transpose(data[i,...]), [1,0]))		# Rotate the data. (num_points x 3)
		temp_rotated_data = tf.add(temp_rotated_data,translation[i,...])						# Add the translation (num_points x 3)
		transformed_data = tf.concat([transformed_data, temp_rotated_data],0)					# Append data (batch_size x num_points x 3)
	transformed_data = tf.reshape(transformed_data, [-1,data.shape[1],3])[1:]					# Reshape data and remove first point cloud. (batch_size x num_point x 3)
	return transformed_data


###################### Display Operations #########################

# Display data inside ModelNet files.
def display_clouds(filename,model_no):
	# Arguments:
		# filename:			Name of file to read the data from. (string)
		# model_no:			Number to choose the model inside that file. (int)

	data = []
	# Read the entire data from that file.
	with open(os.path.join('data','templates',filename),'r') as csvfile:
		csvreader = csv.reader(csvfile)
		for row in csvreader:
			row = [float(x) for x in row]
			data.append(row)
	fig = plt.figure()
	ax = fig.add_subplot(111,projection='3d')
	data = np.asarray(data)

	start_idx = model_no*2048
	end_idx = (model_no+1)*2048
	data = data[start_idx:end_idx,:]		# Choose specific data related to the given model number.

	X,Y,Z = [],[],[]
	for row in data:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	ax.scatter(X,Y,Z)
	plt.show()

# Display given Point Cloud Data in blue color (default).
def display_clouds_data(data):
	# Arguments:
		# data: 		array of point clouds (num_points x 3)

	fig = plt.figure()
	ax = fig.add_subplot(111,projection='3d')
	try:
		data = data.tolist()
	except:
		pass
	X,Y,Z = [],[],[]
	for row in data:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	ax.scatter(X,Y,Z)
	plt.show()

# Display given template, source and predicted point cloud data.
def display_three_clouds(data1,data2,data3,title):
	# Arguments:
		# data1 		Template Data (num_points x 3) (Red)
		# data2			Source Data (num_points x 3) (Green)
		# data3			Predicted Data (num_points x 3) (Blue)

	fig = plt.figure()
	ax = fig.add_subplot(111,projection='3d')
	try:
		data1 = data1.tolist()
		data2 = data2.tolist()
		data3 = data3.tolist()
	except:
		pass
	# Add Template Data in Plot
	X,Y,Z = [],[],[]
	for row in data1:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l1 = ax.scatter(X,Y,Z,c=[1,0,0,1])
	# Add Source Data in Plot
	X,Y,Z = [],[],[]
	for row in data2:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l2 = ax.scatter(X,Y,Z,c=[0,1,0,0.5])
	# Add Predicted Data in Plot
	X,Y,Z = [],[],[]
	for row in data3:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l3 = ax.scatter(X,Y,Z,c=[0,0,1,0.5])

	# Add details to Plot.
	plt.legend((l1,l2,l3),('Template Data','Source Data','Predicted Data'),prop={'size':15},markerscale=4)
	ax.tick_params(labelsize=10)
	ax.set_xlabel('X-axis',fontsize=15)
	ax.set_ylabel('Y-axis',fontsize=15)
	ax.set_zlabel('Z-axis',fontsize=15)
	# ax.set_xlim(-1,1.25)
	# ax.set_ylim(-1,1)
	# ax.set_zlim(-0.5,1.25)
	plt.title(title,fontdict={'fontsize':25})
	ax.xaxis.set_tick_params(labelsize=15)
	ax.yaxis.set_tick_params(labelsize=15)
	ax.zaxis.set_tick_params(labelsize=15)
	plt.show()

# Display template, source, predicted point cloud data with results after each iteration.
def display_itr_clouds(data1,data2,data3,ITR,title):
	# Arguments:
		# data1 		Template Data (num_points x 3) (Red)
		# data2			Source Data (num_points x 3) (Green)
		# data3			Predicted Data (num_points x 3) (Blue)
		# ITR 			Point Clouds obtained after each iteration (iterations x batch_size x num of points x 3) (Yellow)

	fig = plt.figure()
	ax = fig.add_subplot(111,projection='3d')
	print(ITR.shape)		# Display Number of Point Clouds in ITR.
	try:
		data1 = data1.tolist()
		data2 = data2.tolist()
		data3 = data3.tolist()
	except:
		pass
	# Add Template Data in Plot
	X,Y,Z = [],[],[]
	for row in data1:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l1 = ax.scatter(X,Y,Z,c=[1,0,0,1])
	# Add Source Data in Plot
	X,Y,Z = [],[],[]
	for row in data2:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l2 = ax.scatter(X,Y,Z,c=[0,1,0,1])
	# Add Predicted Data in Plot
	X,Y,Z = [],[],[]
	for row in data3:
		X.append(row[0])
		Y.append(row[1])
		Z.append(row[2])
	l3 = ax.scatter(X,Y,Z,c=[0,0,1,1])
	# Add point clouds after each iteration in Plot.
	for itr_data in ITR:
		X,Y,Z = [],[],[]
		for row in itr_data[0]:
			X.append(row[0])
			Y.append(row[1])
			Z.append(row[2])
		ax.scatter(X,Y,Z,c=[1,1,0,0.5])

	# Add details to Plot.
	plt.legend((l1,l2,l3),('Template Data','Source Data','Predicted Data'),prop={'size':15},markerscale=4)
	ax.tick_params(labelsize=10)
	ax.set_xlabel('X-axis',fontsize=15)
	ax.set_ylabel('Y-axis',fontsize=15)
	ax.set_zlabel('Z-axis',fontsize=15)
	plt.title(title,fontdict={'fontsize':25})
	ax.xaxis.set_tick_params(labelsize=15)
	ax.yaxis.set_tick_params(labelsize=15)
	ax.zaxis.set_tick_params(labelsize=15)
	plt.show()

# Log test results to a folder
def log_test_results(LOG_DIR, filename, log):

	# It will log the data in following sequence in csv format:
	# Sr. No., time taken, number of iterations, translation error, rotation error.

	# If log dir doesn't exists create one.
	if not os.path.exists(LOG_DIR): os.mkdir(LOG_DIR)

	# Find params from the dictionary.
	ITR, TIME = log['ITR'], log['TIME']
	Trans_Err, Rot_Err = log['Trans_Err'], log['Rot_Err']
	idxs_5_5, idxs_10_1, idxs_20_2 = log['idxs_5_5'], log['idxs_10_1'], log['idxs_20_2']
	num_batches = log['num_batches']

	# Find mean and variance.
	TIME_mean = sum(TIME)/len(TIME)

	# Log all the data in a csv file.
	import csv
	with open(os.path.join(LOG_DIR, 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]])

	if len(idxs_5_5) != 0:
		accuray_5_5 = len(idxs_5_5)/(num_batches*1.0)
		mean_5_5_rot_err = np.sum(np.array(Rot_Err)[idxs_5_5])/len(idxs_5_5)
		var_5_5_rot_err = np.var(np.array(Rot_Err)[idxs_5_5])
		mean_5_5_trans_err = np.sum(np.array(Trans_Err)[idxs_5_5])/len(idxs_5_5)
		var_5_5_trans_err = np.var(np.array(Trans_Err)[idxs_5_5])
		mean_5_5_itr = np.sum(np.array(ITR)[idxs_5_5])/len(idxs_5_5)
		var_5_5_itr = np.var(np.array(ITR)[idxs_5_5])
		mean_5_5_time = np.sum(np.array(TIME)[idxs_5_5])/len(idxs_5_5)
		var_5_5_time = np.var(np.array(TIME)[idxs_5_5])
	else:
		accuray_5_5, mean_5_5_rot_err, var_5_5_rot_err, mean_5_5_trans_err, var_5_5_trans_err, mean_5_5_itr, var_5_5_itr, mean_5_5_time, var_5_5_time = 0, 0, 0, 0, 0, 0, 0, 0, 0

	if len(idxs_10_1) != 0:
		accuray_10_1 = len(idxs_10_1)/(num_batches*1.0)
		mean_10_1_rot_err = np.sum(np.array(Rot_Err)[idxs_10_1])/len(idxs_10_1)
		var_10_1_rot_err = np.var(np.array(Rot_Err)[idxs_10_1])
		mean_10_1_trans_err = np.sum(np.array(Trans_Err)[idxs_10_1])/len(idxs_10_1)
		var_10_1_trans_err = np.var(np.array(Trans_Err)[idxs_10_1])
		mean_10_1_itr = np.sum(np.array(ITR)[idxs_10_1])/len(idxs_10_1)
		var_10_1_itr = np.var(np.array(ITR)[idxs_10_1])
		mean_10_1_time = np.sum(np.array(TIME)[idxs_10_1])/len(idxs_10_1)
		var_10_1_time = np.var(np.array(TIME)[idxs_10_1])
	else:
		accuray_10_1, mean_10_1_rot_err, var_10_1_rot_err, mean_10_1_trans_err, var_10_1_trans_err, mean_10_1_itr, var_10_1_itr, mean_10_1_time, var_10_1_time = 0, 0, 0, 0, 0, 0, 0, 0, 0		
		
	if len(idxs_20_2) != 0:
		# Find accuracies:
		accuray_20_2 = len(idxs_20_2)/(num_batches*1.0)
		# Find mean rotation error.
		mean_20_2_rot_err = np.sum(np.array(Rot_Err)[idxs_20_2])/len(idxs_20_2)
		# Find variance of rotation error.
		var_20_2_rot_err = np.var(np.array(Rot_Err)[idxs_20_2])
		# Find mean translation error.
		mean_20_2_trans_err = np.sum(np.array(Trans_Err)[idxs_20_2])/len(idxs_20_2)
		# Find variance of translation error.
		var_20_2_trans_err = np.var(np.array(Trans_Err)[idxs_20_2])
		# Find mean iterations.
		mean_20_2_itr = np.sum(np.array(ITR)[idxs_20_2])/len(idxs_20_2)
		# Find variance of iterations.
		var_20_2_itr = np.var(np.array(ITR)[idxs_20_2])
		# Find mean time required.
		mean_20_2_time = np.sum(np.array(TIME)[idxs_20_2])/len(idxs_20_2)
		# Find variance of time.
		var_20_2_time = np.var(np.array(TIME)[idxs_20_2])
	else:
		accuray_20_2, mean_20_2_rot_err, var_20_2_rot_err, mean_20_2_trans_err, var_20_2_trans_err, mean_20_2_itr, var_20_2_itr, mean_20_2_time, var_20_2_time = 0, 0, 0, 0, 0, 0, 0, 0, 0

	with open(os.path.join(LOG_DIR, filename+'.txt'),'w') as file:
		file.write("Mean of Time: {}\n".format(TIME_mean))

		file.write("\n")

		file.write("###### 5 Degree & 0.05 Units ######\n")
		file.write("Accuray: {}%\n".format(accuray_5_5*100))
		file.write("Mean rotational error: {}\n".format(mean_5_5_rot_err))
		file.write("Mean translation error: {}\n".format(mean_5_5_trans_err))
		file.write("Mean time: {}\n".format(mean_5_5_time))
		file.write("Var time: {}\n".format(var_5_5_time))
		file.write("Var translation error: {}\n".format(var_5_5_trans_err))
		file.write("Var rotational error: {}\n".format(var_5_5_rot_err))
		file.write("Mean Iterations: {}\n".format(mean_5_5_itr))
		file.write("Var Iterations: {}\n".format(var_5_5_itr))

		file.write("\n")

		file.write("###### 10 Degree & 0.1 Units ######\n")
		file.write("Accuray: {}%\n".format(accuray_10_1*100))
		file.write("Mean rotational error: {}\n".format(mean_10_1_rot_err))
		file.write("Mean translation error: {}\n".format(mean_10_1_trans_err))
		file.write("Mean time: {}\n".format(mean_10_1_time))
		file.write("Var time: {}\n".format(var_10_1_time))
		file.write("Var translation error: {}\n".format(var_10_1_trans_err))
		file.write("Var rotational error: {}\n".format(var_10_1_rot_err))
		file.write("Mean Iterations: {}\n".format(mean_10_1_itr))
		file.write("Var Iterations: {}\n".format(var_10_1_itr))

		file.write("\n")

		file.write("###### 20 Degree & 0.2 Units ######\n")
		file.write("Accuray: {}%\n".format(accuray_20_2*100))
		file.write("Mean rotational error: {}\n".format(mean_20_2_rot_err))
		file.write("Mean translation error: {}\n".format(mean_20_2_trans_err))
		file.write("Mean time: {}\n".format(mean_20_2_time))
		file.write("Var time: {}\n".format(var_20_2_time))
		file.write("Var translation error: {}\n".format(var_20_2_trans_err))
		file.write("Var rotational error: {}\n".format(var_20_2_rot_err))
		file.write("Mean Iterations: {}\n".format(mean_20_2_itr))
		file.write("Var Iterations: {}\n".format(var_20_2_itr))

	plt.hist(Rot_Err,np.arange(0,185,5))
	plt.xlim(0,180)
	plt.savefig(os.path.join(LOG_DIR,'rot_err_hist.jpeg'),dpi=500,quality=100)
	plt.figure()
	plt.hist(Trans_Err,np.arange(0,1.01,0.01))
	plt.xlim(0,1)
	plt.savefig(os.path.join(LOG_DIR,'trans_err_hist.jpeg'),dpi=500,quality=100)




if __name__=='__main__':
	# a = np.array([[0,0,0,0,0,0],[0,0,0,90,0,0]])
	# print a.shape
	# a = poses_euler2quat(a)
	# print(a[1,3]*a[1,3]+a[1,4]*a[1,4]+a[1,5]*a[1,5]+a[1,6]*a[1,6])
	# print(a[0,3]*a[0,3]+a[0,4]*a[0,4]+a[0,5]*a[0,5]+a[0,6]*a[0,6])
	# print a.shape
	# display_clouds('airplane_templates.csv',0)

	templates = helper.process_templates('multi_model_templates')
	# templates = helper.process_templates('templates')
	# airplane = templates[0,:,:]
	idx = 199
	fig = plt.figure()
	ax = fig.add_subplot(111,projection='3d')
	# start = idx*2048
	# end = (idx+1)*2048
	ax.scatter(templates[idx,:,0],templates[idx,:,1],templates[idx,:,2])
	plt.show()
	print(templates.shape)