%function h3d_data_analyze

h3d_results = load('H3Ddata');

truth = load('data_transformation_h3d.txt');

n_experiments = size(h3d_results,1);

error_angle = zeros(n_experiments,1);
error_translation = zeros(n_experiments,1);

debug_true_angle = zeros(n_experiments,1);
debug_est_angle = zeros(n_experiments,1);

for i = 1:n_experiments
    
    
    recovered_rotation(1,:) = h3d_results(i,3:5);
    recovered_rotation(2,:) = h3d_results(i,6:8);
    recovered_rotation(3,:) = h3d_results(i,9:11);
    true_rotation = zeros(3,3);
    true_rotation(:,1) = truth(i,2:4)';
    true_rotation(:,2) = truth(i,6:8)';
    true_rotation(:,3) = truth(i,10:12)';
    
	rot_error_deg = h3d_rotation_error(true_rotation, recovered_rotation);
    
  	error_angle(i) = rot_error_deg;

    

	true_translation = truth(i,14:16)';
    recovered_translation = h3d_results(i,13:15)';
	
	est_translation = recovered_translation;


    error_translation(i) =  norm(true_translation-est_translation);




    [v_true,theta_true] = rot_matrix_to_axis_angle(true_rotation);	
 	[v_icp,theta_icp] = rot_matrix_to_axis_angle(recovered_rotation);


	debug_est_angle(i) = rad2deg(theta_icp);
	debug_true_angle(i) = rad2deg(theta_true);

end


name = 'HSM3D';
prefix = 'h3d';
common_data_plot