%-------------------- Main Optimization:

fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active));


param = init_param;
change = 1;

iter = 0;

fprintf(1,'Gradient descent iterations: ');

param_list = param;


% Compute the average distance of all the points to the camera:
Zave_list = zeros(1,length(ind_active));
for ii = 1:length(ind_active),
    kk = ind_active(ii);
    eval(['X_kk = X_' num2str(kk) ';']);
    omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 
    Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 
    Y_kk = rigid_motion(X_kk,omckk,Tckk);
    Zave_list(ii) = mean(Y_kk(3,:));
end;

Zave = sum(N_points_views .* Zave_list) / sum(N_points_views);

fprintf(1,'Weak Perspective Camera Calibration - Setting the average depth of the scene at Zave = %.6f\n',Zave);


while (change > 1e-9)&(iter < MaxIter),
    
    fprintf(1,'%d...',iter+1);
    
    f = param(1:2);
    c = param(3:4);
    alpha = param(5);
    k = param(6:10);
    
    %%% Make sure that all the Z translations satisfy the average depth constraint:
    % Compute the average distance of all the points to the camera:
    Zave_list = zeros(1,length(ind_active));
    for ii = 1:length(ind_active),
        kk = ind_active(ii);
        eval(['X_kk = X_' num2str(kk) ';']);
        omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 
        Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 
        Y_kk = rigid_motion(X_kk,omckk,Tckk);
        Zave_list(ii) = mean(Y_kk(3,:));
    end;
    
    Zave2 = sum(N_points_views .* Zave_list) / sum(N_points_views);
    
    change_T = Zave2 - Zave;
    
    for ii = 1:length(ind_active),
        kk = ind_active(ii);
        Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
        %Tckk2 = Tckk .* [1;1;ratio_T];
        Tckk2 = Tckk - [0;0;change_T];
        param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk2;
    end;
    
    
    % To speed up: pre-allocate the memory for the Jacobian JJ3.
    % For that, need to compute the total number of points.
    
    %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active
    %% images) through a one step steepest gradient descent.
    
    

    
    
    % Compute the size of the Jacobian matrix:
    N_points_views_active = N_points_views(ind_active);
    
    JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);
    ex3 = zeros(15 + 6*n_ima,1);
    
    
    for kk = ind_active, %1:n_ima,
        
        omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 
        Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 
        
        if isnan(omckk(1)),
            fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk);
            return;
        end;
        
        eval(['X_kk = X_' num2str(kk) ';']);
        eval(['x_kk = x_' num2str(kk) ';']);
        
        Np = N_points_views(kk);
        
        if ~est_aspect_ratio,
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f(1),c,k,alpha,Zave);
            dxdf = repmat(dxdf,[1 2]);
        else
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f,c,k,alpha,Zave);
        end;
        
        exkk = x_kk - x;
        
        A = [dxdf dxdc dxdalpha dxdk]';
        B = [dxdom dxdT]';
        
        JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');
        
        AB = sparse(A*B');
        JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';
        
        ex3(1:10) = ex3(1:10) + A*exkk(:);
        ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:);
        
        % Check if this view is ill-conditioned:
        if 0, %check_cond,
            JJ_kk = B'; %[dxdom dxdT];
            if (cond(JJ_kk)> thresh_cond),
                active_images(kk) = 0;
                fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk)
                desactivated_images = [desactivated_images kk];
                param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); 
            end;
        end;
                
    end;
    
    
    % List of active images (necessary if changed):
    check_active_images;
    
    
    % The following vector helps to select the variables to update (for only active images):
    selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)];
    if ~est_aspect_ratio,
        if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]),
            selected_variables(2) = 0;
        end;
    end;
    ind_Jac = find(selected_variables)';
    
    JJ4 = JJ3(ind_Jac,ind_Jac);
    ex4 = ex3(ind_Jac);
    
    
    % Try to make the inversion work:
    [U,S,V] = svd(full(JJ4));
    
    
    
    if 0,
        s = diag(S);
        figure(100);
        semilogy(s);
    end;
    
    n_reject = length(ind_active);
    
    U = U(:,1:end-n_reject);
    S = S(1:end-n_reject,1:end-n_reject);

    
    JJ2_inv = U * inv(S) * U';
    
    %JJ2_inv = inv(JJ4); % not bad for sparse matrices!!
    
    
    % Smoothing coefficient:
    
    alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing!
    
    param_innov = alpha_smooth2*JJ2_inv*ex4;
    
    
    param_up = param(ind_Jac) + param_innov;
    param(ind_Jac) = param_up;
    
    
    % New intrinsic parameters:
    
    fc_current = param(1:2);
    cc_current = param(3:4);
    alpha_current = param(5);
    kc_current = param(6:10);
    
    
    if ~est_aspect_ratio & isequal(est_fc,[1;1]),
        fc_current(2) = fc_current(1);
        param(2) = param(1);
    end;
    
    % Change on the intrinsic parameters:
    change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]);
    
    param_list = [param_list param];
    iter = iter + 1;
    
end;

fprintf(1,'done\n');



%%%--------------------------- Computation of the error of estimation:

fprintf(1,'Estimation of uncertainties...');


check_active_images;

solution = param;


% Extraction of the paramters for computing the right reprojection error:

fc = solution(1:2);
cc = solution(3:4);
alpha_c = solution(5);
kc = solution(6:10);

for kk = 1:n_ima,
    
    if active_images(kk), 
        
        omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***   
        Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 
        Rckk = rodrigues(omckk);
        
    else
        
        omckk = NaN*ones(3,1);   
        Tckk = NaN*ones(3,1);
        Rckk = NaN*ones(3,3);
        
    end;
    
    eval(['omc_' num2str(kk) ' = omckk;']);
    eval(['Rc_' num2str(kk) ' = Rckk;']);
    eval(['Tc_' num2str(kk) ' = Tckk;']);
    
end;


% Recompute the error (in the vector ex):
comp_error_calib;

sigma_x = std(ex(:));

% Compute the size of the Jacobian matrix:
N_points_views_active = N_points_views(ind_active);

JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225);

for kk = ind_active,
    
    omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 
    Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 
    
    eval(['X_kk = X_' num2str(kk) ';']);
    
    Np = N_points_views(kk);
    
    %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave);
    
    if ~est_aspect_ratio,
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c,Zave);
        dxdf = repmat(dxdf,[1 2]);
    else
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave);
    end;
    
    A = [dxdf dxdc dxdalpha dxdk]';
    B = [dxdom dxdT]';
    
    JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A');
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B');
    
    AB = sparse(A*B');
    JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)';
    
end;

JJ3 = JJ3(ind_Jac,ind_Jac);
% Try to make the inversion work:
[U,S,V] = svd(full(JJ3));
n_reject = length(ind_active);
U = U(:,1:end-n_reject);
S = S(1:end-n_reject,1:end-n_reject);

JJ2_inv = U * inv(S) * U';
%JJ2_inv = inv(JJ3); % not bad for sparse matrices!!

param_error = zeros(6*n_ima+15,1);
param_error(ind_Jac) =  3*sqrt(full(diag(JJ2_inv)))*sigma_x;

solution_error = param_error;

if ~est_aspect_ratio & isequal(est_fc,[1;1]),
    solution_error(2) = solution_error(1);
end;


%%% Extraction of the final intrinsic and extrinsic paramaters:

extract_parameters;

fprintf(1,'done\n');


fprintf(1,'\n\nCalibration results after optimization for weak perspective camera model (with uncertainties):\n\n');
fprintf(1,'Focal Length:          fc = [ %3.5f   %3.5f ]