%function h3d_data_analyze

h3d_results = load('H3Ddata_noise');

truth = load('data_transformation_h3d_noise.txt');

rot_angles = [15 30 45 60 75 90 105 120 135 150 165 180];

NTESTS = 20;

sigma = [0.005 0.01 0.02 0.03 0.04 0.05 0.06 0.07];

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

angle_error_matrix = zeros(numel(sigma),numel(rot_angles));
trans_error_matrix = zeros(numel(sigma),numel(rot_angles));



for i = 1:size(angle_error_matrix,1)
    start_pos_noise = (i-1)*(NTESTS*numel(rot_angles));
    for j = 1:size(angle_error_matrix,2)
        start_pos_angle = (j-1)*NTESTS+1;
        end_pos_angle = j*NTESTS;
        angle_error_matrix(i,j) = mean(error_angle(start_pos_noise+start_pos_angle:start_pos_noise+end_pos_angle));
        trans_error_matrix(i,j) = mean(error_translation(start_pos_noise+start_pos_angle:start_pos_noise+end_pos_angle));
    end
end

angle_sigma_error = mean(angle_error_matrix');
trans_sigma_error = mean(trans_error_matrix');

% sort everyrhing

common_data_plot