clc
clear
close all
addpath(genpath('../'))
rng(6)
%% DEFINE ALL PARAMETERS

% sample size
N_samples = 1;

% paramters of calibration
N_frames = 6;

sigma_s = 0.3;
% ROUGH DEFORMATION OF TARGET HAND
scale_x = 1.0 + sigma_s*(2*rand -1);
scale_y = 1.0 + sigma_s*(2*rand -1);
scale_z = 1.0 + sigma_s*(2*rand -1);
fat_factor = 1.0 + sigma_s*(2*rand -1);
finger_length_factor = 1.0 + sigma_s*(2*rand -1);
alpha = [scale_x; scale_y; scale_z];


% FINER SCALE DEFORMATION OF TARGET HAND

sigma_radii = 2.0;
sigma_beta = 2.5;
sigma_center = 3.0;

% POSE PARAMETERS
sigma_theta = 0.05;
sigma_rotation = 0.0;
sigma_translation = 0;

% OPTIMIZATION PARAMETERS
N_iter_unif = 5;
N_iter_full = 10;

% NUMBER OF POINTS POINT CLOUD - the lower the higher
downscaling = 6;


%% DEFINE CONTAINERS FOR ERROR

error = zeros(N_samples, N_iter_unif + N_iter_full);
error_r_f = zeros(N_samples, N_iter_unif + N_iter_full);
error_r_p = zeros(N_samples, N_iter_unif + N_iter_full);
error_c_f = zeros(N_samples, N_iter_unif + N_iter_full);
error_c_p = zeros(N_samples, N_iter_unif + N_iter_full);
error_beta = zeros(N_samples, N_iter_unif + N_iter_full);

%% LOAD HAND MODEL

load('hand_model.mat');
[hand_model] = reindex_fullhand(hand_model);

for iter_samples = 1 : N_samples
    
   scale_x = 1.0 + sigma_s*(2*rand -1);
