Index: trunk/src/toolbox_calib/compute_extrinsic.m =================================================================== --- trunk/src/toolbox_calib/compute_extrinsic.m (revision 999) +++ trunk/src/toolbox_calib/compute_extrinsic.m (revision 1000) @@ -34,10 +34,7 @@ %project_points.m: Computes the 2D image projections of a set of 3D points - - if nargin < 8, thresh_cond = inf; end; - if nargin < 7, @@ -45,20 +42,19 @@ end; - if nargin < 6, - alpha_c = 0; - if nargin < 5, - kc = zeros(5,1); - if nargin < 4, - cc = zeros(2,1); - if nargin < 3, - fc = ones(2,1); - if nargin < 2, - error('Need 2D projections and 3D points (in compute_extrinsic.m)'); - return; - end; - end; - end; - end; + alpha_c = 0; + if nargin < 5, + kc = zeros(5,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; end; @@ -69,5 +65,4 @@ % Refinement: [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond); - % computation of the homography (not useful in the end) @@ -81,5 +76,4 @@ ex = x_kk - x; - % Converts the homography in pixel units: @@ -87,7 +81,4 @@ H = KK*H; - - - return; Index: trunk/src/toolbox_calib/compute_homography.m =================================================================== --- trunk/src/toolbox_calib/compute_homography.m (revision 999) +++ trunk/src/toolbox_calib/compute_homography.m (revision 1000) @@ -34,14 +34,12 @@ - - Np = size(m,2); if size(m,1)<3, - m = [m;ones(1,Np)]; + m = [m;ones(1,Np)]; end; if size(M,1)<3, - M = [M;ones(1,Np)]; + M = [M;ones(1,Np)]; end; @@ -82,5 +80,5 @@ if Np > 4, - L = L'*L; + L = L'*L; end; @@ -99,66 +97,62 @@ if 0, - m2 = H*M; - m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; - merr = m(1:2,:) - m2; + m2 = H*M; + m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; + merr = m(1:2,:) - m2; end; %keyboard; - + %%% Homography refinement if there are more than 4 points: if Np > 4, - - % Final refinement: - hhv = reshape(H',9,1); - hhv = hhv(1:8); - - for iter=1:10, - - - mrep = H * M; + % Final refinement: + hhv = reshape(H',9,1); + hhv = hhv(1:8); - J = zeros(2*Np,8); + for iter=1:10, - MMM = (M ./ (ones(3,1)*mrep(3,:))); + mrep = H * M; - J(1:2:2*Np,1:3) = -MMM'; - J(2:2:2*Np,4:6) = -MMM'; - - mrep = mrep ./ (ones(3,1)*mrep(3,:)); + J = zeros(2*Np,8); - m_err = m(1:2,:) - mrep(1:2,:); - m_err = m_err(:); + MMM = (M ./ (ones(3,1)*mrep(3,:))); - MMM2 = (ones(3,1)*mrep(1,:)) .* MMM; - MMM3 = (ones(3,1)*mrep(2,:)) .* MMM; + J(1:2:2*Np,1:3) = -MMM'; + J(2:2:2*Np,4:6) = -MMM'; - J(1:2:2*Np,7:8) = MMM2(1:2,:)'; - J(2:2:2*Np,7:8) = MMM3(1:2,:)'; + mrep = mrep ./ (ones(3,1)*mrep(3,:)); - MMM = (M ./ (ones(3,1)*mrep(3,:)))'; + m_err = m(1:2,:) - mrep(1:2,:); + m_err = m_err(:); - hh_innov = inv(J'*J)*J'*m_err; + MMM2 = (ones(3,1)*mrep(1,:)) .* MMM; + MMM3 = (ones(3,1)*mrep(2,:)) .* MMM; - hhv_up = hhv - hh_innov; + J(1:2:2*Np,7:8) = MMM2(1:2,:)'; + J(2:2:2*Np,7:8) = MMM3(1:2,:)'; - H_up = reshape([hhv_up;1],3,3)'; + MMM = (M ./ (ones(3,1)*mrep(3,:)))'; - %norm(m_err) - %norm(hh_innov) + hh_innov = inv(J'*J)*J'*m_err; - hhv = hhv_up; - H = H_up; - - end; - + hhv_up = hhv - hh_innov; + H_up = reshape([hhv_up;1],3,3)'; + + %norm(m_err) + %norm(hh_innov) + + hhv = hhv_up; + H = H_up; + + end; end; if 0, - m2 = H*M; - m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; - merr = m(1:2,:) - m2; + m2 = H*M; + m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)]; + merr = m(1:2,:) - m2; end; Index: trunk/src/toolbox_calib/extract_parameters.m =================================================================== --- trunk/src/toolbox_calib/extract_parameters.m (revision 999) +++ trunk/src/toolbox_calib/extract_parameters.m (revision 1000) @@ -5,5 +5,5 @@ if ~exist('solution_error') - solution_error = zeros(6*n_ima + 15,1); + solution_error = zeros(6*n_ima + 15,1); end; @@ -19,5 +19,5 @@ % Calibration matrix: - + KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; inv_KK = inv(KK); @@ -26,36 +26,32 @@ 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);%*** - - omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); - Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); - - Rckk = rodrigues(omckk); - - Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; - - Hkk = Hkk / Hkk(3,3); - - else - - omckk = NaN*ones(3,1); - Tckk = NaN*ones(3,1); - Rckk = NaN*ones(3,3); - Hkk = NaN*ones(3,3); - omckk_error = NaN*ones(3,1); - Tckk_error = NaN*ones(3,1); - - end; - - eval(['omc_' num2str(kk) ' = omckk;']); - eval(['Rc_' num2str(kk) ' = Rckk;']); - eval(['Tc_' num2str(kk) ' = Tckk;']); - eval(['H_' num2str(kk) '= Hkk;']); - eval(['omc_error_' num2str(kk) ' = omckk_error;']); - eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); - + + 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);%*** + + omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + Rckk = rodrigues(omckk); + + Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hkk = Hkk / Hkk(3,3); + + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + Hkk = NaN*ones(3,3); + omckk_error = NaN*ones(3,1); + Tckk_error = NaN*ones(3,1); + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + eval(['H_' num2str(kk) '= Hkk;']); + eval(['omc_error_' num2str(kk) ' = omckk_error;']); + eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); end; Index: trunk/src/toolbox_calib/ima_read_calib.m =================================================================== --- trunk/src/toolbox_calib/ima_read_calib.m (revision 999) +++ trunk/src/toolbox_calib/ima_read_calib.m (revision 1000) @@ -1,6 +1,6 @@ if ~exist('calib_name')|~exist('format_image'), - data_calib; - return; + data_calib; + return; end; @@ -8,10 +8,9 @@ if ~exist('n_ima'), - data_calib; - return; + data_calib; + return; end; check_active_images; - images_read = active_images; @@ -19,13 +18,11 @@ if exist('image_numbers'), - first_num = image_numbers(1); + first_num = image_numbers(1); end; - % Just to fix a minor bug: if ~exist('first_num'), - first_num = image_numbers(1); + first_num = image_numbers(1); end; - image_numbers = first_num:n_ima-1+first_num; @@ -36,60 +33,59 @@ while (i <= n_ima), % & (~no_image_file), - - if active_images(i), - - %fprintf(1,'Loading image %d...\n',i); - - if ~type_numbering, - number_ext = num2str(image_numbers(i)); - else - number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); - end; - - ima_name = [calib_name number_ext '.' format_image]; - - if i == ind_active(1), - fprintf(1,'Loading image '); - end; - - if exist(ima_name), - - fprintf(1,'%d...',i); - - if format_image(1) == 'p', - if format_image(2) == 'p', - Ii = double(loadppm(ima_name)); + + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + if exist(ima_name), + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ii = double(loadppm(ima_name)); + else + Ii = double(loadpgm(ima_name)); + end; else - Ii = double(loadpgm(ima_name)); + if format_image(1) == 'r', + Ii = readras(ima_name); + else + Ii = double(imread(ima_name)); + end; end; - else - if format_image(1) == 'r', - Ii = readras(ima_name); - else - Ii = double(imread(ima_name)); + + if size(Ii,3)>1, + Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3); end; - end; - - if size(Ii,3)>1, - Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3); - end; - - eval(['I_' num2str(i) ' = Ii;']); - - else - - %fprintf(1,'%d...no image...',i); - - images_read(i) = 0; - - %no_image_file = 1; - - end; - - end; - - i = i+1; - + eval(['I_' num2str(i) ' = Ii;']); + + else + + %fprintf(1,'%d...no image...',i); + + images_read(i) = 0; + + %no_image_file = 1; + + end; + + end; + + i = i+1; + end; @@ -98,48 +94,41 @@ +if isempty(ind_read), + + fprintf(1,'\nWARNING! No image were read\n'); + + no_image_file = 1; + +else + %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n'); + + if no_image_file, + + %fprintf(1,'WARNING! Some images were not read properly\n'); + end; -if isempty(ind_read), - - fprintf(1,'\nWARNING! No image were read\n'); - - no_image_file = 1; - - -else - + fprintf(1,'\n'); - %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n'); + if size(I_1,1)~=480, + small_calib_image = 1; + else + small_calib_image = 0; + end; - - if no_image_file, - - %fprintf(1,'WARNING! Some images were not read properly\n'); - - end; - - - fprintf(1,'\n'); + [Hcal,Wcal] = size(I_1); % size of the calibration image - if size(I_1,1)~=480, - small_calib_image = 1; - else - small_calib_image = 0; - end; - - [Hcal,Wcal] = size(I_1); % size of the calibration image - - [ny,nx] = size(I_1); - - clickname = []; - - map = gray(256); - - %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'; + [ny,nx] = size(I_1); - %eval(string_save); + clickname = []; - disp('done'); - %click_calib; + map = gray(256); + + %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'; + + %eval(string_save); + + disp('done'); + %click_calib; end;