Index: /trunk/src/toolbox_calib/check_active_images.m =================================================================== --- /trunk/src/toolbox_calib/check_active_images.m (revision 725) +++ /trunk/src/toolbox_calib/check_active_images.m (revision 725) @@ -0,0 +1,38 @@ +if n_ima ~= 0, + + if ~exist('active_images'), + active_images = ones(1,n_ima); + end; + n_act = length(active_images); + if n_act < n_ima, + active_images = [active_images ones(1,n_ima-n_act)]; + else + if n_act > n_ima, + active_images = active_images(1:n_ima); + end; + end; + + ind_active = find(active_images); + + if prod(double(active_images == 0)), + disp('Error: There is no active image. Run Add/Suppress images to add images'); + break + end; + + if exist('center_optim'), + center_optim = double(center_optim); + end; + if exist('est_alpha'), + est_alpha = double(est_alpha); + end; + if exist('est_dist'), + est_dist = double(est_dist); + end; + if exist('est_fc'), + est_fc = double(est_fc); + end; + if exist('est_aspect_ratio'), + est_aspect_ratio = double(est_aspect_ratio); + end; + +end; Index: /trunk/src/toolbox_calib/check_directory.m =================================================================== --- /trunk/src/toolbox_calib/check_directory.m (revision 725) +++ /trunk/src/toolbox_calib/check_directory.m (revision 725) @@ -0,0 +1,190 @@ +% This small script looks in the direcory and checks if the images are there. +% +% This works only on Matlab 5.x (otherwise, the dir commands works differently) + +% (c) Jean-Yves Bouguet - Dec. 27th, 1999 + +l = dir([calib_name '*']); + +Nl = size(l,1); +Nima_valid = 0; +ind_valid = []; +loc_extension = []; +length_name = size(calib_name,2); + +if Nl > 0, + + for pp = 1:Nl, + + filenamepp = l(pp).name; + if isempty(calib_name), + iii = 1; + else + iii = findstr(filenamepp,calib_name); + end; + + loc_ext = findstr(filenamepp,format_image); + string_num = filenamepp(length_name+1:loc_ext - 2); + + if isempty(str2num(string_num)), + iii = []; + end; + + + if ~isempty(iii), + if (iii(1) ~= 1), + iii = []; + end; + end; + + + + if ~isempty(iii) & ~isempty(loc_ext), + + Nima_valid = Nima_valid + 1; + ind_valid = [ind_valid pp]; + loc_extension = [loc_extension loc_ext(1)]; + + end; + + end; + + if (Nima_valid==0), + + % try the upper case format: + + format_image = upper(format_image); + + + + for pp = 1:Nl, + + filenamepp = l(pp).name; + + if isempty(calib_name), + iii = 1; + else + iii = findstr(filenamepp,calib_name); + end; + + loc_ext = findstr(filenamepp,format_image); + string_num = filenamepp(length_name+1:loc_ext - 2); + + if isempty(str2num(string_num)), + iii = []; + end; + + + if ~isempty(iii), + if (iii(1) ~= 1), + iii = []; + end; + end; + + + + if ~isempty(iii) & ~isempty(loc_ext), + + Nima_valid = Nima_valid + 1; + ind_valid = [ind_valid pp]; + loc_extension = [loc_extension loc_ext(1)]; + + end; + + end; + + if (Nima_valid==0), + + fprintf(1,'No image found. File format may be wrong.\n'); + + else + + + % Get all the string numbers: + + string_length = zeros(1,Nima_valid); + indices = zeros(1,Nima_valid); + + + for ppp = 1:Nima_valid, + + name = l(ind_valid(ppp)).name; + string_num = name(length_name+1:loc_extension(ppp) - 2); + string_length(ppp) = size(string_num,2); + indices(ppp) = str2num(string_num); + + end; + + % Number of images... + first_num = min(indices); + n_ima = max(indices) - first_num + 1; + + if min(string_length) == max(string_length), + + N_slots = min(string_length); + type_numbering = 1; + + else + + N_slots = 1; + type_numbering = 0; + + end; + + image_numbers = first_num:n_ima-1+first_num; + + %%% By default, all the images are active for calibration: + + active_images = ones(1,n_ima); + + + + end; + + else + + % Get all the string numbers: + + string_length = zeros(1,Nima_valid); + indices = zeros(1,Nima_valid); + + + for ppp = 1:Nima_valid, + + name = l(ind_valid(ppp)).name; + string_num = name(length_name+1:loc_extension(ppp) - 2); + string_length(ppp) = size(string_num,2); + indices(ppp) = str2num(string_num); + + end; + + % Number of images... + first_num = min(indices); + n_ima = max(indices) - first_num + 1; + + if min(string_length) == max(string_length), + + N_slots = min(string_length); + type_numbering = 1; + + else + + N_slots = 1; + type_numbering = 0; + + end; + + image_numbers = first_num:n_ima-1+first_num; + + %%% By default, all the images are active for calibration: + + active_images = ones(1,n_ima); + + end; + +else + + fprintf(1,'No image found. Basename may be wrong.\n'); + +end; + Index: /trunk/src/toolbox_calib/check_extracted_images.m =================================================================== --- /trunk/src/toolbox_calib/check_extracted_images.m (revision 725) +++ /trunk/src/toolbox_calib/check_extracted_images.m (revision 725) @@ -0,0 +1,37 @@ +check_active_images; + +for kk = ind_active, + + if ~exist(['x_' num2str(kk)]), + + fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk); + + active_images(kk) = 0; + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + else + + eval(['xkk = x_' num2str(kk) ';']); + + if isnan(xkk(1)), + + fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk); + + active_images(kk) = 0; + + end; + + end; + +end; Index: /trunk/src/toolbox_calib/click_calib.m =================================================================== --- /trunk/src/toolbox_calib/click_calib.m (revision 725) +++ /trunk/src/toolbox_calib/click_calib.m (revision 725) @@ -0,0 +1,214 @@ +%if exist('images_read'); +% active_images = active_images & images_read; +%end; + +var2fix = 'dX_default'; + +fixvariable; + +var2fix = 'dY_default'; + +fixvariable; + +var2fix = 'map'; + +fixvariable; + + +if ~exist('n_ima'), + data_calib; +end; + +check_active_images; + +if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + if isempty(ind_read), + disp('Cannot extract corners without images'); + return; + end; +end; + + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +if exist('dX'), + dX_default = dX; +end; + +if exist('dY'), + dY_default = dY; +end; + +if exist('n_sq_x'), + n_sq_x_default = n_sq_x; +end; + +if exist('n_sq_y'), + n_sq_y_default = n_sq_y; +end; + + +if ~exist('dX_default')|~exist('dY_default'); + + % Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang + dX_default = 30; + dY_default = 30; + + % Setup of JY - 3D calibration rig at Google - use units in mm to match Zhang + dX_default = 100; + dY_default = 100; + +end; + + +if ~exist('n_sq_x_default')|~exist('n_sq_y_default'), + n_sq_x_default = 10; + n_sq_y_default = 10; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty +end; + + +if ~exist('wintx') | ~exist('winty'), + clear_windows; % Clear all the window sizes (to re-initiate) +end; + + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +% Useful option to add images: +kk_first = ima_proc(1); %input('Start image number ([]=1=first): '); + + +if exist(['wintx_' num2str(kk_first)]), + + eval(['wintxkk = wintx_' num2str(kk_first) ';']); + + if isempty(wintxkk) | isnan(wintxkk), + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + +else + + disp('Window size for corner finder (wintx and winty):'); + wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); + if isempty(wintx), wintx = wintx_default; end; + wintx = round(wintx); + winty = input(['winty ([] = ' num2str(winty_default) ') = ']); + if isempty(winty), winty = winty_default; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +if ~dont_ask, + fprintf(1,'Do you want to use the automatic square counting mechanism (0=[]=default)\n'); + manual_squares = input(' or do you always want to enter the number of squares manually (1,other)? '); + if isempty(manual_squares), + manual_squares = 0; + else + manual_squares = ~~manual_squares; + end; +else + manual_squares = 0; +end; + + +for kk = ima_proc, + if exist(['I_' num2str(kk)]), + click_ima_calib; + active_images(kk) = 1; + else + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +check_active_images; + + + +% Fix potential non-existing variables: + +for kk = 1:n_ima, + if ~exist(['x_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + + if ~exist(['wintx_' num2str(kk)]) | ~exist(['winty_' num2str(kk)]), + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + end; +end; + +string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY wintx_default winty_default'; + +for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; +end; + +eval(string_save); + +disp('done'); + +return; + +go_calib_optim; + Index: /trunk/src/toolbox_calib/comp_distortion_oulu.m =================================================================== --- /trunk/src/toolbox_calib/comp_distortion_oulu.m (revision 725) +++ /trunk/src/toolbox_calib/comp_distortion_oulu.m (revision 725) @@ -0,0 +1,48 @@ +function [x] = comp_distortion_oulu(xd,k); + +%comp_distortion_oulu.m +% +%[x] = comp_distortion_oulu(xd,k) +% +%Compensates for radial and tangential distortion. Model From Oulu university. +%For more informatino about the distortion model, check the forward projection mapping function: +%project_points.m +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + + +if length(k) == 1, + + [x] = comp_distortion(xd,k); + +else + + k1 = k(1); + k2 = k(2); + k3 = k(5); + p1 = k(3); + p2 = k(4); + + x = xd; % initial guess + + for kk=1:20, + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (xd - delta_x)./(ones(2,1)*k_radial); + + end; + +end; + + Index: /trunk/src/toolbox_calib/comp_ext_calib.m =================================================================== --- /trunk/src/toolbox_calib/comp_ext_calib.m (revision 725) +++ /trunk/src/toolbox_calib/comp_ext_calib.m (revision 725) @@ -0,0 +1,62 @@ +%%% Computes the extrinsic parameters for all the active calibration images + +check_active_images; + +N_points_views = zeros(1,n_ima); + +for kk = 1:n_ima, + + if exist(['x_' num2str(kk)]), + + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + end; + if active_images(kk), + N_points_views(kk) = size(x_kk,2); + [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + end; + end; + if isnan(omckk(1,1)), + %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) + active_images(kk) = 0; + end; + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + + active_images(kk) = 0; + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +check_active_images; Index: /trunk/src/toolbox_calib/compute_extrinsic.m =================================================================== --- /trunk/src/toolbox_calib/compute_extrinsic.m (revision 725) +++ /trunk/src/toolbox_calib/compute_extrinsic.m (revision 725) @@ -0,0 +1,122 @@ +function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% H: Homography between points on the grid and points on the image plane (in pixel) +% This makes sense only if the planar that is used in planar. +% x: Reprojections of the points on the image plane +% ex: Reprojection error: ex = x_kk - x; +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 8, + thresh_cond = inf; +end; + + +if nargin < 7, + MaxIter = 20; +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; +end; + +% Initialization: + +[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + +% 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) + +H = [Rckk(:,1:2) Tckk]; + +% Computes the reprojection error in pixels: + +x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + +ex = x_kk - x; + + +% Converts the homography in pixel units: + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; + +H = KK*H; + + + + +return; + + +% Test of compte extrinsic: + +Np = 4; +sx = 10; +sy = 10; +sz = 5; + +om = randn(3,1); +T = [0;0;100]; + +noise = 2/1000; + +XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; +xx = project_points(XX,om,T); + +xxn = xx + noise * randn(2,Np); + +[omckk,Tckk] = compute_extrinsic(xxn,XX); + +[om omckk om-omckk] +[T Tckk T-Tckk] + +figure(3); +plot(xx(1,:),xx(2,:),'r+'); +hold on; +plot(xxn(1,:),xxn(2,:),'g+'); +hold off; Index: /trunk/src/toolbox_calib/compute_extrinsic_init.m =================================================================== --- /trunk/src/toolbox_calib/compute_extrinsic_init.m (revision 725) +++ /trunk/src/toolbox_calib/compute_extrinsic_init.m (revision 725) @@ -0,0 +1,215 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +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; +end; + + +%keyboard; + +% Compute the normalized coordinates: + +xn = normalize_pixel(x_kk,fc,cc,kc,alpha_c); + + + +Np = size(xn,2); + +%% Check for planarity of the structure: +%keyboard; + +X_mean = mean(X_kk')'; + +Y = X_kk - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +%keyboard; + + +if (r < 1e-3)|(Np < 5), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity + + %fprintf(1,'Planar structure detected: r=%f\n',r); + + % Transform the plane to bring it in the Z=0 plane: + + R_transform = V'; + + %norm(R_transform(1:2,3)) + + if norm(R_transform(1:2,3)) < 1e-6, + R_transform = eye(3); + end; + + if det(R_transform) < 0, R_transform = -R_transform; end; + + T_transform = -(R_transform)*X_mean; + + X_new = R_transform*X_kk + T_transform*ones(1,Np); + + + % Compute the planar homography: + + H = compute_homography(xn,X_new(1:2,:)); + + % De-embed the motion parameters from the homography: + + sc = mean([norm(H(:,1));norm(H(:,2))]); + + H = H/sc; + + % Extra normalization for some reasons... + %H(:,1) = H(:,1)/norm(H(:,1)); + %H(:,2) = H(:,2)/norm(H(:,2)); + + if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!! + + % Case#1: no opposite sign: + + omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk1 = rodrigues(omckk1); + Tckk1 = H(:,3); + + Hs1 = [Rckk1(:,1:2) Tckk1]; + xn1 = Hs1*[X_new(1:2,:);ones(1,Np)]; + xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)]; + e1 = xn1 - xn; + + % Case#2: opposite sign: + + omckk2 = rodrigues([-H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk2 = rodrigues(omckk2); + Tckk2 = -H(:,3); + + Hs2 = [Rckk2(:,1:2) Tckk2]; + xn2 = Hs2*[X_new(1:2,:);ones(1,Np)]; + xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)]; + e2 = xn2 - xn; + + if 1, %norm(e1) < norm(e2), + omckk = omckk1; + Tckk = Tckk1; + Rckk = Rckk1; + else + omckk = omckk2; + Tckk = Tckk2; + Rckk = Rckk2; + end; + + else + + u1 = H(:,1); + u1 = u1 / norm(u1); + u2 = H(:,2) - dot(u1,H(:,2)) * u1; + u2 = u2 / norm(u2); + u3 = cross(u1,u2); + RRR = [u1 u2 u3]; + omckk = rodrigues(RRR); + + %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk = rodrigues(omckk); + Tckk = H(:,3); + + end; + + + + %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform + + Tckk = Tckk + Rckk* T_transform; + Rckk = Rckk * R_transform; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + + +else + + %fprintf(1,'Non planar structure detected: r=%f\n',r); + + % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!): + % The DLT method is applied here!! + + J = zeros(2*Np,12); + + xX = (ones(3,1)*xn(1,:)).*X_kk; + yX = (ones(3,1)*xn(2,:)).*X_kk; + + J(1:2:end,[1 4 7]) = -X_kk'; + J(2:2:end,[2 5 8]) = X_kk'; + J(1:2:end,[3 6 9]) = xX'; + J(2:2:end,[3 6 9]) = -yX'; + J(1:2:end,12) = xn(1,:)'; + J(2:2:end,12) = -xn(2,:)'; + J(1:2:end,10) = -ones(Np,1); + J(2:2:end,11) = ones(Np,1); + + JJ = J'*J; + [U,S,V] = svd(JJ); + + RR = reshape(V(1:9,12),3,3); + + if det(RR) < 0, + V(:,12) = -V(:,12); + RR = -RR; + end; + + [Ur,Sr,Vr] = svd(RR); + + Rckk = Ur*Vr'; + + sc = norm(V(1:9,12)) / norm(Rckk(:)); + Tckk = V(10:12,12)/sc; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + +end; Index: /trunk/src/toolbox_calib/compute_extrinsic_refine.m =================================================================== --- /trunk/src/toolbox_calib/compute_extrinsic_refine.m (revision 725) +++ /trunk/src/toolbox_calib/compute_extrinsic_refine.m (revision 725) @@ -0,0 +1,114 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize_pixel: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(5,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + ex = x_kk - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); Index: /trunk/src/toolbox_calib/compute_homography.m =================================================================== --- /trunk/src/toolbox_calib/compute_homography.m (revision 725) +++ /trunk/src/toolbox_calib/compute_homography.m (revision 725) @@ -0,0 +1,175 @@ +function [H,Hnorm,inv_Hnorm] = compute_homography(m,M); + +%compute_homography +% +%[H,Hnorm,inv_Hnorm] = compute_homography(m,M) +% +%Computes the planar homography between the point coordinates on the plane (M) and the image +%point coordinates (m). +% +%INPUT: m: homogeneous coordinates in the image plane (3xN matrix) +% M: homogeneous coordinates in the plane in 3D (3xN matrix) +% +%OUTPUT: H: Homography matrix (3x3 homogeneous matrix) +% Hnorm: Normalization matrix used on the points before homography computation +% (useful for numerical stability is points in pixel coordinates) +% inv_Hnorm: The inverse of Hnorm +% +%Definition: m ~ H*M where "~" means equal up to a non zero scalar factor. +% +%Method: First computes an initial guess for the homography through quasi-linear method. +% Then, if the total number of points is larger than 4, optimize the solution by minimizing +% the reprojection error (in the least squares sense). +% +% +%Important functions called within that program: +% +%comp_distortion_oulu: Undistorts pixel coordinates. +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +%project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian +% matrix (derivative with respect to the intrinsic and extrinsic parameters). +% This function is called within the minimization loop. + + + + +Np = size(m,2); + +if size(m,1)<3, + m = [m;ones(1,Np)]; +end; + +if size(M,1)<3, + M = [M;ones(1,Np)]; +end; + + +m = m ./ (ones(3,1)*m(3,:)); +M = M ./ (ones(3,1)*M(3,:)); + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = m(1,:); +ay = m(2,:); + +mxx = mean(ax); +myy = mean(ay); +ax = ax - mxx; +ay = ay - myy; + +scxx = mean(abs(ax)); +scyy = mean(abs(ay)); + + +Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; +inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; + +mn = Hnorm*m; + +% Compute the homography between m and mn: + +% Build the matrix: + +L = zeros(2*Np,9); + +L(1:2:2*Np,1:3) = M'; +L(2:2:2*Np,4:6) = M'; +L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)'; +L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)'; + +if Np > 4, + L = L'*L; +end; + +[U,S,V] = svd(L); + +hh = V(:,9); +hh = hh/hh(9); + +Hrem = reshape(hh,3,3)'; +%Hrem = Hrem / Hrem(3,3); + + +% Final homography: + +H = inv_Hnorm*Hrem; + +if 0, + 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; + + J = zeros(2*Np,8); + + MMM = (M ./ (ones(3,1)*mrep(3,:))); + + J(1:2:2*Np,1:3) = -MMM'; + J(2:2:2*Np,4:6) = -MMM'; + + mrep = mrep ./ (ones(3,1)*mrep(3,:)); + + m_err = m(1:2,:) - mrep(1:2,:); + m_err = m_err(:); + + MMM2 = (ones(3,1)*mrep(1,:)) .* MMM; + MMM3 = (ones(3,1)*mrep(2,:)) .* MMM; + + J(1:2:2*Np,7:8) = MMM2(1:2,:)'; + J(2:2:2*Np,7:8) = MMM3(1:2,:)'; + + MMM = (M ./ (ones(3,1)*mrep(3,:)))'; + + hh_innov = inv(J'*J)*J'*m_err; + + 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; +end; + +return; + +%test of Jacobian + +mrep = H*M; +mrep = mrep ./ (ones(3,1)*mrep(3,:)); + +m_err = mrep(1:2,:) - m(1:2,:); +figure(8); +plot(m_err(1,:),m_err(2,:),'r+'); +std(m_err') Index: /trunk/src/toolbox_calib/data_calib.m =================================================================== --- /trunk/src/toolbox_calib/data_calib.m (revision 725) +++ /trunk/src/toolbox_calib/data_calib.m (revision 725) @@ -0,0 +1,108 @@ +%%% This script alets the user enter the name of the images (base name, numbering scheme,... + + +% Checks that there are some images in the directory: + +l_ras = dir('*ras'); +s_ras = size(l_ras,1); +l_bmp = dir('*bmp'); +s_bmp = size(l_bmp,1); +l_tif = dir('*tif'); +s_tif = size(l_tif,1); +l_pgm = dir('*pgm'); +s_pgm = size(l_pgm,1); +l_ppm = dir('*ppm'); +s_ppm = size(l_ppm,1); +l_jpg = dir('*jpg'); +s_jpg = size(l_jpg,1); +l_jpeg = dir('*jpeg'); +s_jpeg = size(l_jpeg,1); + +s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; + +if s_tot < 1, + fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); + break; +end; + + +% IF yes, display the directory content: + +dir; + +Nima_valid = 0; + +while (Nima_valid==0), + + fprintf(1,'\n'); + calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); + + format_image = '0'; + + while format_image == '0', + + format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image), + format_image = 'ras'; + end; + + if lower(format_image(1)) == 'm', + format_image = 'ppm'; + else + if lower(format_image(1)) == 'b', + format_image = 'bmp'; + else + if lower(format_image(1)) == 't', + format_image = 'tif'; + else + if lower(format_image(1)) == 'p', + format_image = 'pgm'; + else + if lower(format_image(1)) == 'j', + format_image = 'jpg'; + else + if lower(format_image(1)) == 'r', + format_image = 'ras'; + else + if lower(format_image(1)) == 'g', + format_image = 'jpeg'; + else + disp('Invalid image format'); + format_image = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + end; + + + check_directory; + +end; + + + +%string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num'; + +%eval(string_save); + + + +if (Nima_valid~=0), + % Reading images: + + ima_read_calib; % may be launched from the toolbox itself + % Show all the calibration images: + + if ~isempty(ind_read), + + mosaic; + + end; + +end; + Index: /trunk/src/toolbox_calib/go_calib_optim.m =================================================================== --- /trunk/src/toolbox_calib/go_calib_optim.m (revision 725) +++ /trunk/src/toolbox_calib/go_calib_optim.m (revision 725) @@ -0,0 +1,84 @@ +%go_calib_optim +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length + +if ~exist('rosette_calibration','var') + rosette_calibration = 0; +end; + +if ~exist('n_ima'), + data_calib; % Load the images + click_calib; % Extract the corners +end; + + +check_active_images; + +check_extracted_images; + +check_active_images; + +desactivated_images = []; + +recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. + +if (rosette_calibration) + %%% Special Setting for the Rosette: + est_dist = ones(5,1); +end; + + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter; + + +if ~isempty(desactivated_images), + + param_list_save = param_list; + + fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); + active_images(desactivated_images) = ones(1,length(desactivated_images)); + desactivated_images = []; + + go_calib_optim_iter; + + if ~isempty(desactivated_images), + fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); + end; + + param_list = [param_list_save(:,1:end-1) param_list]; + +end; + + +%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%% + +%graphout_calib; + Index: /trunk/src/toolbox_calib/go_calib_optim_iter.m =================================================================== --- /trunk/src/toolbox_calib/go_calib_optim_iter.m (revision 725) +++ /trunk/src/toolbox_calib/go_calib_optim_iter.m (revision 725) @@ -0,0 +1,635 @@ +%go_calib_optim_iter +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% alpha_c: Skew coefficient +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + +if ~exist('desactivated_images'), + desactivated_images = []; +end; + + + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +if ~exist('est_fc'); + est_fc = [1;1]; % Set to zero if you do not want to estimate the focal length (it may be useful! believe it or not!) +end; + +if ~exist('recompute_extrinsic'), + recompute_extrinsic = 1; % Set this variable to 0 in case you do not want to recompute the extrinsic parameters + % at each iterstion. +end; + +if ~exist('MaxIter'), + MaxIter = 30; % Maximum number of iterations in the gradient descent +end; + +if ~exist('check_cond'), + check_cond = 1; % Set this variable to 0 in case you don't want to extract view dynamically +end; + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +if exist('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if ~exist('est_dist'), + est_dist = [1;1;1;1;0]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = double(~~center_optim); +est_alpha = double(~~est_alpha); +est_dist = double(~~est_dist); +est_fc = double(~~est_fc); +est_aspect_ratio = double(~~est_aspect_ratio); + + + +fprintf(1,'\n'); + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480. If these are not the right values, change values manually.\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'WARNING: Principal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5) & ~rig3D, + fprintf(1,'WARNING: The principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + %if ~dont_ask, + % quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + % center_optim = isempty(quest); + %end; +end; + + +% A quick fix for solving conflict +if ~isequal(est_fc,[1;1]), + est_aspect_ratio=1; +end; +if ~est_aspect_ratio, + est_fc=[1;1]; +end; + + +if ~est_aspect_ratio, + fprintf(1,'Aspect ratio not optimized (est_aspect_ratio = 0) -> fc(1)=fc(2). Set est_aspect_ratio to 1 for estimating aspect ratio.\n'); +else + if isequal(est_fc,[1;1]), + fprintf(1,'Aspect ratio optimized (est_aspect_ratio = 1) -> both components of fc are estimated (DEFAULT).\n'); + end; +end; + +if ~isequal(est_fc,[1;1]), + if isequal(est_fc,[1;0]), + fprintf(1,'The first component of focal (fc(1)) is estimated, but not the second one (est_fc=[1;0])\n'); + else + if isequal(est_fc,[0;1]), + fprintf(1,'The second component of focal (fc(1)) is estimated, but not the first one (est_fc=[0;1])\n'); + else + fprintf(1,'The focal vector fc is not optimized (est_fc=[0;0])\n'); + end; + end; +end; + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). '); + if ~exist('cc'), + fprintf(1,'It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + else + fprintf(1,'Note: to set it in the middle of the image, clear variable cc, and run calibration again.\n'); + end; +else + fprintf(1,'Principal point optimized (center_optim=1) - (DEFAULT). To reject principal point, set center_optim=0\n'); +end; + + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation (center_optim=0), no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0) - (DEFAULT)\n'); + alpha_c = 0; +else + fprintf(1,'Skew optimized (est_alpha=1). To disable skew estimation, set est_alpha=0.\n'); +end; + + +if ~prod(double(est_dist)), + fprintf(1,'Distortion not fully estimated (defined by the variable est_dist):\n'); + if ~est_dist(1), + fprintf(1,' Second order distortion not estimated (est_dist(1)=0).\n'); + end; + if ~est_dist(2), + fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0).\n'); + end; + if ~est_dist(5), + fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT) .\n'); + end; + if ~prod(double(est_dist(3:4))), + fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]).\n'); + end; +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 0.1; % set alpha_smooth = 1; for steepest gradient descent +% alpha_smooth = 0.01; % modified L. Gostiaux + + +% Conditioning threshold for view rejection +thresh_cond = 1e5; + + + +% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.1; % slow convergence +end; + + +if exist('kc'), + if length(kc) == 4; + fprintf(1,'Adding a new distortion coefficient to kc -> radial distortion model up to the 6th degree'); + kc = [kc;0]; + end; +end; + + + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.1; % slow convergence +end; + +if ~exist('fc')& quick_init, + FOV_angle = 35; % Initial camera field of view in degrees + fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']); + fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.1; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n') + init_intrinsic_param; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.1; % slow convergence + est_fc = [1;1]; +end; + + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(5,1); + alpha_smooth = 0.1; % slow convergence +end; + +if ~est_aspect_ratio, + fc(1) = (fc(1)+fc(2))/2; + fc(2) = fc(1); +end; + +if ~prod(double(est_dist)), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + +if ~prod(double(est_fc)), + fprintf(1,'Warning: The focal length is not fully estimated (est_fc ~= [1;1])\n'); +end; + + +%%% Initialization of the extrinsic parameters for global minimization: +comp_ext_calib; + + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(5,1)]; + +for kk = 1:n_ima, + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + init_param = [init_param; omckk ; Tckk]; +end; + + + +%-------------------- 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-2)&(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:10); + + + % 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_points2(X_kk,omckk,Tckk,f(1),c,k,alpha); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha); + 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 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(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)'; + + 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: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]); + + + %% 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(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(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: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_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + 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); + +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 (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] ? [ %3.5f %3.5f ]\n',[fc;fc_error]); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ] ? [ %3.5f %3.5f ]\n',[cc;cc_error]); +fprintf(1,'Skew: alpha_c = [ %3.5f ] ? [ %3.5f ] => angle of pixel axes = %3.5f ? %3.5f degrees\n',[alpha_c;alpha_c_error],90 - atan(alpha_c)*180/pi,atan(alpha_c_error)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] ? [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc;kc_error]); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); +fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') +%fprintf(1,' For accurate (and stable) error estimates, it is recommended to run Calibration once again.\n\n\n') + + + +%%% Some recommendations to the user to reject some of the difficult unkowns... Still in debug mode. + +alpha_c_min = alpha_c - alpha_c_error/2; +alpha_c_max = alpha_c + alpha_c_error/2; + +if (alpha_c_min < 0) & (alpha_c_max > 0), + fprintf(1,'Recommendation: The skew coefficient alpha_c is found to be equal to zero (within its uncertainty).\n'); + fprintf(1,' You may want to reject it from the optimization by setting est_alpha=0 and run Calibration\n\n'); +end; + +kc_min = kc - kc_error/2; +kc_max = kc + kc_error/2; + +prob_kc = (kc_min < 0) & (kc_max > 0); + +if ~(prob_kc(3) & prob_kc(4)) + prob_kc(3:4) = [0;0]; +end; + + +if sum(prob_kc), + fprintf(1,'Recommendation: Some distortion coefficients are found equal to zero (within their uncertainties).\n'); + fprintf(1,' To reject them from the optimization set est_dist=[%d;%d;%d;%d;%d] and run Calibration\n\n',est_dist & ~prob_kc); +end; + + +return; Index: /trunk/src/toolbox_calib/ima_read_calib.m =================================================================== --- /trunk/src/toolbox_calib/ima_read_calib.m (revision 725) +++ /trunk/src/toolbox_calib/ima_read_calib.m (revision 725) @@ -0,0 +1,150 @@ + +if ~exist('calib_name')|~exist('format_image'), + data_calib; + return; +end; + +check_directory; + +if ~exist('n_ima'), + data_calib; + return; +end; + +check_active_images; + + +images_read = active_images; + + +if exist('image_numbers'), + first_num = image_numbers(1); +end; + + +% Just to fix a minor bug: +if ~exist('first_num'), + first_num = image_numbers(1); +end; + + +image_numbers = first_num:n_ima-1+first_num; + +no_image_file = 0; + +i = 1; + +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)); + else + Ii = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + Ii = readras(ima_name); + else + Ii = double(imread(ima_name)); + 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; + +end; + + +ind_read = find(images_read); + + + + +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; + + + fprintf(1,'\n'); + + 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'; + + %eval(string_save); + + disp('done'); + %click_calib; + +end; + +if ~(exist('map')==1), map = gray(256); end; + +active_images = images_read; + Index: /trunk/src/toolbox_calib/init_intrinsic_param.m =================================================================== --- /trunk/src/toolbox_calib/init_intrinsic_param.m (revision 725) +++ /trunk/src/toolbox_calib/init_intrinsic_param.m (revision 725) @@ -0,0 +1,187 @@ +%init_intrinsic_param +% +%Initialization of the intrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: skew coefficient +% KK: The camera matrix (containing fc, cc and alpha_c) +% +%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes +% the focal length fc from orthogonal vanishing points constraint. +% The principal point cc is assumed at the center of the image. +% Assumes no image distortion (kc = [0;0;0;0]) +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +% +%Important function called within that program: +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +% +%VERY IMPORTANT: This function works only with 2D rigs. +%In the future, a more general function will be there (working with 3D rigs as well). + + +if ~exist('two_focals_init'), + two_focals_init = 0; +end; + +if ~exist('est_aspect_ratio'), + est_aspect_ratio = 1; +end; + +check_active_images; + +if ~exist(['x_' num2str(ind_active(1)) ]), + click_calib; +end; + + +fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); + + +% Initialize the homographies: + +for kk = 1:n_ima, + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + active_images(kk) = 0; + end; + if active_images(kk), + eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']); + else + eval(['H_' num2str(kk) ' = NaN*ones(3,3);']); + end; +end; + +check_active_images; + +% initial guess for principal point and distortion: + +if ~exist('nx'), [ny,nx] = size(I); end; + +c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image +k_init = [0;0;0;0;0]; % initialize to zero (no distortion) + + + +% Compute explicitely the focal length using all the (mutually orthogonal) vanishing points +% note: The vanihing points are hidden in the planar collineations H_kk + +A = []; +b = []; + +% matrix that subtract the principal point: +Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1]; + +for kk=1:n_ima, + + if active_images(kk), + + eval(['Hkk = H_' num2str(kk) ';']); + + Hkk = Sub_cc * Hkk; + + % Extract vanishing points (direct and diagonals): + + V_hori_pix = Hkk(:,1); + V_vert_pix = Hkk(:,2); + V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2; + V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2; + + V_hori_pix = V_hori_pix/norm(V_hori_pix); + V_vert_pix = V_vert_pix/norm(V_vert_pix); + V_diag1_pix = V_diag1_pix/norm(V_diag1_pix); + V_diag2_pix = V_diag2_pix/norm(V_diag2_pix); + + a1 = V_hori_pix(1); + b1 = V_hori_pix(2); + c1 = V_hori_pix(3); + + a2 = V_vert_pix(1); + b2 = V_vert_pix(2); + c2 = V_vert_pix(3); + + a3 = V_diag1_pix(1); + b3 = V_diag1_pix(2); + c3 = V_diag1_pix(3); + + a4 = V_diag2_pix(1); + b4 = V_diag2_pix(2); + c4 = V_diag2_pix(3); + + A_kk = [a1*a2 b1*b2; + a3*a4 b3*b4]; + + b_kk = -[c1*c2;c3*c4]; + + + A = [A;A_kk]; + b = [b;b_kk]; + + end; + +end; + + +% use all the vanishing points to estimate focal length: + + +% Select the model for the focal. (solution to Gerd's problem) +if ~two_focals_init + if b'*(sum(A')') < 0, + two_focals_init = 1; + end; +end; + + + +if two_focals_init + % Use a two focals estimate: + f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess +else + % Use a single focal estimate: + f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used +end; + + +if ~est_aspect_ratio, + f_init(1) = (f_init(1)+f_init(2))/2; + f_init(2) = f_init(1); +end; + +alpha_init = 0; + +%f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used + + +% Global calibration matrix (initial guess): + +KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; +inv_KK = inv(KK); + + +cc = c_init; +fc = f_init; +kc = k_init; +alpha_c = alpha_init; + + +fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); +fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',kc); Index: /trunk/src/toolbox_calib/is3D.m =================================================================== --- /trunk/src/toolbox_calib/is3D.m (revision 725) +++ /trunk/src/toolbox_calib/is3D.m (revision 725) @@ -0,0 +1,19 @@ +function test = is3D(X), + + +Np = size(X,2); + +%% Check for planarity of the structure: + +X_mean = mean(X')'; + +Y = X - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +test = (r > 1e-3); + Index: /trunk/src/toolbox_calib/mosaic.m =================================================================== --- /trunk/src/toolbox_calib/mosaic.m (revision 725) +++ /trunk/src/toolbox_calib/mosaic.m (revision 725) @@ -0,0 +1,92 @@ + +if ~exist('I_1'), + active_images_save = active_images; + ima_read_calib; + active_images = active_images_save; + check_active_images; +end; + +check_active_images; + +if isempty(ind_read), + return; +end; + + +n_col = floor(sqrt(n_ima*nx/ny)); + +n_row = ceil(n_ima / n_col); + + +ker2 = 1; +for ii = 1:n_col, + ker2 = conv(ker2,[1/4 1/2 1/4]); +end; + + +II = I_1(1:n_col:end,1:n_col:end); + +[ny2,nx2] = size(II); + + + +kk_c = 1; + +II_mosaic = []; + +for jj = 1:n_row, + + + II_row = []; + + for ii = 1:n_col, + + if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima), + + if active_images(kk_c), + eval(['I = I_' num2str(kk_c) ';']); + %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing + I = I(1:n_col:end,1:n_col:end); + else + I = zeros(ny2,nx2); + end; + + else + + I = zeros(ny2,nx2); + + end; + + + + II_row = [II_row I]; + + if ii ~= n_col, + + II_row = [II_row zeros(ny2,3)]; + + end; + + + kk_c = kk_c + 1; + + end; + + nn2 = size(II_row,2); + + if jj ~= n_row, + II_row = [II_row; zeros(3,nn2)]; + end; + + II_mosaic = [II_mosaic ; II_row]; + +end; + +figure(2); +image(II_mosaic); +colormap(gray(256)); +title('Calibration images'); +set(gca,'Xtick',[]) +set(gca,'Ytick',[]) +axis('image'); + Index: /trunk/src/toolbox_calib/normalize_pixel.m =================================================================== --- /trunk/src/toolbox_calib/normalize_pixel.m (revision 725) +++ /trunk/src/toolbox_calib/normalize_pixel.m (revision 725) @@ -0,0 +1,47 @@ +function [xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) + +%normalize +% +%[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: +% +%comp_distortion_oulu: undistort pixel coordinates. + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First: Subtract principal point, and divide by the focal length: +x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +if norm(kc) ~= 0, + % Third: Compensate for lens distortion: + xn = comp_distortion_oulu(x_distort,kc); +else + xn = x_distort; +end; Index: /trunk/src/toolbox_calib/project_points2.m =================================================================== --- /trunk/src/toolbox_calib/project_points2.m (revision 725) +++ /trunk/src/toolbox_calib/project_points2.m (revision 725) @@ -0,0 +1,342 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha) + +%project_points2.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(5,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +if nargout > 1, + [Y,dYdom,dYdT] = rigid_motion(X,om,T); +else + Y = rigid_motion(X,om,T); +end; + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + +if nargout > 1, + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); + dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); + dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); +end; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +if nargout > 1, + dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); + dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); +end; + + +r4 = r2.^2; + +if nargout > 1, + dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; + dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; +end + +r6 = r2.^3; + +if nargout > 1, + dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; + dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; +end; + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom; + dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT; + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + +xd1 = x .* (ones(2,1)*cdist); + +if nargout > 1, + dxd1dom = zeros(2*n,3); + dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; + dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; + coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + dxd1dom = dxd1dom + coeff.* dxdom; + + dxd1dT = zeros(2*n,3); + dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; + dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; + dxd1dT = dxd1dT + coeff.* dxdT; + + dxd1dk = zeros(2*n,5); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +%ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdom = zeros(2*n,3); + ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); + ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + + ddelta_xdT = zeros(2*n,3); + ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); + ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + + ddelta_xdk = zeros(2*n,5); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + + +xd2 = xd1 + delta_x; + +if nargout > 1, + dxd2dom = dxd1dom + ddelta_xdom ; + dxd2dT = dxd1dT + ddelta_xdT; + dxd2dk = dxd1dk + ddelta_xdk ; +end; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha +if nargout > 1, + dxd3dom = zeros(2*n,3); + dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); + dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); + dxd3dT = zeros(2*n,3); + dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); + dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); + dxd3dk = zeros(2*n,5); + dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); + dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); + dxd3dalpha = zeros(2*n,1); + dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; +end; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + if nargout > 1, + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd3dom; + dxpdT = (coeff*ones(1,3)) .* dxd3dT; + dxpdk = (coeff*ones(1,5)) .* dxd3dk; + dxpdalpha = (coeff) .* dxd3dalpha; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd3(1,:)'; + dxpdf(2:2:end,2) = xd3(2,:)'; + end; +else + xp = f * xd3 + c*ones(1,n); + if nargout > 1, + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); + end; +end; + +if nargout > 1, + dxpdc = zeros(2*n,2); + dxpdc(1:2:end,1) = ones(n,1); + dxpdc(2:2:end,2) = ones(n,1); +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha); + + +% Test on om: OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points2(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points2(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points2(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points2(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = project_points2(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points2(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) Index: /trunk/src/toolbox_calib/rigid_motion.m =================================================================== --- /trunk/src/toolbox_calib/rigid_motion.m (revision 725) +++ /trunk/src/toolbox_calib/rigid_motion.m (revision 725) @@ -0,0 +1,66 @@ +function [Y,dYdom,dYdT] = rigid_motion(X,om,T); + +%rigid_motion.m +% +%[Y,dYdom,dYdT] = rigid_motion(X,om,T) +% +%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% +%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) +% dYdom: Derivative of Y with respect to om ((3N)x3 matrix) +% dYdT: Derivative of Y with respect to T ((3N)x3 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Y = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector + + + +if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure as input (in rigid_motion.m)'); + return; + end; + end; +end; + + +[R,dRdom] = rodrigues(om); + +[m,n] = size(X); + +Y = R*X + repmat(T,[1 n]); + +if nargout > 1, + + +dYdR = zeros(3*n,9); +dYdT = zeros(3*n,3); + +dYdR(1:3:end,1:3:end) = X'; +dYdR(2:3:end,2:3:end) = X'; +dYdR(3:3:end,3:3:end) = X'; + +dYdT(1:3:end,1) = ones(n,1); +dYdT(2:3:end,2) = ones(n,1); +dYdT(3:3:end,3) = ones(n,1); + +dYdom = dYdR * dRdom; + +end; + + + + Index: /trunk/src/toolbox_calib/rodrigues.m =================================================================== --- /trunk/src/toolbox_calib/rodrigues.m (revision 725) +++ /trunk/src/toolbox_calib/rodrigues.m (revision 725) @@ -0,0 +1,267 @@ +function [out,dout]=rodrigues(in) + +% RODRIGUES Transform rotation matrix into rotation vector and viceversa. +% +% Sintax: [OUT]=RODRIGUES(IN) +% If IN is a 3x3 rotation matrix then OUT is the +% corresponding 3x1 rotation vector +% if IN is a rotation 3-vector then OUT is the +% corresponding 3x3 rotation matrix +% + +%% +%% Copyright (c) March 1993 -- Pietro Perona +%% California Institute of Technology +%% + +%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995. +%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !! + +%% BUG when norm(om)=pi fixed -- April 6th, 1997; +%% Jean-Yves Bouguet + +%% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003; +%% Jean-Yves Bouguet + +% BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007 + + +[m,n] = size(in); +%bigeps = 10e+4*eps; +bigeps = 10e+20*eps; + +if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector + theta = norm(in); + if theta < eps + R = eye(3); + + %if nargout > 1, + + dRdin = [0 0 0; + 0 0 1; + 0 -1 0; + 0 0 -1; + 0 0 0; + 1 0 0; + 0 1 0; + -1 0 0; + 0 0 0]; + + %end; + + else + if n==length(in) in=in'; end; %% make it a column vec. if necess. + + %m3 = [in,theta] + + dm3din = [eye(3);in'/theta]; + + omega = in/theta; + + %m2 = [omega;theta] + + dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; + + alpha = cos(theta); + beta = sin(theta); + gamma = 1-cos(theta); + omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; + A = omega*omega'; + + %m1 = [alpha;beta;gamma;omegav;A]; + + dm1dm2 = zeros(21,4); + dm1dm2(1,4) = -sin(theta); + dm1dm2(2,4) = cos(theta); + dm1dm2(3,4) = sin(theta); + dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]'; + + w1 = omega(1); + w2 = omega(2); + w3 = omega(3); + + dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; + dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; + dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; + + R = eye(3)*alpha + omegav*beta + A*gamma; + + dRdm1 = zeros(9,21); + + dRdm1([1 5 9],1) = ones(3,1); + dRdm1(:,2) = omegav(:); + dRdm1(:,4:12) = beta*eye(9); + dRdm1(:,3) = A(:); + dRdm1(:,13:21) = gamma*eye(9); + + dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; + + + end; + out = R; + dout = dRdin; + + %% it is prob. a rot matr. +elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... + & (abs(det(in)-1) < bigeps)) + R = in; + + % project the rotation matrix to SO(3); + [U,S,V] = svd(R); + R = U*V'; + + tr = (trace(R)-1)/2; + dtrdR = [1 0 0 0 1 0 0 0 1]/2; + theta = real(acos(tr)); + + + if sin(theta) >= 1e-4, + + dthetadtr = -1/sqrt(1-tr^2); + + dthetadR = dthetadtr * dtrdR; + % var1 = [vth;theta]; + vth = 1/(2*sin(theta)); + dvthdtheta = -vth*cos(theta)/sin(theta); + dvar1dtheta = [dvthdtheta;1]; + + dvar1dR = dvar1dtheta * dthetadR; + + + om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; + + dom1dR = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]; + + % var = [om1;vth;theta]; + dvardR = [dom1dR;dvar1dR]; + + % var2 = [om;theta]; + om = vth*om1; + domdvar = [vth*eye(3) om1 zeros(3,1)]; + dthetadvar = [0 0 0 0 1]; + dvar2dvar = [domdvar;dthetadvar]; + + + out = om*theta; + domegadvar2 = [theta*eye(3) om]; + + dout = domegadvar2 * dvar2dvar * dvardR; + + + else + if tr > 0; % case norm(om)=0; + + out = [0 0 0]'; + + dout = [0 0 0 0 0 1/2 0 -1/2 0; + 0 0 -1/2 0 0 0 1/2 0 0; + 0 1/2 0 -1/2 0 0 0 0 0]; + else + + % case norm(om)=pi; + if(0) + + %% fixed April 6th by Bouguet -- not working in all cases! + out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); + %keyboard; + + else + + % Solution by Mike Burl on Feb 27, 2007 + % This is a better way to determine the signs of the + % entries of the rotation vector using a hash table on all + % the combinations of signs of a pairs of products (in the + % rotation matrix) + + % Define hashvec and Smat + hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11]; + Smat = [1,1,1; 1,0,-1; 0,1,-1; 1,-1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,-1; + 1,-1,-1; 1,-1,1]; + + M = (R+eye(3,3))/2; + uabs = sqrt(M(1,1)); + vabs = sqrt(M(2,2)); + wabs = sqrt(M(3,3)); + + mvec = [M(1,2), M(2,3), M(1,3)]; + syn = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function + hash = syn * [9; 3; 1]; + idx = find(hash == hashvec); + svec = Smat(idx,:)'; + + out = theta * [uabs; vabs; wabs] .* svec; + + end; + + if nargout > 1, + fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); + dout = NaN*ones(3,9); + end; + end; + end; + +else + error('Neither a rotation matrix nor a rotation vector were provided'); +end; + +return; + +%% test of the Jacobians: + +%%%% TEST OF dRdom: +om = randn(3,1); +dom = randn(3,1)/1000000; + +[R1,dR1] = rodrigues(om); +R2 = rodrigues(om+dom); + +R2a = R1 + reshape(dR1 * dom,3,3); + +gain = norm(R2 - R1)/norm(R2 - R2a) + +%%% TEST OF dOmdR: +om = randn(3,1); +R = rodrigues(om); +dom = randn(3,1)/10000; +dR = rodrigues(om+dom) - R; + +[omc,domdR] = rodrigues(R); +[om2] = rodrigues(R+dR); + +om_app = omc + domdR*dR(:); + +gain = norm(om2 - omc)/norm(om2 - om_app) + + +%%% OTHER BUG: (FIXED NOW!!!) + +omu = randn(3,1); +omu = omu/norm(omu) +om = pi*omu; +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +%%% NORMAL OPERATION + +om = randn(3,1); +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +return + +% Test: norm(om) = pi + +u = randn(3,1); +u = u / sqrt(sum(u.^2)); +om = pi*u; +R = rodrigues(om); + +R2 = rodrigues(rodrigues(R)); + +norm(R - R2)