scale_y = 1.0 + sigma_s*(2*rand -1);
scale_z = 1.0 + sigma_s*(2*rand -1);
fat_factor = 1.0 + sigma_s*(2*rand -1);
finger_length_factor = 1.0 + sigma_s*(2*rand -1);
alpha = [scale_x; scale_y; scale_z];
    
    iter_samples
    
    %% SET UP VECTOR OF POSES

    vector_pose{1}.global_rotation = [0.0;0.0;0.0];
    vector_pose{1}.global_translation = [0.0;0.0;0.0];
    vector_pose{2}.global_rotation = [0.2;0.2;0.0];
    vector_pose{2}.global_translation = [0.0;0.0;0.0];
    vector_pose{3}.global_rotation = [0.0;0.0;pi];
    vector_pose{3}.global_translation = [0.0;0.0;0.0];
    vector_pose{4}.global_rotation = [0.0;0.5;pi];
    vector_pose{4}.global_translation = [0.0;0.0;0.0];
    vector_pose{5}.global_rotation = [0.1;0.0;pi/6];
    vector_pose{5}.global_translation = [0.0;0.0;0.0];
    vector_pose{6}.global_rotation = [-0.3;0.0;-0.4];
    vector_pose{6}.global_translation = [0.0;0.0;0.0];


    vector_hands = {};
    ddelta_theta{1} = {[-0.2;0.0;-0.3;-0.4], [-0.2;0.0;-0.3;-0.4],  [-0.2;0.0;-0.3;-0.4],  [-0.2;0.0;-0.3;-0.4], [-0.2;0.0;-0.3;-0.4]};
    ddelta_theta{2} = {[-0.2;0.0;-0.3;-0.4], [-0.2;0.0;-0.6;-0.6],  [-0.2;0.0;-0.6;-0.6],  [-0.2;0.0;-0.6;-0.6], [-0.2;0.0;-0.6;-0.6]};
    ddelta_theta{3} = {[-0.0;0.0;-0.0;-0.0], [-0.0;0.0;-0.0;-0.0],  [-0.0;0.0;-0.0;-0.0],  [-0.2;0.0;-0.0;-0.0], [-0.0;-0.1;-0.0;-0.0]};
    ddelta_theta{4} = {[-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1)};
    ddelta_theta{5} = {[-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1)};
    ddelta_theta{6} = {[-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1),  [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1), [-0.2;0.0;-0.3;-0.4]+0.2*rand(4,1)};
    
    
    
    
    for i = 1:N_frames   
     
     [ p_hand_model ] = pose_model(hand_model, ddelta_theta{i},vector_pose{i} );
     [ p_hand_model ] = update_membranes( p_hand_model );
     vector_hands{i} = p_hand_model;   
    end

     %% GENERATE TARGET_HAND

    % Generate target model
    target_hand_model = hand_model;

    target_hand_model.beta = { finger_length_factor*target_hand_model.beta{1}, finger_length_factor*target_hand_model.beta{2},  finger_length_factor*target_hand_model.beta{3}, finger_length_factor*target_hand_model.beta{4}, finger_length_factor*target_hand_model.beta{5} };

    for j = 1:length(target_hand_model.finger_radii)
        target_hand_model.finger_radii{j} = fat_factor*target_hand_model.finger_radii{j};
    end

    for j = 1:length(target_hand_model.palm_wrist_radii)
        target_hand_model.palm_wrist_radii{j} = fat_factor*target_hand_model.palm_wrist_radii{j};
        target_hand_model.palm_wrist_centers_relative{j} = alpha.*target_hand_model.palm_wrist_centers_relative{j};
    end
    % update centers   
    [ target_hand_model ] = update_centers(target_hand_model);
    % perturbate finger centers
    delta_finger_center = {(alpha-1).*target_hand_model.segments{1}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{2}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{3}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{4}{1}.local(1:3,4), (alpha-1).*target_hand_model.segments{5}{1}.local(1:3,4)};
    % update shape
    [ target_hand_model.segments ] = update_fingers_shape(target_hand_model.segments, target_hand_model.beta );    
    % update model pose
    dt = {zeros(4,1);zeros(4,1);zeros(4,1);zeros(4,1);zeros(4,1)};
    [ target_hand_model ] = update_fingers_pose(target_hand_model, dt, delta_finger_center );
    % update membranes
    [ target_hand_model ] = update_membranes( target_hand_model );  


    for i = 1 : length(target_hand_model.finger_radii)
        target_hand_model.finger_radii{i} = target_hand_model.finger_radii{i} + sigma_radii*rand;
    end

    for i = 1:5
        target_hand_model.beta{i} = target_hand_model.beta{i} + sigma_beta* (2*rand(size( hand_model.beta{i})) -1);
    end

    % displace palm
    for i = 1 : 13
        target_hand_model.palm_wrist_centers_relative{i} = target_hand_model.palm_wrist_centers_relative{i} + sigma_center*(2*rand(3,1) - 1);
        target_hand_model.palm_wrist_radii{i} = target_hand_model.palm_wrist_radii{i} + sigma_radii*(2*rand-1);
    end
    target_finger_center = {sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1), sigma_center*(2*rand(3,1) - 1)};
    % update centers   
    [ target_hand_model ] = update_centers(target_hand_model);
    [ target_hand_model.segments ] = update_fingers_shape(target_hand_model.segments, target_hand_model.beta );
    [ target_hand_model ] = update_fingers_pose(target_hand_model, {zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1),zeros(4,1)}, target_finger_center );


    %% CHANGE POSE & THETA OF EVERY TARGET HAND


    vector_target_hands = {};


    for j = 1:N_frames   
        for i = 1:5
            DTHETA{i} = ddelta_theta{j}{i}+ sigma_theta* [2*rand-1;0;2*rand(2,1)-1];   
        end

        % perturbate a bit global pose
        vector_pose{j}.global_rotation = vector_pose{j}.global_rotation + sigma_rotation*rand(3,1);     
        vector_pose{j}.global_translation = vector_pose{j}.global_translation + sigma_translation*rand(3,1);     

        [ p_target_hand_model ] = pose_model(target_hand_model, DTHETA,vector_pose{j} );
        for i = 1:5
            p_target_hand_model.theta{i} = p_target_hand_model.theta{i} + DTHETA{i};
        end
        [ p_target_hand_model ] = update_membranes( p_target_hand_model );
        vector_target_hands{j} = p_target_hand_model;   
    end


    %% CREATE POINT CLOUDS

    view_axis = 'Y';
    sigma_noise = 0.2;

    data_points = {};

    for j = 1:N_frames   
        data_points{j} = generate_synthetic_point_cloud(vector_target_hands{j}, downscaling,view_axis, sigma_noise);
    end


    %% PROJECT POINTS ON MODEL
    for j = 1:N_frames   
        [indices{j}, model_points{j}, block_indices{j}] = compute_projections(data_points{j}, vector_hands{j});
    end


    %% OPTIMIZE 

    tol = 1e-03;

    % SETTINGS FOR OPTIMIZATION PIPELINE
    settings.fov = 15;
    settings.H = 480/12;
    settings.W = 636/12;
    settings.D = 3;
    settings.sparse_data = false;
    settings.RAND_MAX = 32767;
    settings.side = 'front';
    settings.view_axis = view_axis;
    settings.block_safety_factor = 1.1;
    w_bounds = 400;
    w_val = 2000;
    w_stabilization_theta = 300;
    w_stabilization_radii = 100;

