go_calib_optim_iter_fisheye.m

matlab的标定工具箱

%-------------------- 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;


while (change > 1e-6)&(iter < MaxIter),
    
    fprintf(1,'%d...',iter+1);
    
    % 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.
    
    
    f = param(1:2);
    c = param(3:4);
    alpha = param(5);
    k = param(6:9);
    
    
    % 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,
        %if active_images(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); 
        
        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_fisheye(X_kk,omckk,Tckk,f(1),c,k,alpha);
            dxdf = repmat(dxdf,[1 2]);
        else
            [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,f,c,k,alpha);
        end;
        
        exkk = x_kk - x;
        
        A = [dxdf dxdc dxdalpha dxdk]';
        B = [dxdom dxdT]';
        
        JJ3(1:9,1:9) = JJ3(1:9,1:9) + 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:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
        JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)';
        
        ex3(1:9) = ex3(1:9) + A*exkk(:);
        ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:);
        
        
        %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 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;
        
    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(6,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)';
    
    JJ3 = JJ3(ind_Jac,ind_Jac);
    ex3 = ex3(ind_Jac);
    
    JJ2_inv = inv(JJ3); % 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*ex3;
    
    
    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);

    if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)),
        fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n');
        center_optim = 0;
        cc_current = c;
    else
        cc_current = param(3:4);
    end;
    
    alpha_current = param(5);
    kc_current = param(6:9);
    
    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]);
    
    
    %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!!
    %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes)
    %% The complete gradient descent method is useful to precisely update the intrinsic parameters.
    
    
    if recompute_extrinsic,
        MaxIter2 = 20;
        for kk =ind_active, %1:n_ima,
            %if active_images(kk),
            omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
            Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
            eval(['X_kk = X_' num2str(kk) ';']);
            eval(['x_kk = x_' num2str(kk) ';']);
            [omc_current,Tc_current] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current);
            [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond);
            if check_cond,
                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];
                    omckk = NaN*ones(3,1);
                    Tckk = NaN*ones(3,1);
                end;
            end;
            param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk;
            param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk;
            %end;
        end;
    end;
    
    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:9);

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_fisheye;

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_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
    
    if ~est_aspect_ratio,
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c);
        dxdf = repmat(dxdf,[1 2]);
    else
        [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);
    end;
    
    A = [dxdf dxdc dxdalpha dxdk]';
    B = [dxdom dxdT]';
    
    JJ3(1:9,1:9) = JJ3(1:9,1:9) + 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:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB;
    JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)';
    
end;

JJ3 = JJ3(ind_Jac,ind_Jac);

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_fisheye;

fprintf(1,'done\n');


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