Changeset 1000


Ignore:
Timestamp:
Feb 17, 2017, 2:51:46 PM (8 years ago)
Author:
g7moreau
Message:
  • Remove tab
Location:
trunk/src/toolbox_calib
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • trunk/src/toolbox_calib/compute_extrinsic.m

    r926 r1000  
    3434%project_points.m: Computes the 2D image projections of a set of 3D points
    3535
    36 
    37 
    3836if nargin < 8,
    3937   thresh_cond = inf;
    4038end;
    41 
    4239
    4340if nargin < 7,
    … …  
    4542end;
    4643
    47 
    4844if nargin < 6,
    49    alpha_c = 0;
    50         if nargin < 5,
    51         kc = zeros(5,1);
    52         if nargin < 4,
    53         cc = zeros(2,1);
    54         if nargin < 3,
    55                 fc = ones(2,1);
    56                 if nargin < 2,
    57                 error('Need 2D projections and 3D points (in compute_extrinsic.m)');
    58                 return;
    59                 end;
    60         end;
    61         end;
    62         end;
     45    alpha_c = 0;
     46    if nargin < 5,
     47        kc = zeros(5,1);
     48        if nargin < 4,
     49            cc = zeros(2,1);
     50            if nargin < 3,
     51                fc = ones(2,1);
     52                if nargin < 2,
     53                    error('Need 2D projections and 3D points (in compute_extrinsic.m)');
     54                    return;
     55                end;
     56            end;
     57        end;
     58    end;
    6359end;
    6460
    … …  
    6965% Refinement:
    7066[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);
    71 
    7267
    7368% computation of the homography (not useful in the end)
    … …  
    8176ex = x_kk - x;
    8277
    83 
    8478% Converts the homography in pixel units:
    8579
    … …  
    8781
    8882H = KK*H;
    89 
    90 
    91 
    9283
    9384return;

  • trunk/src/toolbox_calib/compute_homography.m

    r926 r1000  
    3434
    3535
    36 
    37 
    3836Np = size(m,2);
    3937
    4038if size(m,1)<3,
    41    m = [m;ones(1,Np)];
     39    m = [m;ones(1,Np)];
    4240end;
    4341
    4442if size(M,1)<3,
    45    M = [M;ones(1,Np)];
     43    M = [M;ones(1,Np)];
    4644end;
    4745
    … …  
    8280
    8381if Np > 4,
    84         L = L'*L;
     82    L = L'*L;
    8583end;
    8684
    … …  
    9997
    10098if 0,
    101    m2 = H*M;
    102    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
    103    merr = m(1:2,:) - m2;
     99    m2 = H*M;
     100    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
     101    merr = m(1:2,:) - m2;
    104102end;
    105103
    106104%keyboard;
    107  
     105
    108106%%% Homography refinement if there are more than 4 points:
    109107
    110108if Np > 4,
    111    
    112    % Final refinement:
    113    hhv = reshape(H',9,1);
    114    hhv = hhv(1:8);
    115    
    116    for iter=1:10,
    117      
    118109
    119    
    120                 mrep = H * M;
     110    % Final refinement:
     111    hhv = reshape(H',9,1);
     112    hhv = hhv(1:8);
    121113
    122                 J = zeros(2*Np,8);
     114    for iter=1:10,
    123115
    124                 MMM = (M ./ (ones(3,1)*mrep(3,:)));
     116         mrep = H * M;
    125117
    126                 J(1:2:2*Np,1:3) = -MMM';
    127                 J(2:2:2*Np,4:6) = -MMM';
    128                
    129                 mrep = mrep ./ (ones(3,1)*mrep(3,:));
     118         J = zeros(2*Np,8);
    130119
    131                 m_err = m(1:2,:) - mrep(1:2,:);
    132                 m_err = m_err(:);
     120         MMM = (M ./ (ones(3,1)*mrep(3,:)));
    133121
    134                 MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
    135                 MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;
     122         J(1:2:2*Np,1:3) = -MMM';
     123         J(2:2:2*Np,4:6) = -MMM';
    136124
    137                 J(1:2:2*Np,7:8) = MMM2(1:2,:)';
    138                 J(2:2:2*Np,7:8) = MMM3(1:2,:)';
     125         mrep = mrep ./ (ones(3,1)*mrep(3,:));
    139126
    140                 MMM = (M ./ (ones(3,1)*mrep(3,:)))';
     127         m_err = m(1:2,:) - mrep(1:2,:);
     128         m_err = m_err(:);
    141129
    142                 hh_innov  = inv(J'*J)*J'*m_err;
     130         MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
     131         MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;
    143132
    144                 hhv_up = hhv - hh_innov;
     133         J(1:2:2*Np,7:8) = MMM2(1:2,:)';
     134         J(2:2:2*Np,7:8) = MMM3(1:2,:)';
    145135
    146                 H_up = reshape([hhv_up;1],3,3)';
     136         MMM = (M ./ (ones(3,1)*mrep(3,:)))';
    147137
    148                 %norm(m_err)
    149                 %norm(hh_innov)
     138         hh_innov  = inv(J'*J)*J'*m_err;
    150139
    151                 hhv = hhv_up;
    152       H = H_up;
    153      
    154    end;
    155    
     140         hhv_up = hhv - hh_innov;
    156141
     142         H_up = reshape([hhv_up;1],3,3)';
     143
     144         %norm(m_err)
     145         %norm(hh_innov)
     146
     147         hhv = hhv_up;
     148       H = H_up;
     149
     150    end;
    157151end;
    158152
    159153if 0,
    160    m2 = H*M;
    161    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
    162    merr = m(1:2,:) - m2;
     154    m2 = H*M;
     155    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
     156    merr = m(1:2,:) - m2;
    163157end;
    164158

  • trunk/src/toolbox_calib/extract_parameters.m

    r926 r1000  
    55
    66if ~exist('solution_error')
    7    solution_error = zeros(6*n_ima + 15,1);
     7    solution_error = zeros(6*n_ima + 15,1);
    88end;
    99
    … …  
    1919
    2020% Calibration matrix:
    21        
     21
    2222KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1];
    2323inv_KK = inv(KK);
    … …  
    2626
    2727for kk = 1:n_ima,
    28    
    29    if active_images(kk),   
    30      
    31       omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***   
    32       Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
    33      
    34       omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
    35       Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
    36      
    37         Rckk = rodrigues(omckk);
    38    
    39         Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];
    40    
    41         Hkk = Hkk / Hkk(3,3);
    42      
    43    else
    44      
    45       omckk = NaN*ones(3,1);   
    46       Tckk = NaN*ones(3,1);
    47       Rckk = NaN*ones(3,3);
    48       Hkk = NaN*ones(3,3);
    49       omckk_error = NaN*ones(3,1);
    50       Tckk_error = NaN*ones(3,1);
    51      
    52    end;
    53    
    54    eval(['omc_' num2str(kk) ' = omckk;']);
    55    eval(['Rc_' num2str(kk) ' = Rckk;']);
    56    eval(['Tc_' num2str(kk) ' = Tckk;']);
    57    eval(['H_' num2str(kk) '= Hkk;']);
    58    eval(['omc_error_' num2str(kk) ' = omckk_error;']);
    59    eval(['Tc_error_' num2str(kk) ' = Tckk_error;']);
    60    
     28
     29    if active_images(kk),
     30        omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%***
     31        Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%***
     32
     33        omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3);
     34        Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6);
     35
     36        Rckk = rodrigues(omckk);
     37
     38        Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk];
     39
     40        Hkk = Hkk / Hkk(3,3);
     41
     42    else
     43        omckk = NaN*ones(3,1);
     44        Tckk = NaN*ones(3,1);
     45        Rckk = NaN*ones(3,3);
     46        Hkk = NaN*ones(3,3);
     47        omckk_error = NaN*ones(3,1);
     48        Tckk_error = NaN*ones(3,1);
     49    end;
     50
     51    eval(['omc_' num2str(kk) ' = omckk;']);
     52    eval(['Rc_' num2str(kk) ' = Rckk;']);
     53    eval(['Tc_' num2str(kk) ' = Tckk;']);
     54    eval(['H_' num2str(kk) '= Hkk;']);
     55    eval(['omc_error_' num2str(kk) ' = omckk_error;']);
     56    eval(['Tc_error_' num2str(kk) ' = Tckk_error;']);
    6157end;

  • trunk/src/toolbox_calib/ima_read_calib.m

    r926 r1000  
    11
    22if ~exist('calib_name')|~exist('format_image'),
    3    data_calib;
    4    return;
     3    data_calib;
     4    return;
    55end;
    66
    … …  
    88
    99if ~exist('n_ima'),
    10    data_calib;
    11    return;
     10    data_calib;
     11    return;
    1212end;
    1313
    1414check_active_images;
    15 
    1615
    1716images_read = active_images;
    … …  
    1918
    2019if exist('image_numbers'),
    21    first_num = image_numbers(1);
     20    first_num = image_numbers(1);
    2221end;
    23 
    2422
    2523% Just to fix a minor bug:
    2624if ~exist('first_num'),
    27    first_num = image_numbers(1);
     25    first_num = image_numbers(1);
    2826end;
    29 
    3027
    3128image_numbers = first_num:n_ima-1+first_num;
    … …  
    3633
    3734while (i <= n_ima), % & (~no_image_file),
    38    
    39    if active_images(i),
    40    
    41         %fprintf(1,'Loading image %d...\n',i);
    42    
    43         if ~type_numbering,   
    44         number_ext =  num2str(image_numbers(i));
    45         else
    46         number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i));
    47         end;
    48        
    49       ima_name = [calib_name  number_ext '.' format_image];
    50      
    51       if i == ind_active(1),
    52          fprintf(1,'Loading image ');
    53       end;
    54      
    55       if exist(ima_name),
    56          
    57          fprintf(1,'%d...',i);
    58          
    59          if format_image(1) == 'p',
    60             if format_image(2) == 'p',
    61                Ii = double(loadppm(ima_name));
     35
     36    if active_images(i),
     37
     38        %fprintf(1,'Loading image %d...\n',i);
     39
     40        if ~type_numbering,
     41            number_ext =  num2str(image_numbers(i));
     42        else
     43            number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i));
     44        end;
     45
     46        ima_name = [calib_name  number_ext '.' format_image];
     47
     48        if i == ind_active(1),
     49            fprintf(1,'Loading image ');
     50        end;
     51
     52        if exist(ima_name),
     53
     54            fprintf(1,'%d...',i);
     55
     56            if format_image(1) == 'p',
     57                if format_image(2) == 'p',
     58                    Ii = double(loadppm(ima_name));
     59                else
     60                    Ii = double(loadpgm(ima_name));
     61                end;
    6262            else
    63                Ii = double(loadpgm(ima_name));
     63                if format_image(1) == 'r',
     64                    Ii = readras(ima_name);
     65                else
     66                    Ii = double(imread(ima_name));
     67                end;
    6468            end;
    65          else
    66             if format_image(1) == 'r',
    67                Ii = readras(ima_name);
    68             else
    69                Ii = double(imread(ima_name));
     69
     70            if size(Ii,3)>1,
     71                Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3);
    7072            end;
    71          end;
    7273
    73        
    74                 if size(Ii,3)>1,
    75             Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3);
    76                 end;
    77    
    78                 eval(['I_' num2str(i) ' = Ii;']);
    79          
    80       else
    81          
    82          %fprintf(1,'%d...no image...',i);
    83          
    84          images_read(i) = 0;
    85          
    86          %no_image_file = 1;
    87          
    88       end;
    89      
    90    end;
    91    
    92    i = i+1;   
    93    
     74            eval(['I_' num2str(i) ' = Ii;']);
     75
     76        else
     77
     78            %fprintf(1,'%d...no image...',i);
     79
     80            images_read(i) = 0;
     81
     82            %no_image_file = 1;
     83
     84        end;
     85
     86    end;
     87
     88    i = i+1;
     89
    9490end;
    9591
    … …  
    9894
    9995
     96if isempty(ind_read),
     97
     98    fprintf(1,'\nWARNING! No image were read\n');
     99
     100    no_image_file = 1;
     101
     102else
     103    %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n');
     104
     105    if no_image_file,
     106
     107        %fprintf(1,'WARNING! Some images were not read properly\n');
     108    end;
    100109
    101110
    102 if isempty(ind_read),
    103    
    104    fprintf(1,'\nWARNING! No image were read\n');
    105    
    106    no_image_file = 1;
    107    
    108    
    109 else
    110    
     111    fprintf(1,'\n');
    111112
    112    %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n');
     113    if size(I_1,1)~=480,
     114        small_calib_image = 1;
     115    else
     116        small_calib_image = 0;
     117    end;
    113118
    114    
    115    if no_image_file,
    116      
    117         %fprintf(1,'WARNING! Some images were not read properly\n');
    118      
    119    end;
    120      
    121    
    122    fprintf(1,'\n');
     119    [Hcal,Wcal] = size(I_1);     % size of the calibration image
    123120
    124    if size(I_1,1)~=480,
    125         small_calib_image = 1;
    126         else
    127         small_calib_image = 0;
    128         end;
    129    
    130    [Hcal,Wcal] = size(I_1);     % size of the calibration image
    131    
    132    [ny,nx] = size(I_1);
    133    
    134    clickname = [];
    135    
    136    map = gray(256);
    137    
    138         %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image';
     121    [ny,nx] = size(I_1);
    139122
    140         %eval(string_save);
     123    clickname = [];
    141124
    142         disp('done');
    143         %click_calib;
     125    map = gray(256);
     126
     127    %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image';
     128
     129    %eval(string_save);
     130
     131    disp('done');
     132    %click_calib;
    144133
    145134end;
Note: See TracChangeset for help on using the changeset viewer.