%% REDUCED PARAMETERS OPTIMIZATION

lambda = 10.0;
n_iter = 0;
target_delta = 2.0;
err = tol + 1;

disp('Optimization over reduced parameters')

while n_iter < N_iter_unif
    
    n_iter = n_iter + 1
    
   [ error(iter_samples, n_iter), error_r_f(iter_samples, n_iter), error_r_p(iter_samples, n_iter), error_c_f(iter_samples, n_iter), error_c_p(iter_samples, n_iter), error_beta(iter_samples, n_iter) ] = compute_errors( vector_hands{1}, vector_target_hands{1} );

    
     % compute Jacobian matrix and right hand side
    J_full = [];
    F_full = [];
    
    for i = 1 :(N_frames)
        % compute Jacobian matrix and right hand side
        [F_d2m, Jtheta_d2m, Jbeta_d2m, Jr_d2m, Jcenters_fingers_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Js_membrane_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m] = COMPUTE_D2M_ENERGY(vector_hands{i}, data_points{i}, model_points{i}, indices{i}, block_indices{i},true,false );
        [F_m2d, Jtheta_m2d, Jbeta_m2d, Jr_m2d, Jcenters_fingers_m2d, Jr_palm_m2d, Jcenters_palm_m2d, Js_membrane_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d] = COMPUTE_JACOBIAN_M2D(vector_hands{i}, data_points{i}, settings,false );
        % start by stacking full jacobian
        J = [Jr_d2m, Jr_palm_d2m, Jbeta_d2m, Jcenters_palm_d2m, Jcenters_fingers_d2m,zeros(length(F_d2m),26*(i-1)), Jtheta_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m,zeros(length(F_d2m),26*(N_frames-i)); Jr_m2d, Jr_palm_m2d, Jbeta_m2d, Jcenters_palm_m2d, Jcenters_fingers_m2d,zeros(length(F_m2d),26*(i-1)),Jtheta_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d,zeros(length(F_m2d),26*(N_frames-i)) ] ;
        F = [F_d2m;F_m2d];

        param_value = zeros( size(J));

        for j = 1:length(vector_hands{i}.finger_radii)
            param_value(:,j) = vector_hands{i}.finger_radii{j};
        end
        l1 = length(vector_hands{i}.finger_radii);
        for j = 1:length(vector_hands{i}.palm_wrist_radii)
           param_value(:,l1 + j) = vector_hands{i}.palm_wrist_radii{j};
        end
        l1 = l1 + length(vector_hands{i}.palm_wrist_radii);
        param_value(:, l1 + 1: l1 + 1 + length(vector_hands{i}.beta)*length(vector_hands{i}.beta{2})) = [ vector_hands{i}.beta{1}', vector_hands{i}.beta{2}',  vector_hands{i}.beta{3}', vector_hands{i}.beta{4}', vector_hands{i}.beta{5}' ].* ones(length(F),16);
        l2 = l1 + length(vector_hands{i}.beta)*length(vector_hands{i}.beta{2}) + 1;

        for j = 1:length( vector_hands{i}.palm_wrist_centers_relative)
            for k = 1:3
                param_value(:,l2 + 3*(j-1) + k) = vector_hands{i}.palm_wrist_centers_relative{j}(k);
            end
        end
        l3 = l2 + 3*length( vector_hands{i}.palm_wrist_centers_relative);
        for j = 1:length(vector_hands{i}.segments)
            for k = 1:3
            param_value(:,l3 + 3*(j-1) + k) = vector_hands{i}.segments{j}{1}.local(k,4);
            end
        end
        l3 = l3 + 3*length(vector_hands{i}.segments);

        param_value(:,l3+1:end) = 1;

        J = J.*param_value;
        J = [ sum(J(:,1:l1),2), sum(J(:,1+l1:l2),2), sum(J(:,1+l2:3:l3),2), sum(J(:,2+l2:3:l3),2), sum(J(:,3+l2:3:l3),2), J(:,l3+1:end); ];
        
        J_full = [J_full;J];
        F_full = [F_full;F];      
    end
    
    % add stabilization energy
    J_full = [J_full; zeros(N_frames*26,5),w_stabilization_theta*eye(N_frames*26,N_frames*26); w_stabilization_radii,zeros(1,4+26*N_frames)];
    F_full = [F_full; zeros(26*N_frames +1,1)];   
        
        
    % perform descent step      
    JtJ = J_full' * J_full;
    LHS = JtJ + lambda*eye(size(JtJ));
    delta = LHS \ (J_full' * F_full);

    err = norm(delta)
    lambda = target_delta/err * lambda;


    delta_radii = delta(1);
    delta_beta = delta(2);
    delta_alpha = delta(3:5);
    
    delta_theta = {};
    delta_pose = {};
    
    for i = 1 :(N_frames)
        delta_theta{i} = { delta(26*(i-1)+6:26*(i-1)+9), delta(26*(i-1)+10:26*(i-1)+13), delta(26*(i-1)+14:26*(i-1)+17), delta(26*(i-1)+18:26*(i-1)+21), delta(26*(i-1)+22:26*(i-1)+25)};
        delta_pose{i}.global_rotation = delta(26*(i-1)+26:26*(i-1)+28);
        delta_pose{i}.global_translation = delta(26*(i-1)+29:26*(i-1)+31);
    end

    % update
    for i = 1:(N_frames)
    
        for j = 1:length(vector_hands{i}.finger_radii)
           vector_hands{i}.finger_radii{j} = vector_hands{i}.finger_radii{j}*(1 + delta_radii);
        end

        for j = 1:length(vector_hands{i}.palm_wrist_radii)
            vector_hands{i}.palm_wrist_radii{j} = vector_hands{i}.palm_wrist_radii{j}*(1 + delta_radii);
            vector_hands{i}.palm_wrist_centers_relative{j} = vector_hands{i}.palm_wrist_centers_relative{j}.*(1 + delta_alpha);
        end
        
        vector_hands{i}.beta = { vector_hands{i}.beta{1}*(1 + delta_beta), vector_hands{i}.beta{2}*(1 + delta_beta),  vector_hands{i}.beta{3}*(1 + delta_beta), vector_hands{i}.beta{4}*(1 + delta_beta), vector_hands{i}.beta{5}*(1 + delta_beta) };

        delta_finger_center = {delta_alpha.*vector_hands{i}.segments{1}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{2}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{3}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{4}{1}.local(1:3,4), delta_alpha.*vector_hands{i}.segments{5}{1}.local(1:3,4)};

        % update parameters
        for j = 1:length(vector_hands{i}.theta)
            vector_hands{i}.theta{j} = vector_hands{i}.theta{j} + delta_theta{i}{j};
        end

        % update centers
        [ vector_hands{i} ] = update_centers(vector_hands{i});
        % update shape
        [ vector_hands{i}.segments ] = update_fingers_shape(vector_hands{i}.segments, vector_hands{i}.beta ); 

        % update model pose
        [ vector_hands{i} ] = update_fingers_pose(vector_hands{i}, delta_theta{i}, delta_finger_center );
        delta_theta{i} = {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
        [ vector_hands{i} ] = pose_model(vector_hands{i}, delta_theta{i},delta_pose{i} );

        % update membranes
        [ vector_hands{i} ] = update_membranes( vector_hands{i} );   

        % compute updated correspondencies
        [indices{i}, model_points{i}, block_indices{i}] = compute_projections(data_points{i}, vector_hands{i});
    end   
        
end

    %% FULL OPTIMIZATION

%     settings.H = 480/3;
%     settings.W = 636/3;
%     
disp('Full optimization')

lambda = 100.0;


n_iter = 0;
target_delta = 0.2;
err = tol + 1;

bound_factor = 1.1;
weight = 800;
w_stabilization_radii = 0;

disp('Full optimization')

while err > tol && n_iter < 5
    
    n_iter = n_iter + 1
    [ error(iter_samples, n_iter+N_iter_unif), error_r_f(iter_samples, n_iter+N_iter_unif), error_r_p(iter_samples, n_iter+N_iter_unif), error_c_f(iter_samples, n_iter+N_iter_unif), error_c_p(iter_samples, n_iter+N_iter_unif), error_beta(iter_samples, n_iter+N_iter_unif) ] = compute_errors( vector_hands{1}, vector_target_hands{1} );
    
    % compute Jacobian matrix and right hand side
    J_full = [];
    F_full = [];
    
    for i = 1:N_frames
    
        % compute Jacobian matrix and right hand side
        [F_d2m, Jtheta_d2m, Jbeta_d2m, Jr_d2m, Jcenters_fingers_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Js_membrane_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m] = COMPUTE_D2M_ENERGY(vector_hands{i}, data_points{i}, model_points{i}, indices{i}, block_indices{i},true,false );
        [F_m2d, Jtheta_m2d, Jbeta_m2d, Jr_m2d, Jcenters_fingers_m2d, Jr_palm_m2d, Jcenters_palm_m2d, Js_membrane_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d] = COMPUTE_JACOBIAN_M2D(vector_hands{i}, data_points{i}, settings,false );

        J = [Jbeta_d2m, Jr_d2m, Jcenters_palm_d2m, Jr_palm_d2m, Jcenters_fingers_d2m, zeros(length(F_d2m),26*(i-1)), Jtheta_d2m, Jglobal_rotation_d2m, Jglobal_translation_d2m,zeros(length(F_d2m),26*(N_frames-i)); Jbeta_m2d, Jr_m2d, Jcenters_palm_m2d, Jr_palm_m2d, Jcenters_fingers_m2d, zeros(length(F_m2d),26*(i-1)), Jtheta_m2d, Jglobal_rotation_m2d, Jglobal_translation_m2d,zeros(length(F_m2d),26*(N_frames-i))] ;
        F = [F_d2m;F_m2d;];
        
        % store in full matrix
        J_full = [J_full;J];
        F_full = [F_full;F];       
    end
    
    % add stabilization energies
    J_full = [J_full; zeros(N_frames*26,104),w_stabilization_theta*eye(N_frames*26,N_frames*26)];
    F_full = [F_full; zeros(26*N_frames,1)];   
    
                
    % perform descent step      
    JtJ = J_full' * J_full;
    LHS = JtJ + lambda*eye(size(JtJ));
    delta = LHS \ (J_full' * F_full);
        
    err = norm(delta)
    lambda = err/target_delta * lambda;
       
    delta_radii = delta(17:37);
    delta_centers = delta (38: 76);
    delta_radii_palm = delta(77 : 89);
    delta_finger_center_vec = delta( 90 : 104 );
    
    delta_theta = {};
    delta_pose = {};
    for i = 1 :N_frames        
        delta_theta{i} = { delta(26*(i-1)+105:26*(i-1)+108), delta(26*(i-1)+109:26*(i-1)+112), delta(26*(i-1)+113:26*(i-1)+116), delta(26*(i-1)+117:26*(i-1)+120), delta(26*(i-1)+121:26*(i-1)+124)};
        delta_pose{i}.global_rotation = delta(26*(i-1)+125:26*(i-1)+127);
        delta_pose{i}.global_translation = delta(26*(i-1)+128:26*(i-1)+130);
    end
    
    %update stuff
    for i = 1 :N_frames
        
         vector_hands{i}.beta = { vector_hands{i}.beta{1} + delta(1:4), vector_hands{i}.beta{2} + delta(5:7),  vector_hands{i}.beta{3} + delta(8:10), vector_hands{i}.beta{4} + delta(11:13), vector_hands{i}.beta{5} + delta(14:16) };
        % update parameters
        for j = 1:length(vector_hands{i}.theta)
            vector_hands{i}.theta{j} = vector_hands{i}.theta{j} + delta_theta{i}{j};
        end

        for j = 1:length(vector_hands{i}.finger_radii)
            vector_hands{i}.finger_radii{j} = max(vector_hands{i}.finger_radii{j} + delta_radii(j),0.1);
        end
        
        indices_ok = [vector_hands{i}.palm_wrist_names_map('wrist_bottom_left'),vector_hands{i}.palm_wrist_names_map( 'wrist_bottom_right'),vector_hands{i}.palm_wrist_names_map('wrist_top_left'),vector_hands{i}.palm_wrist_names_map( 'wrist_top_right')];
        
        for j = 1:length(vector_hands{i}.palm_wrist_radii)
            if ~ismember(j,indices_ok)
                vector_hands{i}.palm_wrist_radii{j} = vector_hands{i}.palm_wrist_radii{j} + delta_radii_palm(j);
                vector_hands{i}.palm_wrist_centers_relative{j} = vector_hands{i}.palm_wrist_centers_relative{j} +  delta_centers( (3*(j-1) +1) : 3*j);
            end
        end
        delta_finger_center = {delta_finger_center_vec(1:3), delta_finger_center_vec(4:6), delta_finger_center_vec(7:9), delta_finger_center_vec(10:12), delta_finger_center_vec(13:15)};


        % update centers
        [ vector_hands{i} ] = update_centers(vector_hands{i});
        % update shape
        [ vector_hands{i}.segments ] = update_fingers_shape(vector_hands{i}.segments, vector_hands{i}.beta );    
        % update model pose
        [ vector_hands{i} ] = update_fingers_pose(vector_hands{i}, delta_theta{i}, delta_finger_center );
        delta_theta{i}= {zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1), zeros(4,1)};
        [ vector_hands{i} ] = pose_model(vector_hands{i}, delta_theta{i},delta_pose{i} );
        % update membranes
        [ vector_hands{i} ] = update_membranes( vector_hands{i} );  
        [vector_hands{i}] = reindex_fullhand(vector_hands{i});
        % compute updated correspondencies
        [indices{i}, model_points{i}, block_indices{i}] = compute_projections(data_points{i}, vector_hands{i});
    end    
end
end


save('convergence.mat')
%% VISUALIZE CONVERGENCE RESULTS

close all
load('convergence.mat')

iterations = 0: N_iter_full+N_iter_unif-1;


mean_error = mean(error,1);
var_error = var(error,1);

mean_error_r_f = mean(error_r_f, 1);
var_error_r_f = var(error_r_f, 1);

mean_error_r_p = mean(error_r_p, 1);
var_error_r_p = var(error_r_p, 1);

mean_error_c_f = mean(error_c_f, 1);
var_error_c_f = var(error_c_f, 1);

mean_error_c_p = mean(error_c_p, 1);
var_error_c_p = var(error_c_p, 1);

mean_error_beta = mean(error_beta, 1);
var_error_beta = var(error_beta, 1);

z_star = 0.2; %corresponding to p = 99%


figure

subplot(2,3,1)
plot(iterations,mean_error,'red','LineWidth',2)
hold on
grid on
plot(iterations,mean_error + z_star/sqrt(N_samples) * var_error, '--r')
plot(iterations,mean_error - z_star/sqrt(N_samples) * var_error, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error + z_star/sqrt(N_iter_unif + N_iter_full) * var_error )];
%plot(x0,y0,'green', 'LineWidth',2)
title('\Theta _s');
xlabel('iterations')
ylabel('E_{calib}')


subplot(2,3,2)
plot(iterations,mean_error_r_f,'red','LineWidth',2)
hold on
grid on
 plot(iterations,mean_error_r_f + z_star/sqrt(N_samples) * var_error_r_f, '--r')
 plot(iterations,mean_error_r_f - z_star/sqrt(N_samples) * var_error_r_f, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_r_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_f)];
%plot(x0,y0,'green', 'LineWidth',2)
title('r_{finger}');
xlabel('iterations')
ylabel('E_{calib}')

subplot(2,3,3)
plot(iterations,mean_error_r_p,'red','LineWidth',2)
hold on
grid on
 plot(iterations,mean_error_r_p + z_star/sqrt(N_samples) * var_error_r_p, '--r')
 plot(iterations,mean_error_r_p - z_star/sqrt(N_samples) * var_error_r_p, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_r_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_r_p)];
