function h3d_data_analyze

h3d_results = load('H3Ddata');

truth = load('data_transformation_h3d.txt');

n_experiments = size(icp_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_rotation'*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

% sort everyrhing

size(debug_true_angle)
[Y,I] = sort(debug_true_angle, 'ascend');

debug_true_angle = debug_true_angle(I);
debug_est_angle = debug_est_angle(I);
error_translation = error_translation(I);
error_angle = error_angle(I);

figure ; 
subplot(3,1,1);
plot(error_translation)
title('error in translation (m)')

subplot(3,1,2);
plot(error_angle)
title('error in rotation (degrees)')

subplot(3,1,3); hold on;
plot(debug_est_angle, 'r');
plot(debug_true_angle, 'k');
legend('estimated','true');
title('absolute rotations')