%plot(x0,y0,'green', 'LineWidth',2)
title('r_{palm}');
xlabel('iterations')
ylabel('E_{calib}')

subplot(2,3,4)
plot(iterations,mean_error_c_f,'red','LineWidth',2)
hold on
grid on
 plot(iterations,mean_error_c_f + z_star/sqrt(N_samples) * var_error_c_f, '--r')
 plot(iterations,mean_error_c_f - z_star/sqrt(N_samples) * var_error_c_f, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_c_f + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_f)];
%plot(x0,y0,'green', 'LineWidth',2)
title('c_{finger}');
xlabel('iterations')
ylabel('E_{calib}')

subplot(2,3,5)
plot(iterations,mean_error_c_p,'red','LineWidth',2)
hold on
grid on
 plot(iterations,mean_error_c_p + z_star/sqrt(N_samples) * var_error_c_p, '--r')
 plot(iterations,mean_error_c_p - z_star/sqrt(N_samples) * var_error_c_p, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_c_p + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_c_p)];
%plot(x0,y0,'green', 'LineWidth',2)
title('c_{palm}');
xlabel('iterations')
ylabel('E_{calib}')

subplot(2,3,6)
plot(iterations,mean_error_beta,'red','LineWidth',2)
hold on
grid on
 plot(iterations,mean_error_beta + z_star/sqrt(N_samples) * var_error_beta, '--r')
 plot(iterations,mean_error_beta - z_star/sqrt(N_samples) * var_error_beta, '--r')
x0 = [N_iter_unif, N_iter_unif];
y0 = [0, max(mean_error_beta + z_star/sqrt(N_iter_unif + N_iter_full) * var_error_beta)];
%plot(x0,y0,'green', 'LineWidth',2)
title('\beta');
xlabel('iterations')
ylabel('E_{calib}')







