diff --git a/ea/Ub5/A13b.png b/ea/Ub5/A13b.png new file mode 100644 index 0000000..213fd51 --- /dev/null +++ b/ea/Ub5/A13b.png Binary files differ diff --git a/ea/Ub5/A13c.png b/ea/Ub5/A13c.png new file mode 100644 index 0000000..85b49c7 --- /dev/null +++ b/ea/Ub5/A13c.png Binary files differ diff --git a/ea/Ub5/A13clog.png b/ea/Ub5/A13clog.png new file mode 100644 index 0000000..aab8afd --- /dev/null +++ b/ea/Ub5/A13clog.png Binary files differ diff --git a/ea/Ub5/A15plot.m b/ea/Ub5/A15plot.m new file mode 100644 index 0000000..7ba13e6 --- /dev/null +++ b/ea/Ub5/A15plot.m @@ -0,0 +1,3 @@ +f= @(x) cos(x+2)+x/2 + +fplot(f,[-2*pi,2*pi]) \ No newline at end of file diff --git a/ea/Ub5/EvA2Base.jar b/ea/Ub5/EvA2Base.jar new file mode 100644 index 0000000..ba81559 --- /dev/null +++ b/ea/Ub5/EvA2Base.jar Binary files differ diff --git a/ea/Ub5/GOParameters.ser b/ea/Ub5/GOParameters.ser new file mode 100644 index 0000000..2422490 --- /dev/null +++ b/ea/Ub5/GOParameters.ser Binary files differ diff --git a/ea/Ub5/Statistics.ser b/ea/Ub5/Statistics.ser new file mode 100644 index 0000000..4661123 --- /dev/null +++ b/ea/Ub5/Statistics.ser Binary files differ diff --git a/ea/Ub5/blatt5.pdf b/ea/Ub5/blatt5.pdf new file mode 100644 index 0000000..aff80c8 --- /dev/null +++ b/ea/Ub5/blatt5.pdf Binary files differ diff --git a/ea/Ub5/lastmodule.ser b/ea/Ub5/lastmodule.ser new file mode 100644 index 0000000..9714c0d --- /dev/null +++ b/ea/Ub5/lastmodule.ser Binary files differ diff --git a/mr/Ub5/TOOLBOX_calib/Compute3D.m b/mr/Ub5/TOOLBOX_calib/Compute3D.m new file mode 100755 index 0000000..7d1872a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/Compute3D.m @@ -0,0 +1,90 @@ +function [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); + +% [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); +% +% Reconstruction of the 3D structure of the striped object. +% +% Xc : The 3D coordinates of the points in the camera reference frame +% Xp : The 3D coordinates of the points in the projector reference frame +% +% xc, xp: Camera coordinates and projector coordinates from ComputeStripes +% R,T : rigid motion from camera to projector: Xp = R*Xc + T +% fc,fp : Camera and Projector focal lengths +% cc,cp : Camera and Projector center of projection +% kc,kp : Camera and Projector distortion factors +% alpha_c, alpha_p: skew coefficients for camera and projector +% +% The set R,T,fc,fp,cc,cp and kc comes from the calibration. + +% Intel Corporation - Dec. 2003 +% (c) Jean-Yves Bouguet + + +if nargin < 12, + alpha_p = 0; + if nargin < 11, + alpha_c = 0; + end; +end; + + +Np = size(xc,2); + + +xc = normalize_pixel(xc,fc,cc,kc,alpha_c); + +xp = (xp - cp(1))/fp(1); + +xp_save = xp; % save the real distorted x - coordinates + alpha'ed + + +if (norm(kp) == 0)&(alpha_p == 0), + N_rep = 1; +else + N_rep = 5; +end; + + +% xp is the first entry of the undistorted projector coordinates (iteratively refined) +% xc is the complete undistorted camera coordinates +for kk = 1:N_rep, + + R2 = R([1 3],:); + if length(T) > 2, + Tp = T([1 3]); % The old technique for calibration + else + Tp = T; % The new technique for calibration (using stripes only) + end; + + % Triangulation: + + D1 = [-xc(1,:);xc(1,:).*xp(1,:);-xc(2,:);xc(2,:).*xp(1,:);-ones(1,Np);xp(1,:)]; + D2 = R2(:)*ones(1,Np); + + D = sum(D1.*D2); + N1 = [-ones(1,Np);xp(1,:)]; + N2 = -sum(N1.*(Tp*ones(1,Np))); + Z = N2./D; + Xc = (ones(3,1)*Z).*[xc;ones(1,Np)]; + + % reproject on the projetor view, and apply distortion... + + Xp = R*Xc + T*ones(1,Np); + + xp_v = [Xp(1,:)./Xp(3,:); Xp(2,:)./Xp(3,:)]; + + xp_v(1,:) = xp_v(1,:) + alpha_p * xp_v(2,:); + + xp_dist = apply_distortion(xp_v,kp); + + %norm(xp_dist(1,:) - xp_save) + + xp_dist(1,:) = xp_save; + + xp_v = comp_distortion(xp_dist,kp); + + xp_v(1,:) = xp_v(1,:) - alpha_p * xp_v(2,:); + + xp = xp_v(1,:); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/ComputeStripes.m b/mr/Ub5/TOOLBOX_calib/ComputeStripes.m new file mode 100755 index 0000000..b90108f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ComputeStripes.m @@ -0,0 +1,378 @@ +function [xc,xp,nx,ny] = ComputeStripes(fname,graphout); + +%[xc,xp] = ComputeStripes(fname,graphout) +% +% This function computes the projector stripe coordinate (at subpixel +% accuracy) at every pixel in the image. The algorithm used in temporal +% processing (with a translationnal pattern of period 32 pixels). A coarse +% to fine pattern projection helpresolve for the period ambiguity (in a +% Gray-Code Scheme). +% +% The naming convention is, +% +% fname_pat##p.ras --> for the positive image from pass ## +% fname_pat##n.ras --> for the negative image from pass ## +% +% ##=00: Black and White images +% +% ##=[01 - 10] : Coarse to fine projection (Gary-Scale projection) +% ##=[11 - 42 ]: 32 translational patterns of period 32 pixels (for +% temporal processing) +% INPUT: +% fname --> base name of the images +% graphout --> Set to 1 to show graphical figures +% +% OUTPUT: +% xc is a 2 by N matrix of the point in the image plane +% (convention: (0,0) -> center of the upper left pixel in the camera image) +% xp is a N vector of the corresponding projector stripe numbers (at subpixel accuracy). +% (convention: (0,0) -> center of the upper left pixel in the projector image) +% (nx,ny): Size of the camera image +% +% (c) in 1996 by Jean-Yves Bouguet - Updated 11/26/2003 + + +if nargin < 2, + graphout = 0; +end; + + +N = 10; + + +%-- Load the white and black images: + +blackIm = imread([fname '_pat00p.bmp']); +whiteIm = imread([fname '_pat00n.bmp']); + +if size(blackIm,3) > 1, + blackIm = 0.299 * double(blackIm(:,:,1)) + 0.5870 * double(blackIm(:,:,2)) + 0.114 * double(blackIm(:,:,3)); + whiteIm = 0.299 * double(whiteIm(:,:,1)) + 0.5870 * double(whiteIm(:,:,2)) + 0.114 * double(whiteIm(:,:,3)); +else + blackIm = double(blackIm); + whiteIm = double(whiteIm); +end; + +[ny,nx] = size(blackIm); + + +%%% Contrast Thresholding + +%% good value for cthresh: 20 + +totContr = whiteIm - blackIm; +totContr(totContr < 0) = 0; + +cthresh = 3; %--> totContr is larger than cthresh for valid pixels + +% In order to remove the highlights (reject image regions where whiteIm >254) +hightlight_reject = 1; + + + +%-- Enable processing of a small region of the image instead of the whole image: + +xs = 1; +xe = nx; +ys = 1; +ye = ny; + +totContr = totContr(ys:ye,xs:xe); +whiteIm = whiteIm(ys:ye,xs:xe); +blackIm = blackIm(ys:ye,xs:xe); + +[yPixels xPixels] = size(totContr); + + +good1 = find((totContr > cthresh)&(whiteIm <= 254)); % the first mask!!! (no need to compute anything outside of this mask) +Ng1 = length(good1); + + + + + +%-------------------------------------------------------------------------- +%-- STEP 1: TEMPORAL PROCESSING -> Finding the edge time at every pixel in the image +%-------------------------------------------------------------------------- + +period = 32; % Total Period size in pixels of the projected pattern + +index_list2 = (0:period-1) + N + 1; + + +% Read all temporal images (and compute max and min images): + +for kk=0:period-1, + + tmp = imread([fname '_pat' sprintf('%.2d',index_list2(kk+1)) 'p.bmp']); + + if size(tmp,3) > 1, + tmp = 0.299 * double(tmp(ys:ye,xs:xe,1)) + 0.5870 * double(tmp(ys:ye,xs:xe,2)) + 0.114 * double(tmp(ys:ye,xs:xe,3)); + else + tmp = double(tmp(ys:ye,xs:xe)); + end; + + + eval(['I_' num2str(kk) '= tmp;']); + + if kk == 0, + Imin = tmp; + Imax = tmp; + else + Imin = min(Imin,tmp); + Imax = max(Imax,tmp); + end; + +end; + + +% Substract opposite images (to compute a zero crossing): +for kk = 0:period-1, + + eval(['I_kk = I_' num2str(kk) ';']); + eval(['I_kk2 = I_' num2str(mod(kk+period/2,period)) ';']); + + eval(['J_' num2str(kk) ' = I_kk - I_kk2;']); + +end; + + +% Start computing the edge points: + +not_computed = ones(yPixels,xPixels); +xp_crossings = -ones(yPixels,xPixels); + + +for kk = 1:period, + + eval(['Ja = J_' num2str(mod(kk-3,period)) ';']); + eval(['Jb = J_' num2str(mod(kk-2,period)) ';']); + eval(['Jc = J_' num2str(mod(kk-1,period)) ';']); + eval(['Jd = J_' num2str(mod(kk,period)) ';']); + eval(['Je = J_' num2str(mod(kk+1,period)) ';']); + eval(['Jf = J_' num2str(mod(kk+2,period)) ';']); + + % Temporal Smoothing: (before zero crossing computation) + J_current = (Jb + 4*Jc + 6*Jd + 4*Je + Jf)/16; + J_prev = (Ja + 4*Jb + 6*Jc + 4*Jd + Je)/16; + + ind_found = find( (J_current >= 0) & (J_prev < 0) & (not_computed) ); + + J_current = J_current(ind_found); + J_prev = J_prev(ind_found); + + xp_crossings(ind_found) = (kk - (J_current ./ (J_current - J_prev))) - 0.5; + + not_computed(ind_found) = zeros(length(ind_found),1); + +end; + +% Final temporal solution: +xp_crossings = mod(xp_crossings,period); + +if graphout, + figure(3); + image(xp_crossings*8); + colormap(gray(256)); + title('STEP1: Subpixel projector coordinate with a 32 pixel ambiguity'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 2: SPATIAL PROCESSING -> Coarse to fine processing to resolve ambiguity +%-------------------------------------------------------------------------- + +bin_current = zeros(yPixels,xPixels); + +num_period = zeros(yPixels,xPixels); + +for i = 1:N-log2(period), + + + tmpN = imread([fname '_pat' sprintf('%.2d',i) 'p.bmp']); + tmpI = imread([fname '_pat' sprintf('%.2d',i) 'n.bmp']); + + if size(tmpN,3) > 1, + tmpN = 0.299 * double(tmpN(ys:ye,xs:xe,1)) + 0.5870 * double(tmpN(ys:ye,xs:xe,2)) + 0.114 * double(tmpN(ys:ye,xs:xe,3)); + tmpI = 0.299 * double(tmpI(ys:ye,xs:xe,1)) + 0.5870 * double(tmpI(ys:ye,xs:xe,2)) + 0.114 * double(tmpI(ys:ye,xs:xe,3)); + else + tmpN = double(tmpN(ys:ye,xs:xe)); + tmpI = double(tmpI(ys:ye,xs:xe)); + end; + + diffI = (tmpN-tmpI)>0; + + bin_current = xor(bin_current,diffI); + + num_period = num_period + (2^(N-i))*bin_current; + +end; + + +if graphout, + figure(4); + image(num_period/4); + colormap(gray(256)); + title('STEP2: Period number (for removing the periodic ambiguity)'); + drawnow; +end; + + +% Finish off the spatial processing to higher resolution: + +xp_spatial = num_period; + +finer_image = 2; + +for i = N-log2(period)+1:N-finer_image+1, + + + tmpN = imread([fname '_pat' sprintf('%.2d',i) 'p.bmp']); + tmpI = imread([fname '_pat' sprintf('%.2d',i) 'n.bmp']); + + if size(tmpN,3) > 1, + tmpN = 0.299 * double(tmpN(ys:ye,xs:xe,1)) + 0.5870 * double(tmpN(ys:ye,xs:xe,2)) + 0.114 * double(tmpN(ys:ye,xs:xe,3)); + tmpI = 0.299 * double(tmpI(ys:ye,xs:xe,1)) + 0.5870 * double(tmpI(ys:ye,xs:xe,2)) + 0.114 * double(tmpI(ys:ye,xs:xe,3)); + else + tmpN = double(tmpN(ys:ye,xs:xe)); + tmpI = double(tmpI(ys:ye,xs:xe)); + end; + + diffI = (tmpN-tmpI)>0; + + bin_current = xor(bin_current,diffI); + + xp_spatial = xp_spatial + (2^(N-i))*bin_current; + +end; + + +% Final spatial solution: +xp_spatial = xp_spatial + (2^(finer_image-1)-1); + + + + +%-------------------------------------------------------------------------- +%-- STEP 3: Solve for periodic ambiguity, and fixing gliches of the temporal processing (due to noise) +% In order to compare xp_spatial and xp_crossings; Not discussed in class +%-------------------------------------------------------------------------- + + +% Fix glitches at the stripe boundaries (of width 4 pixels): + +for kkk = 1:10, + pos_cand = ((num_period == ([1e10*ones(yPixels,1) num_period(:,1:end-1)]+period)) | (num_period == ([1e10*ones(yPixels,2) num_period(:,1:end-2)]+period)))&(xp_crossings > 5*period /6); + neg_cand = ((num_period == ([num_period(:,2:end) zeros(yPixels,1)]-period)) | (num_period == ([num_period(:,3:end) zeros(yPixels,2)]-period)))&(xp_crossings < period /6); + num_period = num_period - period*pos_cand + period*neg_cand; +end; + +xp_crossings2 = xp_crossings + num_period; + +period3 = period / 2; + +% Fix the little glitch at the stripe boundaries: + +% Find single glitches: + +for kkk = 1:5, + delta_x = xp_crossings2(:,2:end)-xp_crossings2(:,1:end-1); + + pos_glitch = (delta_x > 3*period3/4)&(delta_x < 3*period3); + neg_glitch = (delta_x < -period3/4)&(delta_x > -3*period3); + no_glitch = ~pos_glitch & ~neg_glitch; + + % Place to subtract a period: + sub_places = [ (neg_glitch & [zeros(yPixels,1) pos_glitch(:,1:end-1)]) zeros(yPixels,1)] ; + add_places = [ (pos_glitch & [zeros(yPixels,1) neg_glitch(:,1:end-1)]) zeros(yPixels,1)] ; + + xp_crossings3 = xp_crossings2 - period3 * sub_places + period3 * add_places; + + xp_crossings2 = xp_crossings3; + +end; + +% Find double glitches: + +for kkk = 1:5, + delta_x = xp_crossings2(:,2:end)-xp_crossings2(:,1:end-1); + pos_glitch = (delta_x > 3*period3/4)&(delta_x < 3*period3); + neg_glitch = (delta_x < -period3/4)&(delta_x > -3*period3); + no_glitch = ~pos_glitch & ~neg_glitch; + + sub2_places = [((no_glitch)& [zeros(yPixels,1) pos_glitch(:,1:end-1)] & [neg_glitch(:,2:end) zeros(yPixels,1)])|(neg_glitch & [zeros(yPixels,1) no_glitch(:,1:end-1)] & [zeros(yPixels,2) pos_glitch(:,1:end-2)]) zeros(yPixels,1)]; + add2_places = [((no_glitch)& [zeros(yPixels,1) neg_glitch(:,1:end-1)] & [pos_glitch(:,2:end) zeros(yPixels,1)])|(pos_glitch & [zeros(yPixels,1) no_glitch(:,1:end-1)] & [zeros(yPixels,2) neg_glitch(:,1:end-2)]) zeros(yPixels,1)]; + + xp_crossings3 = xp_crossings2 - period3 * sub2_places + period3 * add2_places; + + xp_crossings2 = xp_crossings3; +end; + + +% End fix + + +if graphout, + figure(5); + image(xp_crossings2/4); + colormap(gray(256)); + title('STEP3: Subpixel projector coordinate without the 32 pixel ambiguity'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 4: Compare xp_spatial and xp_crossings2 and retains the +% xp_crossings that are valid +%-------------------------------------------------------------------------- + +%comparison of spatial and temporal xp for rejecting the bad pixels: +err_xp = xp_spatial - xp_crossings2; + +spatial_temporal_agree = (err_xp <= 2^(finer_image-1)) & (err_xp > -1); + +mask_temporal = zeros(yPixels,xPixels); +mask_temporal(2:(yPixels-1),2:(xPixels-1)) = ones(yPixels-2,xPixels-2); + +if hightlight_reject, + highlight = (whiteIm > 254); + mask_good = (totContr > cthresh) & (not_computed >= 0) & mask_temporal & spatial_temporal_agree & ~highlight; +else + mask_good = (totContr > cthresh) & (not_computed >= 0) & mask_temporal & spatial_temporal_agree; +end; + +good1 = find(mask_good); + + +xp_crossings3 = xp_crossings2; + +xp_crossings3(~mask_good) = NaN; + + +if graphout, + figure(6); + image(xp_crossings3/4); + colormap(gray(256)); + title('STEP4: Final clean subpixel projector coordinates'); + drawnow; +end; + + +%-------------------------------------------------------------------------- +%-- STEP 5: Produce the camera coordinates and the projector coordinates xc, xp +%-------------------------------------------------------------------------- + +% extract the good pixels only: +xp = xp_crossings2(good1)'; + +[X,Y] = meshgrid(0:xPixels-1,0:yPixels-1); + +xc = [X(good1)';Y(good1)']; + +%%% Express the coordinates of the points in the original +%%% image coordinates: + +xc(1,:) = xc(1,:) + xs - 1; +xc(2,:) = xc(2,:) + ys - 1; + diff --git a/mr/Ub5/TOOLBOX_calib/Distor2Calib.m b/mr/Ub5/TOOLBOX_calib/Distor2Calib.m new file mode 100755 index 0000000..a82f583 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/Distor2Calib.m @@ -0,0 +1,391 @@ +function [fc_2,Rc_2,Tc_2,H_2,distance,V_vert,V_hori,x_all_c,V_hori_pix,V_vert_pix,V_diag1_pix,V_diag2_pix]=Distor2Calib(k_dist,grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,f_ini,N_iter,two_focal); + +% Computes the calibration parameters knowing the +% distortion factor k_dist + +% grid_pts_centered are the grid point coordinates after substraction of +% the optical center. + +% can give an optional guess for the focal length f_ini (can set to []) +% can provide the number of iterations for the Iterative Vanishing Point Algorithm + +% if the focal length is known perfectly, then, there is no need to iterate, +% and therefore, one can fix: N_iter = 0; + +% California Institute of Technology +% (c) Jean-Yves Bouguet - October 7th, 1997 + + + +%keyboard; + +if exist('two_focal'), + if isempty(two_focal), + two_focal=0; + end; +else + two_focal = 0; +end; + + +if exist('N_iter'), + if ~isempty(N_iter), + disp('Use number of iterations provided'); + else + N_iter = 10; + end; +else + N_iter = 10; +end; + +if exist('f_ini'), + if ~isempty(f_ini), + disp('Use focal provided'); + if length(f_ini)<2, f_ini=[f_ini;f_ini]; end; + fc_2 = f_ini; + x_all_c = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + x_all_c = comp_distortion(x_all_c,k_dist); % we can this time!!! + else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; + end; +else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; +end; + + +dX = W/n_sq_x; +dY = L/n_sq_y; + + +N_x = n_sq_x+1; +N_y = n_sq_y+1; + + +x_grid = zeros(N_x,N_y); +y_grid = zeros(N_x,N_y); + + + + + +%%% Computation of the four vanishing points in pixels + + + x_grid(:) = grid_pts_centered(1,:); + y_grid(:) = grid_pts_centered(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + V_hori_pix = V_hori; + V_vert_pix = V_vert; + V_diag1_pix = V_diag1; + V_diag2_pix = V_diag2; + + +% end of computation of the vanishing points in pixels. + + + + + + + + +if two_focal, % only if we attempt to estimate two focals... + % Use diagonal lines also to add two extra vanishing points (?) + N_min = min(N_x,N_y); + + if N_min < 2, + use_diag = 0; + two_focal = 0; + disp('Cannot estimate two focals (no existing diagonals)'); + else + use_diag = 1; + Delta_N = abs(N_x-N_y); + N_extra = round((N_min - Delta_N - 1)/2); + diag_list = -N_extra:Delta_N+N_extra; + N_diag = length(diag_list); + diag_1 = zeros(3,N_diag); + diag_2 = zeros(3,N_diag); + end; +else + % Give up the use of the diagonals (so far) + % it seems that the error is increased + use_diag = 0; +end; + + + +% The vertical lines: vert, Horizontal lines: hori +vert = zeros(3,n_sq_x+1); +hori = zeros(3,n_sq_y+1); + +for counter_k = 1:N_iter, % the Iterative Vanishing Points Algorithm to + % estimate the focal length accurately + + x_grid(:) = x_all_c(1,:); + y_grid(:) = x_all_c(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + + + + % Estimation of the focal length, and normalization: + + % Compute the ellipsis of (1/f^2) positions: + % a * (1/fx)^2 + b * (1/fx)^2 = -c + + + a1 = V_hori(1); + b1 = V_hori(2); + c1 = V_hori(3); + + a2 = V_vert(1); + b2 = V_vert(2); + c2 = V_vert(3); + + a3 = V_diag1(1); + b3 = V_diag1(2); + c3 = V_diag1(3); + + a4 = V_diag2(1); + b4 = V_diag2(2); + c4 = V_diag2(3); + + + if two_focal, + + + A = [a1*a2 b1*b2;a3*a4 b3*b4]; + b = -[c1*c2;c3*c4]; + + f = sqrt(abs(1./(inv(A)*b))); + + else + + f = sqrt(abs(-(c1*c2*(a1*a2 + b1*b2) + c3*c4*(a3*a4 + b3*b4))/(c1^2*c2^2 + c3^2*c4^2))); + + f = [f;f]; + + end; + + + + % REMARK: + % if both a and b are small, the calibration is impossible. + % if one of them is small, only the other focal length is observable + % if none is small, both focals are observable + + + fc_2 = fc_2 .* f; + + + % DEBUG PART: fix focal to 500... + %fc_2= [500;500]; disp('Line 293 to be earased in Distor2Calib.m'); + + + % end of focal compensation + + % normalize by the current focal: + + x_all = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + + % Compensate by the distortion factor: + + x_all_c = comp_distortion(x_all,k_dist); + +end; + +% At that point, we hope that the distortion is gone... + +x_grid(:) = x_all_c(1,:); +y_grid(:) = x_all_c(2,:); + +for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); +end; + +for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); +end; + +% Vanishing points: +[U,S,V] = svd(vert); +V_vert = U(:,3); +[U,S,V] = svd(hori); +V_hori = U(:,3); + +% Horizon: + +H_2 = cross(V_vert,V_hori); + +% H_2 = cross(V_vert,V_hori); + +% pick a plane in front of the camera (positive depth) +if H_2(3) < 0, H_2 = -H_2; end; + + +% Rotation matrix: + +if V_hori(1) < 0, V_hori = -V_hori; end; + +V_hori = V_hori/norm(V_hori); +H_2 = H_2/norm(H_2); + +V_hori = V_hori - dot(V_hori,H_2)*H_2; + +Rc_2 = [V_hori cross(H_2,V_hori) H_2]; + +Rc_2 = Rc_2 / det(Rc_2); + +%omc_2 = rodrigues(Rc_2); + +%Rc_2 = rodrigues(omc_2); + +% Find the distance of the plane for translation vector: + +xc_2 = [x_all_c;ones(1,Np)]; + +Zc_2 = 1./sum(xc_2 .* (Rc_2(:,3)*ones(1,Np))); + +Xo_2 = [sum(xc_2 .* (Rc_2(:,1)*ones(1,Np))).*Zc_2 ; sum(xc_2 .* (Rc_2(:,2)*ones(1,Np))).*Zc_2]; + +XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np); + +distance_x = norm(Xgrid_2(1,:))/norm(XXo_2(1,:)); +distance_y = norm(Xgrid_2(2,:))/norm(XXo_2(2,:)); + + +distance = sum(sum(XXo_2(1:2,:).*Xgrid_2(1:2,:)))/sum(sum(XXo_2(1:2,:).^2)); + +alpha = abs(distance_x - distance_y)/distance; + +if (alpha>0.1)&~two_focal, + disp('Should use two focals in x and y...'); +end; + +% Deduce the translation vector: + +Tc_2 = distance * H_2; + + + + + +return; + + V_hori_pix/V_hori_pix(3) + V_vert_pix/V_vert_pix(3) + V_diag1_pix/V_diag1_pix(3) + V_diag2_pix/V_diag2_pix(3) diff --git a/mr/Ub5/TOOLBOX_calib/Meshing.m b/mr/Ub5/TOOLBOX_calib/Meshing.m new file mode 100755 index 0000000..59a3507 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/Meshing.m @@ -0,0 +1,458 @@ +function [Xc3,tri3,xc3,xp3,dc3,xc_texture,nc3,conf_nc3,Nn3] = Meshing(Xc2,xc2,xp2,Thresh_connect,N_smoothing,om,T,N_x,N_y,fc,cc,kc,alpha_c,fp,cp,kp,alpha_p), + +% scaled connection threshold + +T_connect = Thresh_connect; % scaled threshold + +fprintf(1,'Organizing the data...\n'); + +xp_frac = rem(xp2,1); +xc_frac = rem(xc2(1,:),1); + +if std(xp_frac) > std(xc_frac), + disp('Dense depth map in the image coordinates'); + temporal = 1; + spatial = 0; +else + disp('Dense depth map in the cross image and projector frame'); + temporal = 0; + spatial = 1; +end; + + +if spatial, + + % Something to fix the organization: + xpmin = min(xp2); + xp_ind = round(unique(xp2 - xpmin)); + step_xp = min(diff(xp_ind)); %xp_ind(2) - xp_ind(1); + xp4 = (xp2 - xpmin)/step_xp + xpmin; + + xpmin = min(xp4); + xpmax = max(xp4); + + xcmin = min(xc2(2,:)); + xcmax = max(xc2(2,:)); + +else + + % Something to fix the organization: + + xp4 = xc2(1,:); + + xpmin = min(xp4); + xpmax = max(xp4); + + xcmin = min(xc2(2,:)); + xcmax = max(xc2(2,:)); + +end; + + +Nrow = xcmax - xcmin + 1; +Ncol = xpmax - xpmin + 1; + +Xmesh = zeros(Nrow,Ncol); +Ymesh = zeros(Nrow,Ncol); +Zmesh = zeros(Nrow,Ncol); +Cmesh = zeros(Nrow,Ncol); +xcmesh = zeros(Nrow,Ncol); +ycmesh = zeros(Nrow,Ncol); + +ind_col = round(xp4 - xpmin + 1); +ind_row = xc2(2,:) - xcmin + 1; +ind = ind_row + (ind_col-1)*Nrow; +ni = length(ind); + + +% Good format for ivview: +Xmesh(ind) = Xc2(1,:); +Ymesh(ind) = Xc2(2,:); % tries to have it in the right position +Zmesh(ind) = Xc2(3,:); % tries to have it in the right position +Cmesh(ind) = ones(1,ni); +xcmesh(ind) = xc2(1,:); +ycmesh(ind) = xc2(2,:); + +% Hypothesis on the triangles: + +D1 = Cmesh(2:Nrow,1:(Ncol-1)) & Cmesh(1:(Nrow-1),2:Ncol); +F1 = Cmesh(1:(Nrow-1),1:(Ncol-1)) & D1; +F2 = Cmesh(2:Nrow,2:Ncol) & D1; +C1 = (F1 | F2); + +D2 = Cmesh(1:(Nrow-1),1:(Ncol-1)) & Cmesh(2:Nrow,2:Ncol); +F3 = Cmesh(1:(Nrow-1),2:Ncol) & D2; +F4 = Cmesh(2:Nrow,1:(Ncol-1)) & D2; +C2 = (F3 | F4); + +Ambi = C1 & C2; % ambiguous zones +Div1 = C1 & ~C2; % needs to check relative distance of points +Div2 = ~C1 & C2; % needs to check relative distance of points + +% diagonal measure: + +%Dm1 = abs(Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)); + + +Dm1 =( Xmesh(2:Nrow,1:(Ncol-1)) - Xmesh(1:(Nrow-1),2:Ncol)).^2 + (Ymesh(2:Nrow,1:(Ncol-1)) - Ymesh(1:(Nrow-1),2:Ncol)).^2 + (Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)).^2; + +%Dm2 = abs(Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)); + +Dm2 = (Xmesh(1:(Nrow-1),1:(Ncol-1)) - Xmesh(2:Nrow,2:Ncol)).^2 + (Ymesh(1:(Nrow-1),1:(Ncol-1)) - Ymesh(2:Nrow,2:Ncol)).^2 + (Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)).^2 ; + +Div1n = Div1 | ((Dm1 <= Dm2)&Ambi); +Div2n = Div2 | ((Dm2 < Dm1)&Ambi); + +Div11 = Div1n & F1; +Div12 = Div1n & F2; +Div21 = Div2n & F3; +Div22 = Div2n & F4; + + +% look at local difference: + +dZ_r = abs(Zmesh(:,2:Ncol)-Zmesh(:,1:(Ncol-1))); +dZ_c = abs(Zmesh(2:Nrow,:)-Zmesh(1:(Nrow-1),:)); + +Div11 = Div11 & (dZ_r(1:(Nrow-1),:) 1) & (is < Nrow) & (js > 1) & (js < Ncol)); + + is = is(indd); + js = js(indd); + + sm = is + (js-1)*(Nrow); + sm_t = is + (js-1)*(Nrow) - 1; + sm_b = is + (js-1)*(Nrow) + 1; + sm_r = is + (js)*(Nrow); + sm_l = is + (js-2)*(Nrow); + + sm_tr = is + (js)*(Nrow) - 1; + sm_br = is + (js)*(Nrow) + 1; + sm_tl = is + (js-2)*(Nrow) -1; + sm_bl = is + (js-2)*(Nrow) + 1; + + + XX(sm) = 0.5 * XX(sm) + 0.5 * ((XX(sm_t).*t(sm)+XX(sm_b).*b(sm)+XX(sm_r).*r(sm)+XX(sm_l).*l(sm)+XX(sm_tr).*tr(sm)+XX(sm_br).*br(sm)+XX(sm_tl).*tl(sm)+XX(sm_bl).*bl(sm))./Nn(sm)); + + YY(sm) = 0.5 * YY(sm) + 0.5 * ((YY(sm_t).*t(sm)+YY(sm_b).*b(sm)+YY(sm_r).*r(sm)+YY(sm_l).*l(sm)+YY(sm_tr).*tr(sm)+YY(sm_br).*br(sm)+YY(sm_tl).*tl(sm)+YY(sm_bl).*bl(sm))./Nn(sm)); + + ZZ(sm) = 0.5 * ZZ(sm) + 0.5 * ((ZZ(sm_t).*t(sm)+ZZ(sm_b).*b(sm)+ZZ(sm_r).*r(sm)+ZZ(sm_l).*l(sm)+ZZ(sm_tr).*tr(sm)+ZZ(sm_br).*br(sm)+ZZ(sm_tl).*tl(sm)+ZZ(sm_bl).*bl(sm))./Nn(sm)); + + Xmesh = XX(1:Nrow,1:Ncol); + Ymesh = YY(1:Nrow,1:Ncol); + Zmesh = ZZ(1:Nrow,1:Ncol); + + + %%% reconnect after smoothing: + + % diagonal measure: + + Dm1 =( Xmesh(2:Nrow,1:(Ncol-1)) - Xmesh(1:(Nrow-1),2:Ncol)).^2 + (Ymesh(2:Nrow,1:(Ncol-1)) - Ymesh(1:(Nrow-1),2:Ncol)).^2 + (Zmesh(2:Nrow,1:(Ncol-1)) - Zmesh(1:(Nrow-1),2:Ncol)).^2; + + Dm2 = (Xmesh(1:(Nrow-1),1:(Ncol-1)) - Xmesh(2:Nrow,2:Ncol)).^2 + (Ymesh(1:(Nrow-1),1:(Ncol-1)) - Ymesh(2:Nrow,2:Ncol)).^2 + (Zmesh(1:(Nrow-1),1:(Ncol-1)) - Zmesh(2:Nrow,2:Ncol)).^2 ; + + + Div1n = Div1 | ((Dm1 <= Dm2)&Ambi); + Div2n = Div2 | ((Dm2 < Dm1)&Ambi); + + Div11 = Div1n & F1; + Div12 = Div1n & F2; + Div21 = Div2n & F3; + Div22 = Div2n & F4; + + + % look at local difference: + + dZ_r = abs(Zmesh(:,2:Ncol)-Zmesh(:,1:(Ncol-1))); + dZ_c = abs(Zmesh(2:Nrow,:)-Zmesh(1:(Nrow-1),:)); + + Div11 = Div11 & (dZ_r(1:(Nrow-1),:) n_ima, + active_images = active_images(1:n_ima); + end; +end; + +ind_active = find(active_images); + +% I did not call check_active_images, because I want to prevent a break +%check_active_images; + + +fprintf(1,'\nThis function is useful to select a subset of images to calibrate\n'); + + fprintf(1,'\nThere are currently %d active images selected for calibration (out of %d):\n',length(ind_active),n_ima); + + if ~isempty(ind_active), + + if length(ind_active) > 2, + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + if length(ind_active) == 2, + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + fprintf(1,'%d.',ind_active(end)); + + end; + + + end; + + end; + + + fprintf(1,'\n'); + + if length(ind_active)==0, + fprintf(1,'\nYou probably want to add images\n'); + choice = 1; + else + if length(ind_active)==n_ima, + fprintf(1,'\nYou probably want to suppress images\n'); + choice = 0; + else + choice = 2; + end; + end; + + if (choice~=0) & (choice ~=1), + fprintf(1,'\nDo you want to suppress or add images from that list?\n'); + end; + +while (choice~=0)&(choice~=1), + choice = input('For suppressing images enter 0, for adding images enter 1 ([]=no change): '); + if isempty(choice), + fprintf(1,'No change applied to the list of active images.\n'); + return; + end; + if (choice~=0)&(choice~=1), + disp('Bad entry. Try again.'); + end; +end; + + +if choice, + + ima_numbers = input('Number(s) of image(s) to add ([] = all images) = '); + +if isempty(ima_numbers), + fprintf(1,'All %d images are now active\n',n_ima); + ima_proc = 1:n_ima; + else + ima_proc = ima_numbers; + end; + +else + + + ima_numbers = input('Number(s) of image(s) to suppress ([] = no image) = '); + + if isempty(ima_numbers), + fprintf(1,'No image has been suppressed. No modication of the list of active images.\n',n_ima); + ima_proc = []; + else + ima_proc = ima_numbers; + end; + +end; + +if ~isempty(ima_proc), + + active_images(ima_proc) = choice * ones(1,length(ima_proc)); + +end; + + + check_active_images; + + + fprintf(1,'\nThere is now a total of %d active images for calibration:\n',length(ind_active)); + + if ~isempty(ind_active), + + if length(ind_active) > 2, + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + if length(ind_active) == 2, + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + else + + fprintf(1,'%d.',ind_active(end)); + + end; + + + end; + + end; + + + fprintf(1,'\n\nYou may now run ''Calibration'' to recalibrate based on this new set of images.\n'); + + + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/affine.m b/mr/Ub5/TOOLBOX_calib/affine.m new file mode 100755 index 0000000..c51b3d1 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/affine.m @@ -0,0 +1,33 @@ +function A = affine(X,n,om,T,fc,cc,alpha_c) + +if nargin < 7, + alpha_c = 0; + if nargin < 6, + cc = [0;0]; + if nargin < 5, + fc = [1;1]; + if nargin < 4, + T = zeros(3,1); + if nargin < 3, + om = zeros(3,1); + if nargin < 2, + n = [0;0;-1]; + if nargin < 1, + error('Error: affine needs some arguments: A = affine(X,n,om,T,fc,cc,alpha_c);'); + end; + end; + end; + end; + end; + end; +end; + + +KK = [fc(1) alpha_c*fc(1) cc(1); 0 fc(2) cc(2);0 0 1]; +R = rodrigues(om); +omega = n / dot(n,X); +x = X/X(3); + +H = KK * [R + T*omega'] * inv(KK); + +A = (H(3,3)*H(1:2,1:2) - H(1:2,3)*H(3,1:2) + (H(3,2)*H(1:2,1) - H(3,1)*H(1:2,2))*[x(2) -x(1)])/(H(3,:)*x)^2; diff --git a/mr/Ub5/TOOLBOX_calib/align_structures.m b/mr/Ub5/TOOLBOX_calib/align_structures.m new file mode 100755 index 0000000..b5b02d6 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/align_structures.m @@ -0,0 +1,40 @@ +function [om,T,Y] = align_structures(X1,X2), + +% Find om (R) and T, such that Y = R*X1 + T is as close as +% possible to X2. + +[m,n] = size(X1); + +% initialization: + + + + +% initialize param to no motion: +param = zeros(6,1); +change = 1; + +while change > 1e-6, + + om = param(1:3); + T = param(4:6); + + [Y,dYdom,dYdT] = rigid_motion(X1,om,T); + + J = [dYdom dYdT]; + + err = X2(:) - Y(:); + + param_up = inv(J'*J)*J'*err; + + param = param + param_up; + + change = norm(param_up)/norm(param); + +end; + +om = param(1:3); + +T = param(4:6); + +[Y,dYdom,dYdT] = rigid_motion(X1,om,T); diff --git a/mr/Ub5/TOOLBOX_calib/analyse_error.m b/mr/Ub5/TOOLBOX_calib/analyse_error.m new file mode 100755 index 0000000..0299d59 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/analyse_error.m @@ -0,0 +1,157 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if n_ima ~=0, +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + + +%if ~exist('no_grid'), +no_grid = 0; +%end; + +colors = 'brgkcm'; + + +figure(5); + +for kk = 1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if ~no_grid, + eval(['XX_kk = X_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + end; + + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + end; +end; + +hold off; +axis('equal'); +if 1, %~no_grid, + title('Reprojection error (in pixel) - To exit: right button'); +else + title('Reprojection error (in pixel)'); +end; +xlabel('x'); +ylabel('y'); + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + +if n_ima == 0, + + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); + +else + +err_std = std(ex')'; + +fprintf(1,'Pixel error: err = [ %3.5f %3.5f] (all active images)\n\n',err_std); + + +b = 1; + +while b==1, + + [xp,yp,b] = ginput4(1); + + if b==1, + ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2; + + [mind,indmin] = min(ddd); + + + done = 0; + kk_ima = 1; + while (~done)&(kk_ima<=n_ima), + %fprintf(1,'%d...',kk_ima); + eval(['ex_kk = ex_' num2str(kk_ima) ';']); + sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin))); + if isempty(sol_kk), + kk_ima = kk_ima + 1; + else + done = 1; + end; + end; + + eval(['x_kk = x_' num2str(kk_ima) ';']); + xpt = x_kk(:,sol_kk); + + if ~no_grid, + + eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']); + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + y1 = floor((sol_kk-1)./Nx); + x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx); + + y1 = (n_sq_y+1) - y1; + x1 = x1 + 1; + + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Pattern coordinates (in units of (dX,dY)): (X,Y)=(%d,%d)\n',[x1-1 y1-1]); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + else + + fprintf(1,'\n'); + fprintf(1,'Selected image: %d\n',kk_ima); + fprintf(1,'Selected point index: %d\n',sol_kk); + fprintf(1,'Image coordinates (in pixel): (%3.2f,%3.2f)\n',[xpt']); + fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + + end; + + + if exist(['wintx_' num2str(kk_ima)]), + + eval(['wintx = wintx_' num2str(kk_ima) ';']); + eval(['winty = winty_' num2str(kk_ima) ';']); + + fprintf(1,'Window size: (wintx,winty) = (%d,%d)\n',[wintx winty]); + end; + + + end; + +end; + +disp('done'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/anisdiff.m b/mr/Ub5/TOOLBOX_calib/anisdiff.m new file mode 100755 index 0000000..1365dd6 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/anisdiff.m @@ -0,0 +1,55 @@ +function Is = anisdiff(I,sigI,NIter); + +if nargin < 3, + NIter = 4; + if nargin < 2, + sigI = 10; + end; +end; + +% Function that diffuse an image anisotropially. + +Is = I; + +[ny,nx] = size(I); + +c_n = zeros(ny-2,nx-2); +c_s = zeros(ny-2,nx-2); +c_w = zeros(ny-2,nx-2); +c_e = zeros(ny-2,nx-2); +c_nw = zeros(ny-2,nx-2); +c_ne = zeros(ny-2,nx-2); +c_sw = zeros(ny-2,nx-2); +c_se = zeros(ny-2,nx-2); +c_c = ones(ny-2,nx-2); + + +for k=1:NIter, + + dI_n = Is(2:end-1,2:end-1) - Is(1:end-2,2:end-1); + dI_s = Is(2:end-1,2:end-1) - Is(3:end,2:end-1); + dI_w = Is(2:end-1,2:end-1) - Is(2:end-1,1:end-2); + dI_e = Is(2:end-1,2:end-1) - Is(2:end-1,3:end); + dI_nw = Is(2:end-1,2:end-1) - Is(1:end-2,1:end-2); + dI_ne = Is(2:end-1,2:end-1) - Is(1:end-2,3:end); + dI_sw = Is(2:end-1,2:end-1) - Is(3:end,1:end-2); + dI_se = Is(2:end-1,2:end-1) - Is(3:end,3:end); + + + c_n = exp(-.5*(dI_n/sigI).^2)/8; + c_s = exp(-.5*(dI_s/sigI).^2)/8; + c_w = exp(-.5*(dI_w/sigI).^2)/8; + c_e = exp(-.5*(dI_e/sigI).^2)/8; + c_nw = exp(-.5*(dI_nw/sigI).^2)/16; + c_ne = exp(-.5*(dI_ne/sigI).^2)/16; + c_sw = exp(-.5*(dI_sw/sigI).^2)/16; + c_se = exp(-.5*(dI_se/sigI).^2)/16; + + c_c = 1 - (c_n + c_s + c_w + c_e + c_nw + c_ne + c_sw + c_se); + + + Is(2:end-1,2:end-1) = c_c .* Is(2:end-1,2:end-1) + c_n .* Is(1:end-2,2:end-1) + c_s .* Is(3:end,2:end-1) + ... + c_w .* Is(2:end-1,1:end-2) + c_e .* Is(2:end-1,3:end) + c_nw .* Is(1:end-2,1:end-2) + c_ne .* Is(1:end-2,3:end) + ... + c_sw .* Is(3:end,1:end-2) + c_se .* Is(3:end,3:end); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/apply_distortion.m b/mr/Ub5/TOOLBOX_calib/apply_distortion.m new file mode 100755 index 0000000..4842f1a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/apply_distortion.m @@ -0,0 +1,97 @@ +function [xd,dxddk] = apply_distortion(x,k) + + +% Complete the distortion vector if you are using the simple distortion model: +length_k = length(k); +if length_k <5 , + k = [k ; zeros(5-length_k,1)]; +end; + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +r6 = r2.^3; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + 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]; + +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_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; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; + if length_k < 5, + dxddk = dxddk(:,1:length_k); + end; +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +lk = 1; + +x = 10*randn(2,n); +k = 0.5*randn(lk,1); + +[xd,dxddk] = apply_distortion(x,k); + + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(lk,1); +k2 = k + dk; + +[x2] = apply_distortion(x,k2); + +x_pred = xd + reshape(dxddk * dk,2,n); + + +norm(x2-xd)/norm(x2 - x_pred) diff --git a/mr/Ub5/TOOLBOX_calib/apply_distortion2.m b/mr/Ub5/TOOLBOX_calib/apply_distortion2.m new file mode 100755 index 0000000..6061f56 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/apply_distortion2.m @@ -0,0 +1,106 @@ +function [xd,dxddk,dxddx] = apply_distortion2(x,k) + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +r6 = r2.^3; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2) r6']; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + 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]; + +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_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; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; +end; + +if nargout > 2, + d1 = 1 + k(1).*a2' + k(2).*r2'.*(a2+2*x(1,:).^2)' + k(5).*r4'.*(a2+4*x(1,:).^2)' + 2*k(3).*x(2,:)' + 6*k(4).*x(1,:)'; + d2 = 1 + k(1).*a3' + k(2).*r2'.*(a3+2*x(2,:).^2)' + k(5).*r4'.*(a3+4*x(2,:).^2)' + 6*k(3).*x(2,:)' + 2*k(4).*x(1,:)'; + d3 = (k(1) + 2*k(2).*r2' + 3*k(5).*r4').*a1' + 2*k(3).*x(1,:)' + 2*k(4).*x(2,:)'; + + i = [1:2:2*n 1:2:2*n 2:2:2*n 2:2:2*n]; + j = [1:2:2*n 2:2:2*n 1:2:2*n 2:2:2*n]; + s = [d1' d3' d3' d2']; + + dxddx = sparse(i, j, s, 2*n, 2*n); +end + +return; + +% Test of the Jacobians: + +n = 10; + +x = 10*randn(2,n); +k = 0.5*randn(5,1); + +[xd,dxddk,dxddx] = apply_distortion2(x,k); + + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(5,1); +k2 = k + dk; + +[x2] = apply_distortion2(x,k2); + +x_pred = xd + reshape(dxddk * dk,2,n); + + +norm(x2-xd)/norm(x2 - x_pred) + +% Test on x: +dx = 0.000001 *randn(2,n); +x2 = x + dx; + +xd2 = apply_distortion2(x2,k); + +x_pred = xd + reshape(dxddx*dx(:),2,n); + +norm(xd2-xd)/norm(xd2-x_pred) \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/apply_fisheye_distortion.m b/mr/Ub5/TOOLBOX_calib/apply_fisheye_distortion.m new file mode 100755 index 0000000..7dcdb7a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/apply_fisheye_distortion.m @@ -0,0 +1,47 @@ +function [xd, dxd_dx] = apply_fisheye_distortion(x,k) + +%apply_fisheye_distortion.m +% +%[x] = apply_fisheye_distortion(xd,k) +% +%Apply the fisheye distortions +% +%INPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Fisheye distortion coefficients (5x1 vector) +% +%OUTPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% dxd_dx : jacobian (2x2xN matrix) + +r = sqrt(x(1,:).^2 + x(2,:).^2); + +theta = atan(r); +theta_d = theta .* (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); + +scaling = ones(1,length(r)); + +ind_good = find(r > 1e-8); + +scaling(ind_good) = theta_d(ind_good) ./ r(ind_good); + +xd = x .* (ones(2,1)*scaling); + +if (nargout > 1) + n = size(x,2); + dr_dx = x; % 2xn + dr_dx(:,ind_good) = dr_dx(:,ind_good) ./ repmat(r(ind_good), [2,1]); + + theta_d_r2 = scaling; + theta_d_r2(ind_good) = theta_d_r2(ind_good) ./ r(ind_good); + + dtheta_dr = 1 ./ (1 + r.^2); + dtheta_d_dtheta = 1 + 3*k(1)*theta.^2 + 5*k(2)*theta.^4 + ... + 7*k(3)*theta.^6 + 9*k(4)*theta.^8; + dtheta_d_dr_r = dtheta_d_dtheta .* dtheta_dr; % 1xn + dtheta_d_dr_r(ind_good) = dtheta_d_dr_r(ind_good) ./ r(ind_good); + + dxd_dx = zeros(2,2,n); + for i = 1:n + dxd_dx(:,:,i) = scaling(i)*eye(2,2) + x(:,i)*(dtheta_d_dr_r(i) - ... + theta_d_r2(i))*dr_dx(:,i)'; + end +end diff --git a/mr/Ub5/TOOLBOX_calib/calib.m b/mr/Ub5/TOOLBOX_calib/calib.m new file mode 100755 index 0000000..9be1481 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib.m @@ -0,0 +1,18 @@ +function calib(mode), + +% calib(mode) +% +% Runs the Camera Calibration Toolbox. +% Set mode to 1 to run the memory efficient version. +% Any other value for mode will run the normal version (see documentation) + + +if nargin < 1, + + calib_gui; + +else + + calib_gui(mode); + +end; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/calib_gui.m b/mr/Ub5/TOOLBOX_calib/calib_gui.m new file mode 100755 index 0000000..b104a5b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib_gui.m @@ -0,0 +1,30 @@ +%function calib_gui(mode) + +% calib_gui(mode) +% +% Runs the Camera Calibration Toolbox. +% Set mode to 1 to run the memory efficient version. +% Any other value for mode will run the normal version (see documentation) +% +% INFORMATION ABOUT THE MEMORY EFFICIENT MODE FOR THE CAMERA CALIBRATION TOOLBOX: +% +% If your calibration images are large, or if you calibrate using a lot of images, you may have experienced memory problems +% in Matlab when using the calibration toolbox (OUT OF MEMORY errors). If this is the case, you can now run the +% new memory efficient version of the toolbox that loads every image one by one without storing them all in memory. +% If you choose to run the standard version of the toolbox now, you can always switch to the other memory efficient mode +% later in case the OUT OF MEMORY error message is encountered. The two modes of operation are totally compatible. + + + +cell_list = {}; + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Select mode of operation:'; + +cell_list{1,1} = {'Standard (all the images are stored in memory)','calib_gui_normal;'}; +cell_list{2,1} = {'Memory efficient (the images are loaded one by one)','calib_gui_no_read;'}; +cell_list{3,1} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; + + +show_window(cell_list,fig_number,title_figure,290,18,0,'clean',12); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/calib_gui_fisheye.m b/mr/Ub5/TOOLBOX_calib/calib_gui_fisheye.m new file mode 100755 index 0000000..68485ef --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib_gui_fisheye.m @@ -0,0 +1,39 @@ +%function calib_gui_no_read, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Fisheye Camera Calibration Toolbox'; + +cell_list{1,1} = {'Image names','data_calib_no_read;'}; +cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; +cell_list{1,3} = {'Extract grid corners','click_calib_fisheye_no_read;'}; +cell_list{1,4} = {'Calibration','go_calib_optim_fisheye_no_read;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_fisheye_no_read;'}; %recomp_corner_calib_no_read;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib_fisheye;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results_fisheye;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + + diff --git a/mr/Ub5/TOOLBOX_calib/calib_gui_no_read.m b/mr/Ub5/TOOLBOX_calib/calib_gui_no_read.m new file mode 100755 index 0000000..ba8d354 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib_gui_no_read.m @@ -0,0 +1,37 @@ +%function calib_gui_no_read, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Memory efficient version'; + +cell_list{1,1} = {'Image names','data_calib_no_read;'}; +cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; +cell_list{1,3} = {'Extract grid corners','click_calib_no_read;'}; +cell_list{1,4} = {'Calibration','go_calib_optim_no_read;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_no_read;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% diff --git a/mr/Ub5/TOOLBOX_calib/calib_gui_normal.m b/mr/Ub5/TOOLBOX_calib/calib_gui_normal.m new file mode 100755 index 0000000..5c52498 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib_gui_normal.m @@ -0,0 +1,43 @@ +%function calib_gui_normal, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Camera Calibration Toolbox - Standard Version'; + +clear fc cc kc KK +kc = zeros(5,1); +clearwin; + +cell_list{1,1} = {'Image names','data_calib;'}; +cell_list{1,2} = {'Read images','ima_read_calib;'}; +cell_list{1,3} = {'Extract grid corners','click_calib;'}; +cell_list{1,4} = {'Calibration','go_calib_optim;'}; +cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; +cell_list{2,2} = {'Reproject on images','reproject_calib;'}; +cell_list{2,3} = {'Analyse error','analyse_error;'}; +cell_list{2,4} = {'Recomp. corners','recomp_corner_calib;'}; +cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; +cell_list{3,2} = {'Save','saving_calib;'}; +cell_list{3,3} = {'Load','loading_calib;'}; +cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; +cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; +cell_list{4,2} = {'Undistort image','undistort_image;'}; +cell_list{4,3} = {'Export calib data','export_calib_data;'}; +cell_list{4,4} = {'Show calib results','show_calib_results;'}; +%cell_list{5,1} = {'Smooth images','smooth_images;'}; + + +show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + diff --git a/mr/Ub5/TOOLBOX_calib/calib_stereo.m b/mr/Ub5/TOOLBOX_calib/calib_stereo.m new file mode 100755 index 0000000..e376751 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calib_stereo.m @@ -0,0 +1,541 @@ +% calib_stereo +% Script for Calibrating a stereo rig (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% The two calibration result files must have been saved under Calib_Results_left.mat and Calib_Results_right.mat prior to running this script. +% This script is not fully documented, therefore use it at your own risks. However, it should be rather straighforward to run. +% +% INPUT: Calib_Results_left.mat, Calib_Results_right.mat -> Generated by the standard calibration toolbox on the two cameras individually) +% OUTPUT: Calib_Results_stereo.mat -> The saved result after global stereo calibration +% +% Main result variables stored in Calib_Results_stereo.mat: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered, that probably +% means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. + +% (c) Jean-Yves Bouguet - Intel Corporation +% October 25, 2001 -- Last updated April 12, 2002 + + +fprintf(1,'\n\nStereo Calibration script (for more info, try help calib_stereo)\n\n'); + +if (exist('Calib_Results_left.mat')~=2)|(exist('Calib_Results_right.mat')~=2), + fprintf(1,'Error: Need the left and right calibration files Calib_Results_left.mat and Calib_Results_right.mat to run stereo calibration\n'); + return; +end; + +if ~exist('recompute_intrinsic_right'), + recompute_intrinsic_right = 1; +end; + + +if ~exist('recompute_intrinsic_left'), + recompute_intrinsic_left = 1; +end; + +fprintf(1,'Loading the left camera calibration result file Calib_Results_left.mat...\n'); + +clear calib_name + +load Calib_Results_left; + +fc_left = fc; +cc_left = cc; +kc_left = kc; +alpha_c_left = alpha_c; +KK_left = KK; + +if exist('calib_name'), + calib_name_left = calib_name; + format_image_left = format_image; + type_numbering_left = type_numbering; + image_numbers_left = image_numbers; + N_slots_left = N_slots; +else + calib_name_left = ''; + format_image_left = ''; + type_numbering_left = ''; + image_numbers_left = ''; + N_slots_left = ''; +end; + + +X_left = []; + + +om_left_list = []; +T_left_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_left = [X_left Xckk]; + + om_left_list = [om_left_list omckk]; + + T_left_list = [T_left_list Tckk]; + + end; +end; + + + +fprintf(1,'Loading the right camera calibration result file Calib_Results_right.mat...\n'); + +clear calib_name + +load Calib_Results_right; + +fc_right = fc; +cc_right = cc; +kc_right = kc; +alpha_c_right = alpha_c; +KK_right = KK; + +if exist('calib_name'), + calib_name_right = calib_name; + format_image_right = format_image; + type_numbering_right = type_numbering; + image_numbers_right = image_numbers; + N_slots_right = N_slots; +else + calib_name_right = ''; + format_image_right = ''; + type_numbering_right = ''; + image_numbers_right = ''; + N_slots_right = ''; +end; + +X_right = []; + +om_right_list = []; +T_right_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_right = [X_right Xckk]; + + om_right_list = [om_right_list omckk]; + T_right_list = [T_right_list Tckk]; + + end; +end; + + + + +om_ref_list = []; +T_ref_list = []; +for ii = 1:length(om_left_list), + % Align the structure from the first view: + R_ref = rodrigues(om_right_list(:,ii)) * rodrigues(om_left_list(:,ii))'; + T_ref = T_right_list(:,ii) - R_ref * T_left_list(:,ii); + om_ref = rodrigues(R_ref); + om_ref_list = [om_ref_list om_ref]; + T_ref_list = [T_ref_list T_ref]; +end; + + +% Robust estimate of the initial value for rotation and translation between the two views: +om = median(om_ref_list')'; +T = median(T_ref_list')'; + + + + +if 0, + figure(10); + plot3(X_right(1,:),X_right(2,:),X_right(3,:),'bo'); + hold on; + [Xr2] = rigid_motion(X_left,om,T); + plot3(Xr2(1,:),Xr2(2,:),Xr2(3,:),'r+'); + hold off; + drawnow; +end; + + +R = rodrigues(om); + + + +% Re-optimize now over all the set of extrinsic unknows (global optimization) and intrinsic parameters: + +load Calib_Results_left; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_left_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_left_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['omc_left_' num2str(kk) ' = omc_' num2str(kk) ';']); + eval(['Rc_left_' num2str(kk) ' = Rc_' num2str(kk) ';']); + eval(['Tc_left_' num2str(kk) ' = Tc_' num2str(kk) ';']); + end; +end; + +center_optim_left = center_optim; +est_alpha_left = est_alpha; +est_dist_left = est_dist; +est_fc_left = est_fc; +est_aspect_ratio_left = est_aspect_ratio; +active_images_left = active_images; + + +load Calib_Results_right; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_right_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_right_' num2str(kk) ' = x_' num2str(kk) ';']); + end; +end; + +center_optim_right = center_optim; +est_alpha_right = est_alpha; +est_dist_right = est_dist; +est_fc_right = est_fc; +est_aspect_ratio_right = est_aspect_ratio; +active_images_right = active_images; + + + + +if ~recompute_intrinsic_left, + fprintf(1,'No recomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 0)\n'); + center_optim_left = 0; + est_alpha_left = 0; + est_dist_left = zeros(5,1); + est_fc_left = [0;0]; + est_aspect_ratio_left = 1; % just to fix conflicts +end; + +if ~recompute_intrinsic_right, + fprintf(1,'No recomputation of the intrinsic parameters of the right camera (recompute_intrinsic_left = 0)\n'); + center_optim_right = 0; + est_alpha_right = 0; + est_dist_right = zeros(5,1); + est_fc_right = [0;0]; + est_aspect_ratio_right = 1; % just to fix conflicts +end; + + + + +threshold = 50; %1e10; %50; + +active_images = active_images_left & active_images_right; + +history = []; + +fprintf(1,'\nMain stereo calibration optimization procedure - Number of pairs of images: %d\n',length(find(active_images))); +fprintf(1,'Gradient descent iteration: '); + +for iter = 1:12; + + + fprintf(1,'%d...',iter); + + % Jacobian: + + J = []; + e = []; + + param = [fc_left;cc_left;alpha_c_left;kc_left;fc_right;cc_right;alpha_c_right;kc_right;om;T]; + + + for kk = 1:n_ima, + + if active_images(kk), + + % Project the structure onto the left view: + + eval(['Xckk = X_left_' num2str(kk) ';']); + eval(['omckk = omc_left_' num2str(kk) ';']); + eval(['Tckk = Tc_left_' num2str(kk) ';']); + + eval(['xlkk = x_left_' num2str(kk) ';']); + eval(['xrkk = x_right_' num2str(kk) ';']); + + param = [param;omckk;Tckk]; + + % number of points: + Nckk = size(Xckk,2); + + + Jkk = sparse(4*Nckk,20+(1+n_ima)*6); + ekk = zeros(4*Nckk,1); + + + if ~est_aspect_ratio_left, + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left(1),cc_left,kc_left,alpha_c_left); + dxldfl = repmat(dxldfl,[1 2]); + else + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left,cc_left,kc_left,alpha_c_left); + end; + + + ekk(1:2*Nckk) = xlkk(:) - xl(:); + + Jkk(1:2*Nckk,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxldomckk); + Jkk(1:2*Nckk,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxldTckk); + + Jkk(1:2*Nckk,1:2) = sparse(dxldfl); + Jkk(1:2*Nckk,3:4) = sparse(dxldcl); + Jkk(1:2*Nckk,5) = sparse(dxldalphal); + Jkk(1:2*Nckk,6:10) = sparse(dxldkl); + + + % Project the structure onto the right view: + + [omr,Tr,domrdomckk,domrdTckk,domrdom,domrdT,dTrdomckk,dTrdTckk,dTrdom,dTrdT] = compose_motion(omckk,Tckk,om,T); + + if ~est_aspect_ratio_right, + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right(1),cc_right,kc_right,alpha_c_right); + dxrdfr = repmat(dxrdfr,[1 2]); + else + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right,cc_right,kc_right,alpha_c_right); + end; + + + ekk(2*Nckk+1:end) = xrkk(:) - xr(:); + + + dxrdom = dxrdomr * domrdom + dxrdTr * dTrdom; + dxrdT = dxrdomr * domrdT + dxrdTr * dTrdT; + + dxrdomckk = dxrdomr * domrdomckk + dxrdTr * dTrdomckk; + dxrdTckk = dxrdomr * domrdTckk + dxrdTr * dTrdTckk; + + + Jkk(2*Nckk+1:end,1+20:3+20) = sparse(dxrdom); + Jkk(2*Nckk+1:end,4+20:6+20) = sparse(dxrdT); + + + Jkk(2*Nckk+1:end,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxrdomckk); + Jkk(2*Nckk+1:end,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxrdTckk); + + Jkk(2*Nckk+1:end,11:12) = sparse(dxrdfr); + Jkk(2*Nckk+1:end,13:14) = sparse(dxrdcr); + Jkk(2*Nckk+1:end,15) = sparse(dxrdalphar); + Jkk(2*Nckk+1:end,16:20) = sparse(dxrdkr); + + + emax = max(abs(ekk)); + + if emax < threshold, + + J = [J;Jkk]; + e = [e;ekk]; + + else + + fprintf(1,'Disabling view %d - Reason: the left and right images are found inconsistent (try help calib_stereo for more information)\n',kk); + + active_images(kk) = 0; + + end; + + else + + param = [param;NaN*ones(6,1)]; + + end; + + end; + + history = [history param]; + + ind_Jac = find([est_fc_left & [1;est_aspect_ratio_left];center_optim_left*ones(2,1);est_alpha_left;est_dist_left;est_fc_right & [1;est_aspect_ratio_right];center_optim_right*ones(2,1);est_alpha_right;est_dist_right;ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]); + + ind_active = find(active_images); + + J = J(:,ind_Jac); + J2 = J'*J; + J2_inv = inv(J2); + + param_update = J2_inv*J'*e; + + + param(ind_Jac) = param(ind_Jac) + param_update; + + fc_left = param(1:2); + cc_left = param(3:4); + alpha_c_left = param(5); + kc_left = param(6:10); + fc_right = param(11:12); + cc_right = param(13:14); + alpha_c_right = param(15); + kc_right = param(16:20); + + + if ~est_aspect_ratio_left, + fc_left(2) = fc_left(1); + end; + if ~est_aspect_ratio_right, + fc_right(2) = fc_right(1); + end; + + + om = param(1+20:3+20); + T = param(4+20:6+20); + + + for kk = 1:n_ima; + + if active_images(kk), + + omckk = param(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_' num2str(kk) ' = omckk;']); + eval(['Tc_left_' num2str(kk) ' = Tckk;']); + + end; + + end; + + +end; + +fprintf(1,'done\n'); + + +history = [history param]; + +inconsistent_images = ~active_images & (active_images_left & active_images_right); + + +sigma_x = std(e(:)); +param_error = zeros(20 + (1+n_ima)*6,1); +param_error(ind_Jac) = 3*sqrt(full(diag(J2_inv)))*sigma_x; + +for kk = 1:n_ima; + + if active_images(kk), + + omckk_error = param_error(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param_error(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_error_' num2str(kk) ' = omckk;']); + eval(['Tc_left_error_' num2str(kk) ' = Tckk;']); + + else + + eval(['omc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['omc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + + end; + +end; + +fc_left_error = param_error(1:2); +cc_left_error = param_error(3:4); +alpha_c_left_error = param_error(5); +kc_left_error = param_error(6:10); +fc_right_error = param_error(11:12); +cc_right_error = param_error(13:14); +alpha_c_right_error = param_error(15); +kc_right_error = param_error(16:20); + +if ~est_aspect_ratio_left, + fc_left_error(2) = fc_left_error(1); +end; +if ~est_aspect_ratio_right, + fc_right_error(2) = fc_right_error(1); +end; + + +om_error = param_error(1+20:3+20); +T_error = param_error(4+20:6+20); + + +KK_left = [fc_left(1) fc_left(1)*alpha_c_left cc_left(1);0 fc_left(2) cc_left(2); 0 0 1]; +KK_right = [fc_right(1) fc_right(1)*alpha_c_right cc_right(1);0 fc_right(2) cc_right(2); 0 0 1]; + + +R = rodrigues(om); + + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); + + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') + + +fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat\n\n'); + +string_save = 'save Calib_Results_stereo om R T recompute_intrinsic_right recompute_intrinsic_left calib_name_left format_image_left type_numbering_left image_numbers_left N_slots_left calib_name_right format_image_right type_numbering_right image_numbers_right N_slots_right fc_left cc_left kc_left alpha_c_left KK_left fc_right cc_right kc_right alpha_c_right KK_right active_images dX dY nx ny n_ima active_images_right active_images_left inconsistent_images center_optim_left est_alpha_left est_dist_left est_fc_left est_aspect_ratio_left center_optim_right est_alpha_right est_dist_right est_fc_right est_aspect_ratio_right history param param_error sigma_x om_error T_error fc_left_error cc_left_error kc_left_error alpha_c_left_error fc_right_error cc_right_error kc_right_error alpha_c_right_error'; + +for kk = 1:n_ima, + if active_images(kk), + string_save = [string_save ' X_left_' num2str(kk) ' omc_left_' num2str(kk) ' Tc_left_' num2str(kk) ' omc_left_error_' num2str(kk) ' Tc_left_error_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk)]; + end; +end; +eval(string_save); + + +% Plot the extrinsic parameters: +ext_calib_stereo; + diff --git a/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.eps b/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.eps new file mode 100755 index 0000000..bcdd8af --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.eps Binary files differ diff --git a/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.pdf b/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.pdf new file mode 100755 index 0000000..d310f3e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/calibration_pattern/pattern.pdf @@ -0,0 +1 @@ +%PDF-1.2 1 0 obj << /Creator (Adobe Illustrator_TM_ 7.0) /Producer (Acrobat PDF File Format 1.1 for Macintosh) >> endobj 2 0 obj << /Pages 4 0 R /Type /Catalog /Outlines 7 0 R >> endobj 3 0 obj << /Type /Page /Parent 4 0 R /Resources << /ProcSet [ /PDF /Text ] >> /Contents 5 0 R >> endobj 4 0 obj << /Kids [ 3 0 R ] /Count 1 /Type /Pages /MediaBox [ 0 0 612 792 ] >> endobj 5 0 obj << /Length 6 0 R >> stream endstream endobj 6 0 obj 0 endobj 7 0 obj << /Count 0 /Type /Outlines >> endobj xref 0 8 0000000000 65535 f 0000000009 00000 n 0000000123 00000 n 0000000193 00000 n 0000000303 00000 n 0000000394 00000 n 0000000448 00000 n 0000000466 00000 n trailer << /Size 8 /Info 1 0 R /Root 2 0 R >> startxref 516 %%EOF 8 0 obj << /Length 9 0 R >> stream 1 0 0 1 0 -1 cm BX /Layer << /Title (Layer 1) /Visible true /Preview true /Editable true /Printed true /Dimmed false /Color [20224 32768 65535 ] >> BDC 0 0 0 1 k 0 0 0 1 K 0 w [ ] 0 d 0 J 0 j 4 M 0 i %%Note: 93.5508 269.7693 m 8.5254 269.7693 l 8.5254 184.7419 l 93.5508 184.7419 l 93.5508 269.7693 l b 93.5508 439.8240 m 8.5254 439.8240 l 8.5254 354.7967 l 93.5508 354.7967 l 93.5508 439.8240 l b 178.5585 184.7508 m 93.5332 184.7508 l 93.5332 99.7235 l 178.5585 99.7235 l 178.5585 184.7508 l b 263.6016 269.7959 m 178.5762 269.7959 l 178.5762 184.7685 l 263.6016 184.7685 l 263.6016 269.7959 l b 178.5940 354.7701 m 93.5685 354.7701 l 93.5685 269.7427 l 178.5940 269.7427 l 178.5940 354.7701 l b 263.6016 439.8507 m 178.5762 439.8507 l 178.5762 354.8232 l 263.6016 354.8232 l 263.6016 439.8507 l b 93.5863 609.9053 m 8.5608 609.9053 l 8.5608 524.8779 l 93.5863 524.8779 l 93.5863 609.9053 l b 93.5863 779.9600 m 8.5608 779.9600 l 8.5608 694.9327 l 93.5863 694.9327 l 93.5863 779.9600 l b 178.5940 524.8868 m 93.5685 524.8868 l 93.5685 439.8594 l 178.5940 439.8594 l 178.5940 524.8868 l b 263.6371 609.9319 m 178.6116 609.9319 l 178.6116 524.9045 l 263.6371 524.9045 l 263.6371 609.9319 l b 178.6293 694.9062 m 93.6040 694.9062 l 93.6040 609.8788 l 178.6293 609.8788 l 178.6293 694.9062 l b 263.6371 779.9866 m 178.6116 779.9866 l 178.6116 694.9592 l 263.6371 694.9592 l 263.6371 779.9866 l b 348.6624 184.7375 m 263.6371 184.7375 l 263.6371 99.7102 l 348.6624 99.7102 l 348.6624 184.7375 l b 433.7055 269.7826 m 348.6802 269.7826 l 348.6802 184.7552 l 433.7055 184.7552 l 433.7055 269.7826 l b 348.6447 354.8100 m 263.6193 354.8100 l 263.6193 269.7826 l 348.6447 269.7826 l 348.6447 354.8100 l b 433.7055 439.8374 m 348.6802 439.8374 l 348.6802 354.8100 l 433.7055 354.8100 l 433.7055 439.8374 l b 518.7132 184.7641 m 433.6879 184.7641 l 433.6879 99.7368 l 518.7132 99.7368 l 518.7132 184.7641 l b 603.7563 269.8092 m 518.7310 269.8092 l 518.7310 184.7818 l 603.7563 184.7818 l 603.7563 269.8092 l b 518.7487 354.7834 m 433.7232 354.7834 l 433.7232 269.7560 l 518.7487 269.7560 l 518.7487 354.7834 l b 603.7563 439.8639 m 518.7310 439.8639 l 518.7310 354.8365 l 603.7563 354.8365 l 603.7563 439.8639 l b 348.6979 524.8735 m 263.6725 524.8735 l 263.6725 439.8462 l 348.6979 439.8462 l 348.6979 524.8735 l b 433.7410 609.9185 m 348.7156 609.9185 l 348.7156 524.8913 l 433.7410 524.8913 l 433.7410 609.9185 l b 348.6802 694.9460 m 263.6548 694.9460 l 263.6548 609.9185 l 348.6802 609.9185 l 348.6802 694.9460 l b 433.7410 779.9733 m 348.7156 779.9733 l 348.7156 694.9460 l 433.7410 694.9460 l 433.7410 779.9733 l b 518.7487 524.9001 m 433.7232 524.9001 l 433.7232 439.8727 l 518.7487 439.8727 l 518.7487 524.9001 l b 603.7918 609.9452 m 518.7664 609.9452 l 518.7664 524.9178 l 603.7918 524.9178 l 603.7918 609.9452 l b 518.7841 694.9194 m 433.7587 694.9194 l 433.7587 609.8920 l 518.7841 609.8920 l 518.7841 694.9194 l b 603.7918 779.9999 m 518.7664 779.9999 l 518.7664 694.9725 l 603.7918 694.9725 l 603.7918 779.9999 l b 93.5608 99.6839 m 8.5355 99.6839 l 8.5355 14.6566 l 93.5608 14.6566 l 93.5608 99.6839 l b 263.6116 99.7106 m 178.5862 99.7106 l 178.5862 14.6832 l 263.6116 14.6832 l 263.6116 99.7106 l b 433.7155 99.6972 m 348.6902 99.6972 l 348.6902 14.6699 l 433.7155 14.6699 l 433.7155 99.6972 l b 603.7663 99.7239 m 518.7410 99.7239 l 518.7410 14.6965 l 603.7663 14.6965 l 603.7663 99.7239 l b EMC EX endstream endobj 9 0 obj 3422 endobj 3 0 obj << /Type /Page /Parent 4 0 R /Contents 8 0 R /Resources << /ProcSet [ /PDF ] >> /Rotate 0 /MediaBox [ 0 0 612 792 ] /CropBox [ 0 0 612 792 ] /PieceInfo <> >> >> endobj 1 0 obj << /CreationDate (D:19990908151621) /ModDate (D:19990908151621) /Creator (Adobe Illustrator 7.0) /Producer (Acrobat PDF File Format 1.1 for Windows) /Author (Unknown) >> endobj xref 0 1 0000000000 65535 f 1 1 0000004478 00000 n 3 1 0000004250 00000 n 8 2 0000000754 00000 n 0000004229 00000 n trailer << /Size 10 /Prev 516 /Root 2 0 R /Info 1 0 R >> startxref 4663 %%EOF \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/cam_proj_calib.m b/mr/Ub5/TOOLBOX_calib/cam_proj_calib.m new file mode 100755 index 0000000..8c34c77 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cam_proj_calib.m @@ -0,0 +1,191 @@ +%%% This code is an additional code that helps doing projector calibration in 3D scanning setup. +%%% This is not a useful code for anyone else but me. +%%% I included it in the toolbox for illustration only. + + +fprintf(1,'3D scanner calibration code\n'); +fprintf(1,'(c) Jean-Yves Bouguet - August 2000\n'); +fprintf(1,'Intel Corporation\n'); + + +if ~exist('camera_results.mat'), + if exist('Calib_Results.mat'), + copyfile('Calib_Results.mat','camera_results.mat'); + delete('Calib_Results.mat'); + else + disp('ERROR: Need to calibrate the camera first, save results, and run cam_proj_calib'); + break; + end; +end; + + + +if 0, % If I want to run camera calibration again + load camera_results; + % Do estimate distortion: + est_dist = [1 0 0 0 0]; %ones(5,1); + est_alpha = 0; + center_optim = 1; + % Run the main calibration routine: + go_calib_optim; + saving_calib; + copyfile('Calib_Results.mat','camera_results.mat'); + delete('Calib_Results.mat'); +end; + + + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% START THE MAIN PROCEDURE %%%%%%%%%%%%%%%%%%%%%%%%%%% + +load camera_results; +param = solution; + +% Save camera parameters: +fc_save = fc; +cc_save = cc; +kc_save = kc; +alpha_c_save = alpha_c; + +omc_1_save = omc_1; +Rc_1_save = Rc_1; +Tc_1_save = Tc_1; + +clear fc cc kc alpha_c + + +param_cam = param([1:10 16:end]); + + +% Extract projector data? +if ~exist('projector_data.mat'), + projector_calib; % extract the projector corners (all the data) +else + load projector_data; % load the projector corners (previously saved) +end; + + + +% Start projector calibration: + +X_proj = []; +x_proj = []; +n_ima_proj = []; + +for kk = ind_active, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc_save,cc_save,kc_save,alpha_c_save); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); + n_ima_proj = [n_ima_proj kk*ones(1,Np_proj)]; +end; + +% Image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +n_ima_save = n_ima; +X_1_save = X_1; +x_1_save = x_1; +dX_save = dX; +dY_save = dY; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; + +% Set the toolbox not to prompt the user (choose default values) +dont_ask = 1; + +% Do estimate distortion: +est_dist = [1 0 0 0 0]'; %ones(5,1); +est_alpha = 0; +center_optim = 1; + + +% Run the main calibration routine: +clear fc kc cc alpha_c KK +go_calib_optim; + +param = solution; + +param_proj = param([1:10 16:end]); + +% Shows the extrinsic parameters: +dX = 30; +dY = 30; +ext_calib; + +% Reprojection on the original images: +reproject_calib; +%saving_calib; +%copyfile([save_name '.mat'],'projector_results.mat'); + + +saving_calib; + +copyfile('Calib_Results.mat','projector_results.mat'); +delete('Calib_Results.mat'); + + +n_ima = n_ima_save; +X_1 = X_1_save; +x_1 = x_1_save; +no_image = 0; +dX = dX_save; +dY = dY_save; + +%----------------------- Retrieve results: + +% Intrinsic: + +% Projector: +fp = fc; +cp = cc; +kp = kc; +alpha_p = alpha_c; + +% Camera: +fc = fc_save; +cc = cc_save; +kc = kc_save; +alpha_c = alpha_c_save; + + +% Extrinsic: + +% Relative position of projector and camera: +T = Tc_1; +om = omc_1; +R = rodrigues(om); + +% Relative prosition of camera wrt world: +omc = omc_1_save; +Rc = Rc_1_save; +Tc = Tc_1_save; + +% relative position of projector wrt world: +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + +eval(['save calib_cam_proj R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp']); + + + +% Final refinement: + +%----------------- global optimization: --------------------- + +cam_proj_calib_optim; + diff --git a/mr/Ub5/TOOLBOX_calib/cam_proj_calib_optim.m b/mr/Ub5/TOOLBOX_calib/cam_proj_calib_optim.m new file mode 100755 index 0000000..729f1d5 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cam_proj_calib_optim.m @@ -0,0 +1,122 @@ +load camera_results; + +string_global = 'global n_ima'; +for kk = 1:n_ima, + string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_global); + + +%----------------- global optimization: --------------------- + +load projector_data; % load the projector corners (previously saved) +load projector_results; + +solution_projector = solution; + + +load camera_results; + +solution_camera = solution; + +param_cam = solution_camera([1:10 16:end]); +param_proj = solution_projector([1:10 16:end]); + +param = [param_cam;param_proj]; + + +% Restart the minimization from here (if need be): +load camera_results; +load calib_cam_proj_optim2; + + +options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; + +%if 0, % use the full distortion model: + +% fprintf(1,'Take the complete distortion model\n'); + + % test the global error function: +% e_global = error_cam_proj(param); + +% param_init = param; + +% param = leastsq('error_cam_proj',param,options); + + +%else + + % Use a limitd distortion model (no 6th order) + fprintf(1,'Take the 6th order distortion coefficient out\n'); + + param = param([1:9 11:11+6*n_ima-1 11+6*n_ima:11+6*n_ima+9-1 11+6*n_ima+9+1:end]); + + % test the global error function: + e_global2 = error_cam_proj2(param); + + param_init = param; + + param = leastsq('error_cam_proj2',param,options); + + param = [param(1:9);0;param(10:10+6*n_ima-1);param(10+6*n_ima:10+6*n_ima+9-1);0;param(10+6*n_ima+9:end)]; + +%end; + + + + +% Extract the parameters: + +cam_proj_extract_param; + + +% Relative prosition of camera wrt world: +omc = omc_1; +Rc = Rc_1; +Tc = Tc_1; + +% relative position of projector wrt world: +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + +eval(['save calib_cam_proj_optim3 R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp param param_init']); + +no_image = 0; +% Image size: (may or may not be available) +nx = 640; +ny = 480; + +comp_error_calib; + +% Save the optimal camera parameters: +saving_calib; +copyfile('Calib_Results.mat','camera_results_optim3.mat'); +delete('Calib_Results.mat'); + +% Save the optimal camera parameters: +fc = fp; +cc = cp; +alpha_c = alpha_p; +kc = kp; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; +omc_1 = om; +Tc_1 = T; +Rc_1 = R; + +% Image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','projector_results_optim3.mat'); +delete('Calib_Results.mat'); + diff --git a/mr/Ub5/TOOLBOX_calib/cam_proj_extract_param.m b/mr/Ub5/TOOLBOX_calib/cam_proj_extract_param.m new file mode 100755 index 0000000..9675c37 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cam_proj_extract_param.m @@ -0,0 +1,58 @@ + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = param(6:10); + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); + Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + + +fp = param((1:2)+n_ima * 6 + 10); +cp = param((3:4)+n_ima * 6 + 10); +alpha_p = param((5)+n_ima * 6 + 10); +kp = param((6:10)+n_ima * 6 + 10); + +om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); +T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); +R = rodrigues(om); + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + +e_global = [e_cam e_proj]; + diff --git a/mr/Ub5/TOOLBOX_calib/centercirclefinder.m b/mr/Ub5/TOOLBOX_calib/centercirclefinder.m new file mode 100755 index 0000000..62ff492 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/centercirclefinder.m @@ -0,0 +1,173 @@ +function [xc,good,bad] = centercirclefinder(xt,I,wintx,winty); + + +%xt = [521.6249 ; 469.9916]; +%wintx = 60; +%winty = 60; + + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +mask = ones(2*wintx + 1,2*winty + 1); +%mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.0001; + +MaxIter = 15; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type_feat = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gyy .* px - gxy .* py)); sum(sum(-gxy .* px + gxx .* py))]; + + a = sum(sum(gyy)); + b = -sum(sum(gxy)); + c = sum(sum(gxx)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/mr/Ub5/TOOLBOX_calib/check_active_images.m b/mr/Ub5/TOOLBOX_calib/check_active_images.m new file mode 100755 index 0000000..87c866e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/check_active_images.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/check_convergence.m b/mr/Ub5/TOOLBOX_calib/check_convergence.m new file mode 100755 index 0000000..c4b13fd --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/check_convergence.m @@ -0,0 +1,48 @@ +%%% Replay the set of solution vectors: + + +if ~exist('param_list'), + if ~exist('solution'); + fprintf(1,'Error: Need to calibrate first\n'); + return; + else + param_list = solution; + end; +end; + +N_iter = size(param_list,2); + +if N_iter == 1, + fprintf(1,'Warning: There is a unique state in the list of parameters.\n'); +end; + + + +%M = moviein(N_iter); + +for nn = 1:N_iter, + + solution = param_list(:,nn); + + extract_parameters; + comp_error_calib; + + ext_calib; + + drawnow; + +% Mnn = getframe(gcf); + +% M(:,nn) = Mnn; + +end; + +%fig = gcf; + + +%figure(fig+1); +%close; +%figure(fig+1); + +%clf; +%movie(M,20); diff --git a/mr/Ub5/TOOLBOX_calib/check_directory.m b/mr/Ub5/TOOLBOX_calib/check_directory.m new file mode 100755 index 0000000..9a94163 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/check_directory.m @@ -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; + diff --git a/mr/Ub5/TOOLBOX_calib/check_extracted_images.m b/mr/Ub5/TOOLBOX_calib/check_extracted_images.m new file mode 100755 index 0000000..fa7df87 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/check_extracted_images.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/clear_windows.m b/mr/Ub5/TOOLBOX_calib/clear_windows.m new file mode 100755 index 0000000..1eccbd3 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/clear_windows.m @@ -0,0 +1,4 @@ +for kk = 1:n_ima, + eval(['clear wintx_' num2str(kk)]); + eval(['clear winty_' num2str(kk)]); +end; diff --git a/mr/Ub5/TOOLBOX_calib/clearwin.m b/mr/Ub5/TOOLBOX_calib/clearwin.m new file mode 100755 index 0000000..fccc8c4 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/clearwin.m @@ -0,0 +1,14 @@ +% Function that clears all the wintx_i and winty_i +% In normal operation of the toolbox, this function should not be +% useful. +% only in cases where you want to re-extract corners using the Extract grid corners another time... not common. You might as well use the Recomp. corners. + +if exist('n_ima','var')~=1 + return; +end; + +for kk = 1:n_ima, + + eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/click_calib.m b/mr/Ub5/TOOLBOX_calib/click_calib.m new file mode 100755 index 0000000..41d011f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_calib.m @@ -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; + diff --git a/mr/Ub5/TOOLBOX_calib/click_calib_fisheye_no_read.m b/mr/Ub5/TOOLBOX_calib/click_calib_fisheye_no_read.m new file mode 100755 index 0000000..c773dec --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_calib_fisheye_no_read.m @@ -0,0 +1,260 @@ +%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_no_read; +end; + +check_active_images; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + + + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +%disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + +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 of the Rosette (Google) - use units in mm to match Zhang + dX_default = 0.1524; %100; + dY_default = 0.1524; %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'), + if ~exist('nx'), + wintx_default = 20; + winty_default = wintx_default; + clear wintx winty + else + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty + end; +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 isempty(kk_first), kk_first = 1; end; + + +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 ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + fprintf(1,'\nProcessing image %d...\n',kk); + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + Wcal = nx; % to avoid errors later + Hcal = ny; % to avoid errors later + + click_ima_calib_fisheye_no_read; + + 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'; + +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'); + diff --git a/mr/Ub5/TOOLBOX_calib/click_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/click_calib_no_read.m new file mode 100755 index 0000000..aa45f03 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_calib_no_read.m @@ -0,0 +1,257 @@ +%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_no_read; +end; + +check_active_images; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if (exist('map')~=1), map = gray(256); end; + + +%disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + +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 = 0.1524; %100; + dY_default = 0.1524; %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'), + if ~exist('nx'), + wintx_default = 30; + winty_default = wintx_default; + clear wintx winty + else + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; + clear wintx winty + end; +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 isempty(kk_first), kk_first = 1; end; + + +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 ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + fprintf(1,'\nProcessing image %d...\n',kk); + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + Wcal = nx; % to avoid errors later + Hcal = ny; % to avoid errors later + + click_ima_calib_no_read; + + 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'; + +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'); + diff --git a/mr/Ub5/TOOLBOX_calib/click_ima_calib.m b/mr/Ub5/TOOLBOX_calib/click_ima_calib.m new file mode 100755 index 0000000..adb9b2a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_ima_calib.m @@ -0,0 +1,312 @@ +% Cleaned-up version of init_calib.m + +fprintf(1,'\nProcessing image %d...\n',kk); + +eval(['I = I_' num2str(kk) ';']); + +if exist(['wintx_' num2str(kk)]), + + eval(['wintxkk = wintx_' num2str(kk) ';']); + + if ~isempty(wintxkk) & ~isnan(wintxkk), + + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + + end; +end; + + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); +%fprintf(1,'Note: To reset the window size, clear wintx and winty and run ''Extract grid corners'' again\n'); + + +figure(2); +image(I); +colormap(map); +set(2,'color',[1 1 1]); + +title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + +disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + +x= [];y = []; +figure(2); hold on; +for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; +end; +plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); +drawnow; +hold off; + + +%[x,y] = ginput4(4); + +[Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + +x = Xc(1,:)'; +y = Xc(2,:)'; + + +% Sort the corners: +x_mean = mean(x); +y_mean = mean(y); +x_v = x - x_mean; +y_v = y - y_mean; + +theta = atan2(-y_v,x_v); +[junk,ind] = sort(theta); + +[junk,ind] = sort(mod(theta-theta(1),2*pi)); + +%ind = ind([2 3 4 1]); + +ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + +x = x(ind); +y = y(ind); +x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); +y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + +% Find center: +p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); +x5 = p_center(1)/p_center(3); +y5 = p_center(2)/p_center(3); + +% center on the X axis: +x6 = (x3 + x4)/2; +y6 = (y3 + y4)/2; + +% center on the Y axis: +x7 = (x1 + x4)/2; +y7 = (y1 + y4)/2; + +% Direction of displacement for the X axis: +vX = [x6-x5;y6-y5]; +vX = vX / norm(vX); + +% Direction of displacement for the X axis: +vY = [x7-x5;y7-y5]; +vY = vY / norm(vY); + +% Direction of diagonal: +vO = [x4 - x5; y4 - y5]; +vO = vO / norm(vO); + +delta = 30; + + +figure(2); +image(I); +colormap(map); +hold on; +plot([x;x(1)],[y;y(1)],'g-'); +plot(x,y,'og'); +hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); +set(hx,'color','g','Fontsize',14); +hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); +set(hy,'color','g','Fontsize',14); +hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); +hold off; + + +if manual_squares, + + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + +else + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + +end; + + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + + +if (exist('dX')~=1)|(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + +else + + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'mm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'mm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); + %fprintf(1,'Note: To reset the size of the squares, clear the variables dX and dY\n'); + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrix as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + + +%%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% +figure(2); +hold on; +plot(XX(1,:),XX(2,:),'r+'); +title('The red crosses should be close to the image corners'); +hold off; + +disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); +disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); +quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + +quest_distort = ~isempty(quest_distort); + +if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; +end; +%%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + + + + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + + +%save all_corners x y grid_pts + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + +figure(3); +image(I); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + + +% Saves all the data into variables: + +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); + +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); + +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/mr/Ub5/TOOLBOX_calib/click_ima_calib3D.m b/mr/Ub5/TOOLBOX_calib/click_ima_calib3D.m new file mode 100755 index 0000000..0109cca --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_ima_calib3D.m @@ -0,0 +1,482 @@ + % Cleaned-up version of init_calib.m + + eval(['I = I_' num2str(kk) ';']); + + figure(2); + image(I); + colormap(map); + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% LEFT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + + title(['Click on the four extreme corners of the left rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the four extreme corners of the left rectangular pattern...'); + + [x,y] = ginput4(4); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + if (x(2) > x(1)), + x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2); + else + x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1); + end; + if (x(3) > x(4)), + x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4); + else + x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3); + end; + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','g','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','g','Fontsize',14); + hold off; + + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + image(I); colormap(map); hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The left pannel info: + + xl = x; + Xl = X; + nl_sq_x = n_sq_x; + nl_sq_y = n_sq_y; + Hl = H_total; + + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% RIGHT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + x1 = a10(1)/a10(3); + x4 = a11(1)/a11(3); + + y1 = a10(2)/a10(3); + y4 = a11(2)/a11(3); + + + figure(2); + hold on; + plot([x1 x4],[y1 y4],'c-'); + plot([x1 x4],[y1 y4],'co'); + hold off; + + title(['Click on the two remaining extreme corners of the right rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the two remaining extreme corners of the right rectangular pattern...'); + + [x,y] = ginput4(2); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + x2 = x(2); + x3 = x(1); + + y2 = y(2); + y3 = y(1); + + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'c-'); + plot(x,y,'oc'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','c','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','c','Fontsize',14); + hold off; + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The right pannel info: + + xr = x; + Xr = X; + nr_sq_x = n_sq_x; + nr_sq_y = n_sq_y; + Hr = H_total; + + + +%%%%%%%% REGROUP THE LEFT AND RIHT PATTERNS %%%%%%%%%%%%% + + +Xr2 = [0 0 1;0 1 0;-1 0 0]*Xr + [dX*nl_sq_x;0;0]*ones(1,length(Xr)); + + +x = [xl xr]; + +X = [Xl Xr2]; + + + + eval(['x_' num2str(kk) ' = x;']); + eval(['X_' num2str(kk) ' = X;']); + + eval(['nl_sq_x_' num2str(kk) ' = nl_sq_x;']); + eval(['nl_sq_y_' num2str(kk) ' = nl_sq_y;']); + + eval(['nr_sq_x_' num2str(kk) ' = nr_sq_x;']); + eval(['nr_sq_y_' num2str(kk) ' = nr_sq_y;']); + + % Save the global planar homography: + + eval(['Hl_' num2str(kk) ' = Hl;']); + eval(['Hr_' num2str(kk) ' = Hr;']); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/click_ima_calib_fisheye_no_read.m b/mr/Ub5/TOOLBOX_calib/click_ima_calib_fisheye_no_read.m new file mode 100755 index 0000000..ceb51ea --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_ima_calib_fisheye_no_read.m @@ -0,0 +1,308 @@ +% Rough estimates for principal point and focal length: +fg = [1009.661057548567 1009.706970843858 ]'; +cg = [ 1031.688786545889 1288.395050759215]'; +kg = [-0.054285891395760 -0.026812583950935 0 0]'; + +%fg = [ 139.77712 139.61650 ]'; +%cg = [ 319.62550 258.16616 ]'; +%kg = [ 0.02859 -0.01812 0.00839 -0.00182 ]'; + +%fg = [700;700]; %[ 1397.7712 1396.1650 ]'; +%cg = [ 319.62550 258.16616 ]'; +%kg = [ -1 -0.01812 0.00839 -0.00182 ]'; + +if exist(['wintx_' num2str(kk)]), + eval(['wintxkk = wintx_' num2str(kk) ';']); + if ~isempty(wintxkk) & ~isnan(wintxkk), + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + end; +end; + +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration), + wintx = 20; + winty = 20; +end; + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); + +grid_success = 0; + +while (~grid_success) + + figure(2); clf; + image(I); + axis image; + colormap(map); + set(2,'color',[1 1 1]); + title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + x = x(ind); + y = y(ind); + + if (rosette_calibration && (kk < 17)) + % Enforce the ordering convention for the Rosette calibration. + c_ima = [nx/2 + 0.5 ; ny/2 + 0.5]; + + vects = [x'-c_ima(1) ; y'-c_ima(2)]; + r = sqrt(sum(vects.^2)); + + [r_junk,ind_sort_r] = sort(r); + ind_23 = ind_sort_r(1:2); + ind_14 = ind_sort_r(3:4); + + if det(vects(:,ind_23))<0 + ind2 = ind_23(1); + ind3 = ind_23(2); + else + ind2 = ind_23(2); + ind3 = ind_23(1); + end; + if det(vects(:,ind_14))<0 + ind1 = ind_14(1); + ind4 = ind_14(2); + else + ind1 = ind_14(2); + ind4 = ind_14(1); + end; + + ind_resort = [ind1;ind2;ind3;ind4]; + x = x(ind_resort); + y = y(ind_resort); + end; + + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + figure(2); image(I); + axis image; + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + for iii = 1:4, + text(x(iii),y(iii),num2str(iii)); + end; + hold off; + + if manual_squares, + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + % Try to automatically count the number of squares in the grid + if (rosette_calibration) + win_count = 10; + else + win_count = wintx; + end; + n_sq_x1 = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win_count, fg, cg, kg); + n_sq_x2 = count_squares_fisheye_distorted(I,x3,y3,x4,y4,win_count, fg, cg, kg); + n_sq_y1 = count_squares_fisheye_distorted(I,x2,y2,x3,y3,win_count, fg, cg, kg); + n_sq_y2 = count_squares_fisheye_distorted(I,x4,y4,x1,y1,win_count, fg, cg, kg); + %n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + %n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + %n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + %n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + % If could not count the number of squares, enter manually + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + if ~rosette_calibration, + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + grid_success = 0; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + end; + + if ~grid_success + fprintf(1,'Invalid grid. Try again.\n'); + end; + +end; + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + +if (exist('dX')~=1)|(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; +else + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'mm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'mm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); +end; + +x_n = (x - 1 - cg(1))/fg(1); +y_n = (y - 1 - cg(2))/fg(2); + +[x_pn] = comp_fisheye_distortion([x_n' ; y_n'],kg); + +% Compute the inside points through computation of the planar homography (collineation) +a00 = [x_pn(1,1);x_pn(2,1);1]; +a10 = [x_pn(1,2);x_pn(2,2);1]; +a11 = [x_pn(1,3);x_pn(2,3);1]; +a01 = [x_pn(1,4);x_pn(2,4);1]; + +% Compute the planar collineation: (return the normalization matrix as well) +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + +% Build the grid using the planar collineation: +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XXpn = Homo*pts; +XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + +XX = apply_fisheye_distortion(XXpn,kg); + +XX(1,:) = fg(1)*XX(1,:) + cg(1) + 1; +XX(2,:) = fg(2)*XX(2,:) + cg(2) + 1; + +% Complete size of the rectangle +W = n_sq_x*dX; +L = n_sq_y*dY; + +Np = (n_sq_x+1)*(n_sq_y+1); +disp('Corner extraction...'); +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! +%grid_pts = XX; %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + +figure(3); +image(I); axis image; colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + +% All the point coordinates (on the image, and in 3D) - for global optimization: +x = grid_pts; +X = Xgrid; + +% Saves all the data into variables: +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/mr/Ub5/TOOLBOX_calib/click_ima_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/click_ima_calib_no_read.m new file mode 100755 index 0000000..aeba2c5 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_ima_calib_no_read.m @@ -0,0 +1,382 @@ +if ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + % Guesses for the intrinsic parameters. + fg = [ 2300.044453320904 2304.757690230091 ]'; + cg = [924.743157649778 1282.463968890109 ]'; + kg = [-0.443251898487364 0.247922214991804 -0.000682045390384 0.000326391030429 -0.092405396596532]'; +end; + +if exist(['wintx_' num2str(kk)]), + eval(['wintxkk = wintx_' num2str(kk) ';']); + if ~isempty(wintxkk) & ~isnan(wintxkk), + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + end; +end; + +if (rosette_calibration), + wintx = 20; + winty = 20; +end; + +fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d (Note: To reset the window size, run script clearwin)\n',wintx,winty,2*wintx+1,2*winty+1); + +grid_success = 0; + +while (~grid_success) + bad_clicks = 1; + + while bad_clicks, + + figure(2); clf; + image(I); + colormap(map); + axis image; + set(2,'color',[1 1 1]); + title(['Click on the four extreme corners of the rectangular pattern (first corner = origin)... Image ' num2str(kk)]); + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + if (rosette_calibration) + position_fig = get(2,'position'); + position_fig(3:4) = [674 824]; + set(2,'position',position_fig); + findfigs; + end; + + wintx_save = wintx; + winty_save = winty; + + if (rosette_calibration) + wintx = 30; + winty = 30; + end; + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + wintx = wintx_save; + winty = winty_save; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + bad_clicks = (sum(bad)>0); + + end; + + x = Xc(1,:)'; + y = Xc(2,:)'; + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + x = x(ind); + y = y(ind); + + if (rosette_calibration) + % Enforce the ordering convention for the Rosette calibration. + % get the first two points by sorting the x: + [x_junk,ind_sort_x] = sort(x); + first_two_points = ind_sort_x(1:2); + last_two_points = ind_sort_x(3:4); + + [y_junk, ind_sort_y] = sort(y(first_two_points)); + ind1 = first_two_points(ind_sort_y(2)); + ind2 = first_two_points(ind_sort_y(1)); + [y_junk, ind_sort_y] = sort(y(last_two_points)); + ind3 = last_two_points(ind_sort_y(1)); + ind4 = last_two_points(ind_sort_y(2)); + + ind_resort = [ind1;ind2;ind3;ind4]; + x = x(ind_resort); + y = y(ind_resort); + end; + + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + figure(2); image(I); + axis image; + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + for iii = 1:4, + text(x(iii),y(iii),num2str(iii)); + end; + hold off; + + if manual_squares, + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + if (rosette_calibration) + n_sq_x1 = count_squares_distorted(I,x1,y1,x2,y2,wintx, fg, cg, kg); + n_sq_x2 = count_squares_distorted(I,x3,y3,x4,y4,wintx, fg, cg, kg); + n_sq_y1 = count_squares_distorted(I,x2,y2,x3,y3,wintx, fg, cg, kg); + n_sq_y2 = count_squares_distorted(I,x4,y4,x1,y1,wintx, fg, cg, kg); + else + % Try to automatically count the number of squares in the grid + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + end; + + % If could not count the number of squares, enter manually + if ((n_sq_x1~=n_sq_x2)||(n_sq_y1~=n_sq_y2)||... + (min([n_sq_x1 n_sq_x2 n_sq_y1 n_sq_y2] < 0))), + if (rosette_calibration) + % Try harder using an existing intrinsic model: + n_sq_x1 = count_squares_distorted(I,x1,y1,x2,y2,wintx, fg, cg, kg); + n_sq_x2 = count_squares_distorted(I,x3,y3,x4,y4,wintx, fg, cg, kg); + n_sq_y1 = count_squares_distorted(I,x2,y2,x3,y3,wintx, fg, cg, kg); + n_sq_y2 = count_squares_distorted(I,x4,y4,x1,y1,wintx, fg, cg, kg); + end; + if ((n_sq_x1~=n_sq_x2)||(n_sq_y1~=n_sq_y2)||... + (min([n_sq_x1 n_sq_x2 n_sq_y1 n_sq_y2] < 0))), + if ~rosette_calibration, + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 + if isempty(n_sq_x), n_sq_x = n_sq_x_default; end; + n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 + if isempty(n_sq_y), n_sq_y = n_sq_y_default; end; + grid_success = 1; + else + grid_success = 0; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + else + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + grid_success = 1; + end; + end; + if ~grid_success + fprintf(1,'Invalid grid. Try again.\n'); + end; +end; + +n_sq_x_default = n_sq_x; +n_sq_y_default = n_sq_y; + + +if (exist('dX')~=1)||(exist('dY')~=1), % This question is now asked only once + % Enter the size of each square + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; +else + fprintf(1,['Size of each square along the X direction: dX=' num2str(dX) 'm\n']); + fprintf(1,['Size of each square along the Y direction: dY=' num2str(dY) 'm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); +end; + +if (~rosette_calibration) + % Compute the inside points through computation of the planar homography (collineation) + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + % Compute the planar collineation: (return the normalization matrix as well) + [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + % Build the grid using the planar collineation: + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); +else + [x_pn] = normalize_pixel([x' ; y'] - 1,fg,cg,kg,0); + + % Compute the inside points through computation of the planar homography (collineation) + a00 = [x_pn(1,1);x_pn(2,1);1]; + a10 = [x_pn(1,2);x_pn(2,2);1]; + a11 = [x_pn(1,3);x_pn(2,3);1]; + a01 = [x_pn(1,4);x_pn(2,4);1]; + + % Compute the planar collineation: (return the normalization matrix as well) + [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + % Build the grid using the planar collineation: + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XXpn = Homo*pts; + XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + + XX = apply_distortion2(XXpn,kg); + + XX(1,:) = fg(1)*XX(1,:) + cg(1) + 1; + XX(2,:) = fg(2)*XX(2,:) + cg(2) + 1; +end; + +% Complete size of the rectangle +W = n_sq_x*dX; +L = n_sq_y*dY; + +if (~rosette_calibration) +%%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% +figure(2); +hold on; +plot(XX(1,:),XX(2,:),'r+'); +title('The red crosses should be close to the image corners'); +hold off; + +disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); +disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); +quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + +quest_distort = ~isempty(quest_distort); + +if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; +end; +%%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +end; + + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +%save all_corners x y grid_pts + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + +figure(3); +image(I); axis image; colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + + +% Saves all the data into variables: + +eval(['dX_' num2str(kk) ' = dX;']); +eval(['dY_' num2str(kk) ' = dY;']); + +eval(['wintx_' num2str(kk) ' = wintx;']); +eval(['winty_' num2str(kk) ' = winty;']); + +eval(['x_' num2str(kk) ' = x;']); +eval(['X_' num2str(kk) ' = X;']); + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); diff --git a/mr/Ub5/TOOLBOX_calib/click_stereo.m b/mr/Ub5/TOOLBOX_calib/click_stereo.m new file mode 100755 index 0000000..8c4b569 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/click_stereo.m @@ -0,0 +1,53 @@ +function [xL,xR] = click_stereo(NUMBER_OF_POINTS,IL,IR,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); + + +figure(1); +image(IL); + +figure(2); +image(IR); + +[ny,nx] = size(IL); + +xL = []; +xR = []; + +for kk = 1:NUMBER_OF_POINTS, + + figure(1); + hold on; + x = ginput(1); + plot(x(1),x(2),'g.'); + hold off; + x = x'-1; + + xL = [xL x]; + + [epipole] = compute_epipole(x,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); + + figure(2); + hold on; + h = plot(epipole(1,:)+1,epipole(2,:)+1,'r.','markersize',1); + hold off; + + x2 = ginput(1); + x2 = x2' - 1; + + NN = size(epipole,2); + d = sum((epipole - repmat(x2,1,NN)).^2); + [junk,indmin] = min(d); + + x2 = epipole(:,indmin); + + xR = [xR x2]; + + delete(h); + + figure(2); + hold on; + plot(x2(1)+1,x2(2)+1,'g.'); + drawnow; + hold off; + +end; + diff --git a/mr/Ub5/TOOLBOX_calib/combine_calib.m b/mr/Ub5/TOOLBOX_calib/combine_calib.m new file mode 100755 index 0000000..277c11d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/combine_calib.m @@ -0,0 +1,40 @@ +%%% Script that combines two calibration sets together. + + +dir; + +name1 = input('Calibration file name #1: ','s'); +name2 = input('Calibration file name #2: ','s'); + + +load(name1); + +n_ima_1 = n_ima; + + +load(name2); + +n_ima_2= n_ima; +active_images_2 = active_images; + +for kk=n_ima:-1:1, + + eval(['X_' num2str(kk+n_ima_1) '=X_' num2str(kk) ';' ]); + eval(['x_' num2str(kk+n_ima_1) '=x_' num2str(kk) ';' ]); + eval(['dX_' num2str(kk+n_ima_1) '=dX_' num2str(kk) ';' ]); + eval(['dY_' num2str(kk+n_ima_1) '=dY_' num2str(kk) ';' ]); + eval(['n_sq_x_' num2str(kk+n_ima_1) '=n_sq_x_' num2str(kk) ';' ]); + eval(['n_sq_y_' num2str(kk+n_ima_1) '=n_sq_y_' num2str(kk) ';' ]); + eval(['wintx_' num2str(kk+n_ima_1) '=wintx_' num2str(kk) ';' ]); + eval(['winty_' num2str(kk+n_ima_1) '=winty_' num2str(kk) ';' ]); + +end; + +load(name1); + +n_ima = n_ima + n_ima_2; +active_images = [ active_images active_images_2]; + +%no_image = 1; + +clear calib_name format_image type_numbering image_numbers N_slots \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/comp_distortion.m b/mr/Ub5/TOOLBOX_calib/comp_distortion.m new file mode 100755 index 0000000..6e757bc --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_distortion.m @@ -0,0 +1,43 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% (do it up to a second order approximation) + + +[two,N] = size(x_dist); + + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + + +if length(k2) > 1, + + [x_comp] = comp_distortion_oulu(x_dist,k2); + +else + + radius_2= x_dist(1,:).^2 + x_dist(2,:).^2; + radial_distortion = 1 + ones(2,1)*(k2 * radius_2); + radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:); + radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp); + x_comp = x_dist ./ radial_distortion; + +end; + +%% Function completely checked : It works fine !!! \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/comp_distortion2.m b/mr/Ub5/TOOLBOX_calib/comp_distortion2.m new file mode 100755 index 0000000..e2f70ec --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_distortion2.m @@ -0,0 +1,73 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% k2: Radial distortion factor +% +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% Solve for cubic roots using method from Numerical Recipes in C 2nd Ed. +% pages 184-185. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet - April 27th, 1998 + +% fully checked! JYB, april 27th, 1998 - 2am + +if k2 ~= 0, + +[two,N] = size(x_dist); + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + + +ph = atan2(x_dist(2,:),x_dist(1,:)); + +Q = -1/(3*k2); +R = -x_dist(1,:)./(2*k2*cos(ph)); + +R2 = R.^2; +Q3 = Q^3; + + +if k2 < 0, + + % this works in all practical situations (it starts failing for very large + % values of k2) + + th = acos(R./sqrt(Q3)); + r = -2*sqrt(Q)*cos((th-2*pi)/3); + +else + + % note: this always works, even for ridiculous values of k2 + + A = (sqrt(R2-Q3)-R).^(1/3); + B = Q*(1./A); + r = (A+B); + +end; + +x_comp = [r.*cos(ph); r.*sin(ph)]; + +else + + x_comp = x_dist; + +end; + +x_comp = real(x_comp); diff --git a/mr/Ub5/TOOLBOX_calib/comp_distortion_oulu.m b/mr/Ub5/TOOLBOX_calib/comp_distortion_oulu.m new file mode 100755 index 0000000..1002036 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_distortion_oulu.m @@ -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; + + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/comp_error_calib.m b/mr/Ub5/TOOLBOX_calib/comp_error_calib.m new file mode 100755 index 0000000..b88d454 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_error_calib.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/mr/Ub5/TOOLBOX_calib/comp_error_calib_fisheye.m b/mr/Ub5/TOOLBOX_calib/comp_error_calib_fisheye.m new file mode 100755 index 0000000..f0cb4ef --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_error_calib_fisheye.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points_fisheye(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/mr/Ub5/TOOLBOX_calib/comp_ext_calib.m b/mr/Ub5/TOOLBOX_calib/comp_ext_calib.m new file mode 100755 index 0000000..3400f57 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_ext_calib.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/comp_ext_calib_fisheye.m b/mr/Ub5/TOOLBOX_calib/comp_ext_calib_fisheye.m new file mode 100755 index 0000000..6b61c2c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_ext_calib_fisheye.m @@ -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_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(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; diff --git a/mr/Ub5/TOOLBOX_calib/comp_fisheye_distortion.m b/mr/Ub5/TOOLBOX_calib/comp_fisheye_distortion.m new file mode 100755 index 0000000..88439f1 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/comp_fisheye_distortion.m @@ -0,0 +1,49 @@ +function [x] = comp_fisheye_distortion(xd,k) + +%comp_fisheye_distortion.m +% +%[x] = comp_fisheye_distortion(xd,k) +% +%Compensates for fisheye distortions +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Fisheye distortion coefficients (5x1 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. + +theta_d = sqrt(xd(1,:).^2 + xd(2,:).^2); +theta = theta_d; % initial guess +for kk=1:20, + theta = theta_d ./ (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); +end; +scaling = tan(theta) ./ theta_d; + +x = xd .* (ones(2,1)*scaling); + +return; + +% Test + +n = 4; +xxx = rand(2,n); + +xxx = [[1.0840 0.3152 0.2666 0.9347 ];[ 0.7353 0.6101 -0.6415 -0.8006]]; + +k = 0.00 * randn(4,1); + +[xd] = comp_fisheye_distortion(xxx,k); +x2 = apply_fisheye_distortion(xd,k); +norm(x2-xd)/norm(x2-xxx) + + +%[xd] = apply_fisheye_distortion(xxx,k); +%x2 = comp_fisheye_distortion(xd,k); +%norm(x2-xd)/norm(x2-xxx) + + + diff --git a/mr/Ub5/TOOLBOX_calib/compose_motion.m b/mr/Ub5/TOOLBOX_calib/compose_motion.m new file mode 100755 index 0000000..eb19582 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compose_motion.m @@ -0,0 +1,62 @@ +function [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); + +% Rotations: + +[R1,dR1dom1] = rodrigues(om1); +[R2,dR2dom2] = rodrigues(om2); + +R3 = R2 * R1; + +[dR3dR2,dR3dR1] = dAB(R2,R1); + +[om3,dom3dR3] = rodrigues(R3); + +dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; +dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; + +dom3dT1 = zeros(3,3); +dom3dT2 = zeros(3,3); + + +% Translations: + +T3t = R2 * T1; + +[dT3tdR2,dT3tdT1] = dAB(R2,T1); + +dT3tdom2 = dT3tdR2 * dR2dom2; + + +T3 = T3t + T2; + +dT3dT1 = dT3tdT1; +dT3dT2 = eye(3); + +dT3dom2 = dT3tdom2; +dT3dom1 = zeros(3,3); + + +return; + +% Test of the Jacobians: + +om1 = randn(3,1); +om2 = randn(3,1); +T1 = 10*randn(3,1); +T2 = 10*randn(3,1); + +[om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); + + +dom1 = randn(3,1) / 1000; +dom2 = randn(3,1) / 1000; +dT1 = randn(3,1) / 10000; +dT2 = randn(3,1) / 10000; + +[om3r,T3r] = compose_motion(om1+dom1,T1+dT1,om2+dom2,T2+dT2); + +om3p = om3 + dom3dom1*dom1 + dom3dT1*dT1 + dom3dom2*dom2 + dom3dT2*dT2; +T3p = T3 + dT3dom1*dom1 + dT3dT1*dT1 + dT3dom2*dom2 + dT3dT2*dT2; + +norm(om3r - om3) / norm(om3r - om3p) +norm(T3r - T3) / norm(T3r - T3p) diff --git a/mr/Ub5/TOOLBOX_calib/compute_collineation.m b/mr/Ub5/TOOLBOX_calib/compute_collineation.m new file mode 100755 index 0000000..809c309 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_collineation.m @@ -0,0 +1,66 @@ +function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + +% new formalism using homographies + +a00 = a00 / a00(3); +a10 = a10 / a10(3); +a11 = a11 / a11(3); +a01 = a01 / a01(3); + + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = [a00(1);a10(1);a11(1);a01(1)]; +ay = [a00(2);a10(2);a11(2);a01(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]; + + +a00n = Hnorm*a00; +a10n = Hnorm*a10; +a11n = Hnorm*a11; +a01n = Hnorm*a01; + + +% Computation of the vanishing points: + +V1n = cross(cross(a00n,a10n),cross(a01n,a11n)); +V2n = cross(cross(a00n,a01n),cross(a10n,a11n)); + +V1 = inv_Hnorm*V1n; +V2 = inv_Hnorm*V2n; + + +% Normalizaion of the vanishing points: + +V1n = V1n/norm(V1n); +V2n = V2n/norm(V2n); + + +% Closed-form solution of the coefficients: + +alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2)); + +alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2)); + + +% Remaining Homography + +Hrem = [alpha_x*V1n alpha_y*V2n a00n]; + + +% Final homography: + +H = inv_Hnorm*Hrem; + diff --git a/mr/Ub5/TOOLBOX_calib/compute_epipole.m b/mr/Ub5/TOOLBOX_calib/compute_epipole.m new file mode 100755 index 0000000..e013dcd --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_epipole.m @@ -0,0 +1,57 @@ +function [epipole] = compute_epipole(xLp,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left,D); + +if ~exist('D'), + D = 400; +end; + +uo = [ normalize_pixel(xLp,fc_left,cc_left,kc_left,alpha_c_left); 1 ]; + +S = [ 0 -T(3) T(2) + T(3) 0 -T(1) + -T(2) T(1) 0 ]; + +l_epipole = (S*R)*uo; + +KK_right = [fc_right(1) alpha_c_right * fc_right(1) cc_right(1) ; 0 fc_right(2) cc_right(2) ; 0 0 1]; + +N_line = 800; + +if norm(l_epipole(2)) > norm(l_epipole(1)), + + % Horizontal line: + + limit_x_pos = ((xLp(1) + D/2) - cc_right(1)) / fc_right(1); + limit_x_neg = ((xLp(1) - D/2) - cc_right(1)) / fc_right(1); + + + %limit_x_pos = ((nx-1) - cc_right(1)) / fc_right(1); + %limit_x_neg = (0 - cc_right(1)) / fc_right(1); + + x_list = (limit_x_pos - limit_x_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_x_neg; + + + pt = cross(repmat(l_epipole,1,N_line),[ones(1,N_line);zeros(1,N_line);-x_list]); + + +else + + limit_y_pos = ((xLp(2) + D/2) - cc_right(2)) / fc_right(2); + limit_y_neg = ((xLp(2) - D/2) - cc_right(2)) / fc_right(2); + + %limit_y_pos = ((ny-1) - cc_right(2)) / fc_right(2); + %limit_y_neg = (0 - cc_right(2)) / fc_right(2); + + y_list = (limit_y_pos - limit_y_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_y_neg; + + + pt = cross(repmat(l_epipole,1,N_line),[zeros(1,N_line);ones(1,N_line);-y_list]); + + +end; + + +pt = [pt(1,:) ./ pt(3,:) ; pt(2,:)./pt(3,:)]; +ptd = apply_distortion(pt,kc_right); +epipole = KK_right * [ ptd ; ones(1,N_line)]; + +epipole = epipole(1:2,:); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic.m new file mode 100755 index 0000000..b526028 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init.m new file mode 100755 index 0000000..670cf06 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init_fisheye.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init_fisheye.m new file mode 100755 index 0000000..97a9ea3 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_init_fisheye.m @@ -0,0 +1,215 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c) + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init_fisheye(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_fisheye(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; diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine.m new file mode 100755 index 0000000..f000f48 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine.m @@ -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); diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine2.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine2.m new file mode 100755 index 0000000..6be0c49 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine2.m @@ -0,0 +1,119 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine2(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine2(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: '); + + +% Normalize the points: +x_kk_n = normalize_pixel(x_kk,fc,cc,kc,alpha_c); + + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk); + + ex = x_kk_n - 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); diff --git a/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine_fisheye.m b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine_fisheye.m new file mode 100755 index 0000000..cecd686 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_extrinsic_refine_fisheye.m @@ -0,0 +1,114 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine_fisheye(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond) + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine_fisheye(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: Fisheye 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_points_fisheye(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); diff --git a/mr/Ub5/TOOLBOX_calib/compute_homography.m b/mr/Ub5/TOOLBOX_calib/compute_homography.m new file mode 100755 index 0000000..2c34718 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/compute_homography.m @@ -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') diff --git a/mr/Ub5/TOOLBOX_calib/cornerfinder.m b/mr/Ub5/TOOLBOX_calib/cornerfinder.m new file mode 100755 index 0000000..d51e8aa --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cornerfinder.m @@ -0,0 +1,227 @@ +function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +% another mask: +[X,Y] = meshgrid(-winty:winty,-wintx:wintx); +mask2 = X.^2 + Y.^2; +mask2(wintx+1,winty+1) = 1; +mask2 = 1./mask2; +%mask - mask2; + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + end; + + end; + + + %keyboard; + + % G = [a b;b c]; + % [U,S,V] = svd(G); + + + % if S(1,1)/S(2,2) > 150, + % bb2 = U'*bb; + % xc2 = (V*[bb2(1)/S(1,1) ;0])'; + % else + % xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + % end; + + + %if (abs(a)> 50*abs(c)), + % xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; + % elseif (abs(c)> 50*abs(a)) + % xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; + % else + % xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + % end; + + %keyboard; + + if (isnan(xc2(1)) || isnan(xc2(2))), + xc2 = [0 0]; + end; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + + % keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/mr/Ub5/TOOLBOX_calib/cornerfinder2.m b/mr/Ub5/TOOLBOX_calib/cornerfinder2.m new file mode 100755 index 0000000..3ad364d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cornerfinder2.m @@ -0,0 +1,223 @@ +function [xc,good,bad,type] = cornerfinder2(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +% another mask: +[X,Y] = meshgrid(-winty:winty,-wintx:wintx); +mask2 = X.^2 + Y.^2; +mask2(wintx+1,winty+1) = 1; +mask2 = 1./mask2; +%mask - mask2; + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + end; + + end; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/mr/Ub5/TOOLBOX_calib/cornerfinder_saddle_point.m b/mr/Ub5/TOOLBOX_calib/cornerfinder_saddle_point.m new file mode 100755 index 0000000..d1d4444 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/cornerfinder_saddle_point.m @@ -0,0 +1,235 @@ +function [xc,good,bad,type] = cornerfinder_saddle_point(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder_saddle_point(xt,I,wintx,winty); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + + + px = cIx + offx; + py = cIy + offy; + + + if 1, %~saddle, + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + else + + SI = SI(2:2*wintx+2,2:2*winty+2); + A = repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)]; + param = inv(A'*A)*A'*( mask(:).*SI(:)); + xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))'; + + end; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + + + compt = compt + 1; + + end; + + + + if 1, + + cIx = xc(i,1); % + cIy = xc(i,2); % Coords. of the point + crIx = round(cIx); % on the initial image + crIy = round(cIy); % + itIx = cIx - crIx; % Coefficients + itIy = cIy - crIy; % to compute + if itIx > 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + px = cIx + offx; + py = cIy + offy; + + SI = SI(2:2*wintx+2,2:2*winty+2); + A = repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)]; + param = inv(A'*A)*A'*( mask(:).*SI(:)); + xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))'; + + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + end; + + + +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/mr/Ub5/TOOLBOX_calib/count_squares.m b/mr/Ub5/TOOLBOX_calib/count_squares.m new file mode 100755 index 0000000..54c89ee --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/count_squares.m @@ -0,0 +1,57 @@ +function ns = count_squares(I,x1,y1,x2,y2,win); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1]; +lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda; +l1 = lambda + [0;0;win]; +l2 = lambda - [0;0;win]; +dx = x2-x1; +dy = y2 - y1; + +if abs(dx) > abs(dy), + if x2 > x1, + xs = x1:x2; + else + xs = x1:-1:x2; + end; + ys = -(lambda(3) + lambda(1)*xs)/lambda(2); +else + if y2 > y1, + ys = y1:y2; + else + ys = y1:-1:y2; + end; + xs = -(lambda(3) + lambda(2)*ys)/lambda(1); +end; + +Np = length(xs); +xs_mat = ones(2*win + 1,1)*xs; +ys_mat = ones(2*win + 1,1)*ys; +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); +ind_mat = (xs_mat2 - 1) * ny + ys_mat2; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +%ima2 = ima_patch(:,win+1:end-win); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; + +return; diff --git a/mr/Ub5/TOOLBOX_calib/count_squares_distorted.m b/mr/Ub5/TOOLBOX_calib/count_squares_distorted.m new file mode 100755 index 0000000..6d8e2fe --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/count_squares_distorted.m @@ -0,0 +1,59 @@ +function ns = count_squares_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); +alpha_x = (0:nX)/nX; +pt1n = normalize_pixel([x1;y1]-1,fg,cg,kg,0); +pt2n = normalize_pixel([x2;y2]-1,fg,cg,kg,0); + +ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; + +pts = apply_distortion2(ptsn,kg); +pts(1,:) = fg(1)*pts(1,:) + cg(1); +pts(2,:) = fg(2)*pts(2,:) + cg(2); + +% Check that the curve is within the image: +good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... + (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); + +if ~good_line, + ns = -1; + return; +end; + +% Deviate the trajectory orthogonally: +lambda = [y1 - y2 ; x2 - x1]; +lambda = lambda / sqrt(sum(lambda.^2)); + +Np = size(pts,2); +xs_mat = ones(2*win + 1,1)*pts(1,:); +ys_mat = ones(2*win + 1,1)*pts(2,:); +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); + + +ind_mat = (xs_mat2) * ny + ys_mat2 + 1; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; + + diff --git a/mr/Ub5/TOOLBOX_calib/count_squares_fisheye_distorted.m b/mr/Ub5/TOOLBOX_calib/count_squares_fisheye_distorted.m new file mode 100755 index 0000000..aeb750b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/count_squares_fisheye_distorted.m @@ -0,0 +1,56 @@ +function ns = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); + +[ny,nx] = size(I); + +if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... + (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) + ns = -1; + return; +end; + + +if ((x1 - x2)^2+(y1-y2)^2) < win, + ns = -1; + return; +end; + +nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); +alpha_x = (0:nX)/nX; + +pt1n = normalize_pixel_fisheye([x1;y1]-1,fg,cg,kg,0); +pt2n = normalize_pixel_fisheye([x2;y2]-1,fg,cg,kg,0); + +ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; + +pts = apply_fisheye_distortion(ptsn,kg); +pts(1,:) = fg(1)*pts(1,:) + cg(1); +pts(2,:) = fg(2)*pts(2,:) + cg(2); + +% Check that the curve is within the image: +good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... + (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); + +if ~good_line, + ns = -1; + return; +end; + +% Deviate the trajectory orthogonally: +lambda = [y1 - y2 ; x2 - x1]; +lambda = lambda / sqrt(sum(lambda.^2)); + +Np = size(pts,2); +xs_mat = ones(2*win + 1,1)*pts(1,:); +ys_mat = ones(2*win + 1,1)*pts(2,:); +win_mat = (-win:win)'*ones(1,Np); +xs_mat2 = round(xs_mat - win_mat * lambda(1)); +ys_mat2 = round(ys_mat - win_mat * lambda(2)); +ind_mat = (xs_mat2) * ny + ys_mat2 + 1; +ima_patch = zeros(2*win + 1,Np); +ima_patch(:) = I(ind_mat(:)); + +filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; +out_f = sum(filtk.*ima_patch); +out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); +out_f_f = out_f_f(win+1:end-win); +ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; diff --git a/mr/Ub5/TOOLBOX_calib/dAB.m b/mr/Ub5/TOOLBOX_calib/dAB.m new file mode 100755 index 0000000..277052a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/dAB.m @@ -0,0 +1,39 @@ +function [dABdA,dABdB] = dAB(A,B); + +% [dABdA,dABdB] = dAB(A,B); +% +% returns : dABdA and dABdB + +[p,n] = size(A); [n2,q] = size(B); + +if n2 ~= n, + error(' A and B must have equal inner dimensions'); +end; + +if issparse(A) | issparse(B) | p*q*p*n>625 , + dABdA=spalloc(p*q,p*n,p*q*n); +else + dABdA=zeros(p*q,p*n); +end; + + +for i=1:q, + for j=1:p, + ij = j + (i-1)*p; + for k=1:n, + kj = j + (k-1)*p; + dABdA(ij,kj) = B(k,i); + end; + end; +end; + + +if issparse(A) | issparse(B) | p*q*n*q>625 , + dABdB=spalloc(p*q,n*q,p*q*n); +else + dABdB=zeros(p*q,q*n); +end; + +for i=1:q + dABdB([i*p-p+1:i*p]',[i*n-n+1:i*n]) = A; +end; diff --git a/mr/Ub5/TOOLBOX_calib/data_calib.m b/mr/Ub5/TOOLBOX_calib/data_calib.m new file mode 100755 index 0000000..99675a4 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/data_calib.m @@ -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; + diff --git a/mr/Ub5/TOOLBOX_calib/data_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/data_calib_no_read.m new file mode 100755 index 0000000..7aa90f3 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/data_calib_no_read.m @@ -0,0 +1,99 @@ +%%% 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''=''jpeg'',''g''=''jpeg'', ''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; + + + +if (Nima_valid~=0), + % Reading images: + + ima_read_calib_no_read; % may be launched from the toolbox itself + + + fprintf(1,'\n'); + + fprintf(1,'To display the thumbnail images of all the calibration images, you may run mosaic_no_read (may be slow)\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/downsample.m b/mr/Ub5/TOOLBOX_calib/downsample.m new file mode 100755 index 0000000..c46086e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/downsample.m @@ -0,0 +1,78 @@ +function V = downsample(U) + +% Try the 5x5 filter for building the pyramid: +% Now works with color images! + +U = double(U); + +[r,c,k] = size(U); + +p = floor((r+1)/2); % row +q = floor((c+1)/2); % col + + +U2 = zeros(r+2,c+2,k); + +for i=1:k + + U2(:,:,i) = [U(:,:,i) U(:,end,i)*ones(1,2); ones(2,1)*U(end,:,i) U(end,end,i)*ones(2,2)]; + +end; + +cp = 2*(0:(p-1))+1; % row +cq = 2*(0:(q-1))+1; % col + + +r2 = length(cp); +c2 = length(cq); + +e = cq+1; +ee = cq+2; +w = cq-1; w(1) = 1; +ww = cq-2; ww(1) = 1; ww(2) = 1; +n = cp-1; n(1) = 1; +nn = cp-2; nn(1) = 1; nn(2) = 1; +s = cp+1; +ss = cp+2; + +V = zeros(r2,c2,k); + +for i = 1:k, + + V(:,:,i) = (36*U2(cp,cq,i) + 24*(U2(n,cq,i) + U2(s,cq,i) + U2(cp,e,i) + U2(cp,w,i)) + ... + 16 * (U2(n,e,i) + U2(s,e,i) + U2(n,w,i) + U2(s,w,i)) + ... + 6 * (U2(nn,cq,i) + U2(ss,cq,i) + U2(cp,ee,i) + U2(cp,ww,i)) + ... + 4 * (U2(n,ww,i) + U2(nn,w,i) + U2(n,ee,i) + U2(nn,e,i) + U2(s,ww,i) + U2(ss,w,i) + U2(s,ee,i) + U2(ss,e,i)) + ... + (U2(nn,ee,i) + U2(ss,ee,i) + U2(nn,ww,i) + U2(ss,ww,i)))/256; + +end; + +return + +% DOWNSAMPLE2 9-point subsampling (see Burt,Adelson IEEE Tcomm 31, 532) +% V = downsample2(U) + +[r,c] = size(U); + +p = floor((r+1)/2); +q = floor((c+1)/2); + + +U2 = [U U(:,end); U(end,:) U(end,end)]; + + +cq = 2*(0:(q-1))+1; +cp = 2*(0:(p-1))+1; + +%cp = 2*([1:p]'-1)+1; +%cq = 2*([1:q]-1)+1; + +e = cq+1; %e(q) = e(q)-1; +w = cq-1; w(1) = w(1)+1; +n = cp-1; n(1) = n(1)+1; +s = cp+1; %s(p) = s(p)-1; + +% Interior +V = 0.25 * U2(cp,cq) + ... + 0.125*(U2(n,cq) + U2(s,cq) + U2(cp,e) + U2(cp,w)) + ... + 0.0625*(U2(n,e) + U2(s,e) + U2(n,w) + U2(s,w)); diff --git a/mr/Ub5/TOOLBOX_calib/edgefinder.m b/mr/Ub5/TOOLBOX_calib/edgefinder.m new file mode 100755 index 0000000..8777079 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/edgefinder.m @@ -0,0 +1,218 @@ +function [xc,good,bad,type] = edgefinder(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + +% WARNING!!! This function has not been fully tested!!! + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + %if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + %if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + %end; + + %end; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/mr/Ub5/TOOLBOX_calib/eliminate_boundary.m b/mr/Ub5/TOOLBOX_calib/eliminate_boundary.m new file mode 100755 index 0000000..2dcea7e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/eliminate_boundary.m @@ -0,0 +1,9 @@ +function Im2 = eliminate_boundary(Im); + + +[nny,nnx] = size(Im); + +Im2 = zeros(nny,nnx); + +Im2(2:end-1,2:end-1) = (Im(2:end-1,2:end-1) & Im(1:end-2,2:end-1) & Im(1:end-2,1:end-2) & Im(1:end-2,3:end) & ... + Im(3:end,2:end-1) & Im(3:end,1:end-2) & Im(3:end,3:end) & Im(2:end-1,1:end-2) & Im(2:end-1,3:end)); diff --git a/mr/Ub5/TOOLBOX_calib/error_analysis.m b/mr/Ub5/TOOLBOX_calib/error_analysis.m new file mode 100755 index 0000000..85feac5 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_analysis.m @@ -0,0 +1,182 @@ +%%% ERROR_ANALYSIS +%%% This simulation helps coputing the acturacies of calibration +%%% Run it after the main calibration + + + +N_runs = 200; + +%N_ima_active = 4; + +saving = 1; + +if 1, %~exist('fc_list'), % initialization + + % Initialization: + + load Calib_Results; + check_active_images; + + fc_list = []; + cc_list = []; + kc_list = []; + active_images_list = []; + + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [];']); + eval(['Tc_list_' num2str(kk) ' = [];']); + + end; + + %sx = median(abs(ex(1,:)))*1.4836; + %sy = median(abs(ex(2,:)))*1.4836; + + sx = std(ex(1,:)); + sy = std(ex(2,:)); + + % Saving the feature locations: + + for kk = 1:n_ima, + + eval(['x_save_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['y_save_' num2str(kk) ' = y_' num2str(kk) ';']); + + end; + + active_images_save = active_images; + ind_active_save = ind_active; + + fc_save = fc; + cc_save = cc; + kc_save = kc; + KK_save = KK; + + +end; + + + + +%%% The main loop: + + +for ntrial = 1:N_runs, + + fprintf(1,'\nRun number: %d\n',ntrial); + fprintf(1, '----------\n'); + + for kk = 1:n_ima, + + eval(['y_kk = y_save_' num2str(kk) ';']) + + if active_images(kk) & ~isnan(y_kk(1,1)), + + Nkk = size(y_kk,2); + + x_kk_new = y_kk + [sx * randn(1,Nkk);sy*randn(1,Nkk)]; + + eval(['x_' num2str(kk) ' = x_kk_new;']); + + end; + + end; + + N_active = length(ind_active_save); + junk = randn(1,N_active); + [junk,junk2] = sort(junk); + + active_images = zeros(1,n_ima); + active_images(ind_active_save(junk2(1:N_ima_active))) = ones(1,N_ima_active); + + fc = fc_save; + cc = cc_save; + kc = kc_save; + KK = KK_save; + + go_calib_optim; + + fc_list = [fc_list fc]; + cc_list = [cc_list cc]; + kc_list = [kc_list kc]; + active_images_list = [active_images_list active_images']; + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + + end; + +end; + + + + +if 0, + +% Restoring the feature locations: + +for kk = 1:n_ima, + + eval(['x_' num2str(kk) ' = x_save_' num2str(kk) ';']); + +end; + +fprintf(1,'\nFinal run (with the real data)\n'); +fprintf(1, '------------------------------\n'); + +active_images = active_images_save; +ind_active = ind_active_save; + +go_calib_optim; + +fc_list = [fc_list fc]; +cc_list = [cc_list cc]; +kc_list = [kc_list kc]; +active_images_list = [active_images_list active_images']; + +for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + +end; + +end; + + + + + +if saving, + +disp(['Save Calibration accuracy results under Calib_Accuracies_' num2str(N_ima_active) '.mat']); + +string_save = ['save Calib_Accuracies_' num2str(N_ima_active) ' active_images n_ima N_ima_active N_runs active_images_list fc cc kc fc_list cc_list kc_list']; + +for kk = 1:n_ima, + string_save = [string_save ' Tc_list_' num2str(kk) ' omc_list_' num2str(kk) ' Tc_' num2str(kk) ' omc_' num2str(kk) ]; +end; + +eval(string_save); + +end; + + +return; + +std(fc_list') + +std(cc_list') + +std(kc_list') + +for kk = 1:n_ima, + + eval(['std(Tc_list_' num2str(kk) ''')']) + +end; + + diff --git a/mr/Ub5/TOOLBOX_calib/error_cam_proj.m b/mr/Ub5/TOOLBOX_calib/error_cam_proj.m new file mode 100755 index 0000000..801f898 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_cam_proj.m @@ -0,0 +1,61 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = param(6:10); + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); + Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 10); +cp = param((3:4)+n_ima * 6 + 10); +alpha_p = param((5)+n_ima * 6 + 10); +kp = param((6:10)+n_ima * 6 + 10); + +om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); +T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/mr/Ub5/TOOLBOX_calib/error_cam_proj2.m b/mr/Ub5/TOOLBOX_calib/error_cam_proj2.m new file mode 100755 index 0000000..5671f1c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_cam_proj2.m @@ -0,0 +1,63 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% This is the same model, but with a simpler distortion model (no 6th order) + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = param(5); +kc = [param(6:9);0]; + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(11+(kk-1)*6-1:11+(kk-1)*6+2-1); + Tckk = param(11+(kk-1)*6+3-1:11+(kk-1)*6+3+2-1); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 10-1); +cp = param((3:4)+n_ima * 6 + 10-1); +alpha_p = param((5)+n_ima * 6 + 10-1); +kp = [param((6-1:10-2)+n_ima * 6 + 10);0]; + +om = param(10+n_ima*6+10+1-2:10+n_ima*6+10+1+2-2); +T = param(10+n_ima*6+10+1+2+1-2:10+n_ima*6+10+1+2+1+2-2); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/mr/Ub5/TOOLBOX_calib/error_cam_proj3.m b/mr/Ub5/TOOLBOX_calib/error_cam_proj3.m new file mode 100755 index 0000000..648a725 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_cam_proj3.m @@ -0,0 +1,63 @@ +function e_global = error_cam_proj(param); + +global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 + +% This is the same model, but with a simpler distortion model (no 6th order) + +% Computation of the errors: + +fc = param(1:2); +cc = param(3:4); +alpha_c = 0; +kc = [param(5);zeros(4,1)]; + +e_cam = []; + +for kk = 1:n_ima, + omckk = param(kk*6:kk*6+2); + Tckk = param(kk*6+3:kk*6+5); + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['xkk = x_' num2str(kk) ';']); + + ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); + + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); + + e_cam = [e_cam ekk]; + +end; + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fp = param((1:2)+n_ima * 6 + 5); +cp = param((3:4)+n_ima * 6 + 5); +alpha_p = 0; +kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; + +om = param((6:8)+n_ima * 6 + 5); +T = param((9:11)+n_ima * 6 + 5); + + +e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); + + +e_global = [e_cam e_proj]; + diff --git a/mr/Ub5/TOOLBOX_calib/error_depth.m b/mr/Ub5/TOOLBOX_calib/error_depth.m new file mode 100755 index 0000000..fa19ba1 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_depth.m @@ -0,0 +1,38 @@ +function err_shape = error_depth(param_dist,xcn,xpn,R,T,X_shape,ind); + + + +X_new = depth_compute(xcn,xpn,[param_dist],R,T); + + +N_pt_calib = size(xcn,2); + +% UnNormalized shape extraction: + +X_shape2 = X_new; +X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); + +% map the second vector at [1;0;0]: + +omu = -cross([1;0;0],X_shape2(:,2)); +omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); +Ru = rodrigues(omu); + +X_shape2 = Ru* X_shape2; + +omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); +omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); +Ru2 = rodrigues(omu2); + +X_shape2 = Ru2* X_shape2; + + +% Error: + +err_shape = X_shape2(:,2:end) - X_shape(:,2:end); + +err_shape = err_shape(:); + + + +%err_depth = Z_new - Z_ref; diff --git a/mr/Ub5/TOOLBOX_calib/error_depth_list.m b/mr/Ub5/TOOLBOX_calib/error_depth_list.m new file mode 100755 index 0000000..53ee10f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/error_depth_list.m @@ -0,0 +1,59 @@ +function err_total = error_depth_list(param_dist,xcn_list,xpn_list,R,T,X_shape_list,ind_list); + + +N_view = length(ind_list); + +err_total = []; + +N_pts = zeros(1,N_view); + +for kk = 1:N_view, + + xcn = xcn_list{kk}; + xpn = xpn_list{kk}; + ind = ind_list{kk}; + + xpn = xpn([1 3],:); + + X_shape = X_shape_list{kk}; + + +X_new = depth_compute(xcn,xpn,[param_dist],R,T); + + +N_pt_calib = size(xcn,2); + +% UnNormalized shape extraction: + +X_shape2 = X_new; +X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); + +% map the second vector at [1;0;0]: + +omu = -cross([1;0;0],X_shape2(:,2)); +omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); +Ru = rodrigues(omu); + +X_shape2 = Ru* X_shape2; + +omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); +omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); +Ru2 = rodrigues(omu2); + +X_shape2 = Ru2* X_shape2; + + +% Error: + +err_shape = X_shape2(:,2:end) - X_shape(:,2:end); + +err_shape = err_shape(:); + +N_pts(kk) = N_pt_calib; + +err_total = [ err_total ; err_shape ]; + +end; + + +%err_depth = Z_new - Z_ref; diff --git a/mr/Ub5/TOOLBOX_calib/export_calib_data.m b/mr/Ub5/TOOLBOX_calib/export_calib_data.m new file mode 100755 index 0000000..c66ea1a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/export_calib_data.m @@ -0,0 +1,104 @@ +%% Export calibration data (corners + 3D coordinates) to +%% text files (in Willson-Heikkila's format or Zhang's format) + +%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion +%% of adding this export function to the toolbox. + + +if ~exist('n_ima'), + fprintf(1,'ERROR: No calibration data to export\n'); + +else + + if n_ima == 0, + fprintf(1,'ERROR: No calibration data to export\n'); + return; + end; + + check_active_images; + + check_extracted_images; + + check_active_images; + + fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n'); + + qformat = -1; + + while (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n') + qformat = input('Format of export (enter 0 or 1): '); + + if isempty(qformat) + qformat = -1; + end; + + if (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Invalid entry. Try again.\n') + + end; + + end; + + if qformat == 0, + + fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n'); + outputfile = input('File basename: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Xx = [X_kk ; x_kk]'; + + file_name = [outputfile num2str(kk)]; + + disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']); + + eval(['save ' file_name ' Xx -ASCII']); + + end; + + else + + fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n'); + modelfile = input('File basename for the 3D world coordinates: ','s'); + datafile = input('File basename for the 2D image coordinates: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + if ~exist(['n_sq_x_' num2str(kk)]), + n_sq_x = 1; + n_sq_y = size(X_kk,2); + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + end; + + X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1)); + + x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1)); + + disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']); + + eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']); + eval(['save ' datafile num2str(kk) '.txt xy -ASCII']); + + end; + + +end; + +fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/ext_calib.m b/mr/Ub5/TOOLBOX_calib/ext_calib.m new file mode 100755 index 0000000..2c5093a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ext_calib.m @@ -0,0 +1,224 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('show_camera'), + show_camera = 1; +end; + + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1))]), + fprintf(1,'No calibration data available.\n'); + return; +end; +end; + +%if ~exist('no_grid'), + no_grid = 0; +%end; + +if n_ima ~= 0, +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; +else + no_grid = 1; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + + +if 0, + +err_std = std(ex'); + +fprintf(1,'\n\nCalibration results without principal point estimation:\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,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f]\n',kc); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f]\n\n',err_std); + +end; + + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if n_ima ~= 0, +if ~exist('dX'), + eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; +else + dX = 1; +end; + + +IP = 5*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE = 5*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15); + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +if show_camera, + figure(4); + plot3(BASE(1,:),BASE(3,:),-BASE(2,:),'b-','linewidth',2); + hold on; + plot3(IP(1,:),IP(3,:),-IP(2,:),'r-','linewidth',2); + text(6*dX,0,0,'X_c'); + text(-dX,5*dX,0,'Z_c'); + text(0,0,-6*dX,'Y_c'); + text(-dX,-dX,dX,'O_c'); +else + figure(4); + clf; + hold on; +end; + + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]), + + eval(['XX_kk = X_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_' num2str(kk) ';']); + eval(['Tc_kk = Tc_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk)); + + uu = [-dX;-dY;0]/2; + uu = R_kk * uu + Tc_kk; + + if ~no_grid, + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + %keyboard; + + figure(4); + hhh= mesh(YYx,YYz,-YYy); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + %plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['o' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + else + + figure(4); + plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters (camera-centered)'); +%view(60,30); +view(a,b); +grid on; +hold off; +axis vis3d; +axis tight; +set(4,'color',[1 1 1]); +if ~show_camera, + xlabel('X_c'); + ylabel('Z_c'); + zlabel('<-- Y_c'); +end; + +set(4,'Name','3D','NumberTitle','off'); + +%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n'); + + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; + +if n_ima ~= 0, + if show_camera, + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=0;ext_calib;', 'Position',[1-.30 0.04 .30 .04],'String','Remove camera reference frame','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + else + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=1;ext_calib;', 'Position',[1-.30 0.04 .30 .04],'String','Add camera reference frame','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + end; +end; + + + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +if n_ima ~= 0, +h_switch = uicontrol('Parent',4,'Units','normalized', 'Callback','ext_calib2', 'Position',[1-.30 0 .30 .04],'String','Switch to world-centered view','fontsize',8,'fontname','clean','Tag','Pushbutton1'); +end; + +figure(4); +rotate3d on; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/ext_calib2.m b/mr/Ub5/TOOLBOX_calib/ext_calib2.m new file mode 100755 index 0000000..f056122 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ext_calib2.m @@ -0,0 +1,234 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('show_camera'), + show_camera = 1; +end; + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['omc_' num2str(ind_active(1))]), + fprintf(1,'No calibration data available.\n'); + return; +end; + +%if ~exist('no_grid'), +no_grid = 0; +%end; + +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + + +if 0, + + err_std = std(ex'); + + fprintf(1,'\n\nCalibration results without principal point estimation:\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,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f]\n',kc); + fprintf(1,'Pixel error: err = [ %3.5f %3.5f]\n\n',err_std); + +end; + + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if ~exist('dX'), + eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; + +IP = 2*dX*[1 -alpha_c 0;0 1 0;0 0 1]*[1/fc(1) 0 0;0 1/fc(2) 0;0 0 1]*[1 0 -cc(1);0 1 -cc(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE = 2*(.9)*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15); +POS = [[6*dX;0;0] [0;6*dX;0] [-dX;0;5*dX] [-dX;-dX;-dX] [0;0;-dX]]; + + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +figure(4); +clf; +hold on; + + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]), + + eval(['XX_kk = X_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_' num2str(kk) ';']); + eval(['Tc_kk = Tc_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + BASEk = R_kk'*(BASE - Tc_kk * ones(1,6)); + IPk = R_kk'*(IP - Tc_kk * ones(1,15)); + POSk = R_kk'*(POS - Tc_kk * ones(1,5)); + + YY_kk = XX_kk; + + if ~no_grid, + + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + figure(4); + + if show_camera, + + p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]); + h1 = patch(p1); + set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r'); + p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]); + h2 = patch(p2); + %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none'); + set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none'); + + plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1'); + plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1); + text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold'); + + end; + + hhh= mesh(YYx,YYy,YYz); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + + else + + figure(4); + + if show_camera, + + p1 = struct('vertices',IPk','faces',[1 4 2;2 4 7;2 7 10;2 10 1]); + h1 = patch(p1); + set(h1,'facecolor',[52 217 160]/255,'EdgeColor', 'r'); + p2 = struct('vertices',IPk','faces',[1 10 7;7 4 1]); + h2 = patch(p2); + %set(h2,'facecolor',[236 171 76]/255,'EdgeColor', 'none'); + set(h2,'facecolor',[247 239 7]/255,'EdgeColor', 'none'); + + plot3(BASEk(1,:),BASEk(2,:),BASEk(3,:),'b-','linewidth',1'); + plot3(IPk(1,:),IPk(2,:),IPk(3,:),'r-','linewidth',1); + hww = text(POSk(1,5),POSk(2,5),POSk(3,5),num2str(kk),'fontsize',10,'color','k','FontWeight','bold'); + + end; + + plot3(YY_kk(1,:),YY_kk(2,:),YY_kk(3,:),['.' colors(rem(kk-1,6)+1)]); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters (world-centered)'); +%view(60,30); +xlabel('X_{world}') +ylabel('Y_{world}') +zlabel('Z_{world}') +view(a,b); +axis vis3d; +axis tight; +grid on; +plot3(3*dX*[1 0 0 0 0],3*dX*[0 0 1 0 0],3*dX*[0 0 0 0 1],'r-','linewidth',3); +hold off; +set(4,'color',[1 1 1]); + +set(4,'Name','3D','NumberTitle','off'); + +%hh = axis; +%hh(5) = 0; +%axis(hh); + +%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n'); + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; + +if n_ima ~= 0, + if show_camera, + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=0;ext_calib2;', 'Position',[1-.30 0.04 .30 .04],'String','Remove camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + else + h_switch2 = uicontrol('Parent',4,'Units','normalized', 'Callback','show_camera=1;ext_calib2;', 'Position',[1-.30 0.04 .30 .04],'String','Add camera reference frames','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + end; +end; + + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +h_switch = uicontrol('Parent',4,'Units','normalized', 'Callback','ext_calib', 'Position',[1-.30 0 .30 .04],'String','Switch to camera-centered view','fontsize',8,'fontname','clean','Tag','Pushbutton1'); + +figure(4); +rotate3d on; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/ext_calib_stereo.m b/mr/Ub5/TOOLBOX_calib/ext_calib_stereo.m new file mode 100755 index 0000000..c12cfd9 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ext_calib_stereo.m @@ -0,0 +1,169 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc_right')|~exist('fc_left'), + fprintf(1,'No stereo calibration data available.\n'); + return; +end; + +ind_active = find(active_images); + +no_grid = 0; + +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if ~exist('dX'), + eval(['dX = norm(Tc_left_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; + +%normT = min(norm(T)/2,2*dX); +normT = 2*dX; + + +IP_left = normT *[1 -alpha_c_left 0;0 1 0;0 0 1]*[1/fc_left(1) 0 0;0 1/fc_left(2) 0;0 0 1]*[1 0 -cc_left(1);0 1 -cc_left(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +BASE_left = normT *([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP_left = reshape([IP_left ;BASE_left(:,1)*ones(1,5);IP_left ],3,15); + +IP_right = normT *[1 -alpha_c_right 0;0 1 0;0 0 1]*[1/fc_right(1) 0 0;0 1/fc_right(2) 0;0 0 1]*[1 0 -cc_right(1);0 1 -cc_right(2);0 0 1]*[0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1]; +IP_right = reshape([IP_right;BASE_left(:,1)*ones(1,5);IP_right],3,15); + +% Relative position of right camera wrt left camera: (om,T) +R = rodrigues(om); + + +% Change of reference: +BASE_right = R'*(BASE_left - repmat(T,[1 6])); +IP_right = R'*(IP_right - repmat(T,[1 15])); + + +if ishandle(4), + figure(4); + [a,b] = view; +else + figure(4); + a = 50; + b = 20; +end; + + +figure(4); +plot3(BASE_left(1,:),BASE_left(3,:),-BASE_left(2,:),'b-','linewidth',2'); +hold on; +plot3(IP_left(1,:),IP_left(3,:),-IP_left(2,:),'r-','linewidth',2); +text(BASE_left(1,2),BASE_left(3,2),-BASE_left(2,2),'X','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,6),BASE_left(3,6),-BASE_left(2,6),'Z','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,4),BASE_left(3,4),-BASE_left(2,4),'Y','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_left(1,1),BASE_left(3,1),-BASE_left(2,1),'Left Camera','HorizontalAlignment','center','FontWeight','bold'); +plot3(BASE_right(1,:),BASE_right(3,:),-BASE_right(2,:),'b-','linewidth',2'); +plot3(IP_right(1,:),IP_right(3,:),-IP_right(2,:),'r-','linewidth',2); +text(BASE_right(1,2),BASE_right(3,2),-BASE_right(2,2),'X','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,6),BASE_right(3,6),-BASE_right(2,6),'Z','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,4),BASE_right(3,4),-BASE_right(2,4),'Y','HorizontalAlignment','center','FontWeight','bold'); +text(BASE_right(1,1),BASE_right(3,1),-BASE_right(2,1),'Right Camera','HorizontalAlignment','center','FontWeight','bold'); + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_left_' num2str(kk)]) & exist(['omc_left_' num2str(kk)]), + + eval(['XX_kk = X_left_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_left_' num2str(kk) ';']); + eval(['Tc_kk = Tc_left_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk)); + + uu = [-dX;-dY;0]/2; + uu = R_kk * uu + Tc_kk; + + if ~no_grid, + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + + figure(4); + hhh= mesh(YYx,YYz,-YYy); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1),'HorizontalAlignment','center'); + else + + figure(4); + plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1),'HorizontalAlignment','center'); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters'); +view(a,b); +grid on; +hold off; +axis vis3d; +axis tight; +set(4,'color',[1 1 1]); + +set(4,'Name','3D','NumberTitle','off'); + + +if exist('h_switch')==1, + if ishandle(h_switch), + delete(h_switch); + end; +end; + +if exist('h_switch2')==1, + if ishandle(h_switch2), + delete(h_switch2); + end; +end; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/extract_distortion_data.m b/mr/Ub5/TOOLBOX_calib/extract_distortion_data.m new file mode 100755 index 0000000..b6f8c07 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_distortion_data.m @@ -0,0 +1,46 @@ +%%% Small script file that etst + +%load Calib_Results; + +% Collect all the points distorted (xd) and undistorted (xn) from the +% images: + +xn = []; +xd = []; +for kk = ind_active, + eval(['x_kk = x_' num2str(kk) ';']); + xd_kk = normalize_pixel(x_kk,fc,cc,zeros(5,1),alpha_c); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + xn_kk = project_points2(X_kk,omckk,Tckk); + xd = [xd xd_kk]; + xn = [xn xn_kk]; +end; + + +% Data points: +r = sqrt(sum(xn.^2)); % The undistorted radii +rp = sqrt(sum(xd.^2)); % The distorted radii + +%--- Try different analytical models to fit r_prime = D(r) + +ri = 0.005:.005:max(r); + +% Calibration toolbox model: +rt = ri .* (1 + kc(1)*ri.^2 + kc(2)*ri.^4 + kc(5)*ri.^6); + + + +return; + + +figure(10); +clf; +h1 = plot(r,rp,'r.','markersize',.1); hold on; +h2 = plot(ri,rt,'r-','linewidth',.1); +title('Radial distortion function (with unit focal) - r prime = D(r)'); +xlabel('r'); +ylabel('r prime'); +zoom on; + diff --git a/mr/Ub5/TOOLBOX_calib/extract_grid.m b/mr/Ub5/TOOLBOX_calib/extract_grid.m new file mode 100755 index 0000000..1b1936e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_grid.m @@ -0,0 +1,329 @@ +function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY,xr,yr,click_mode); + +if nargin < 11, + click_mode = 0; +end; + + +if nargin < 10, + xr = []; + yr = []; +end; + + +map = gray(256); + +minI = min(I(:)); +maxI = max(I(:)); + +Id = 255*(I - minI)/(maxI - minI); + +figure(2); +image(Id); +colormap(map); + +if ~isempty(xr); + figure(2); + hold on; + plot(xr,yr,'go'); + hold off; +end; + + +if nargin < 2, + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +need_to_click = 1; + +color_line = 'g'; + +while need_to_click, + + + title('Click on the four extreme corners of the rectangular pattern (first corner = origin)...'); + + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + %ind = ind([2 3 4 1]); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + + x = x(ind); + y = y(ind); + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + + figure(2); image(Id); + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + hold off; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + if ~click_mode, + + % This way, the user manually enters the number of squares and no more clicks. + % Otherwise, he user is asked to click again. + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + need_to_click = 0; + + end; + + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + need_to_click = 0; + + end; + + color_line = 'r'; + +end; + + +if ~exist('dX')|~exist('dY'), + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=30mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']); + if isempty(dX), dX = 30; end; + if isempty(dY), dY = 30; end; + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrice as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + +if nargin < 6, + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +else + + xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(2)],kc); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + r2 = sum(XXu.^2); + XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu; + XX(1,:) = fc(1)*XX(1,:)+cc(1); + XX(2,:) = fc(2)*XX(2,:)+cc(2); + +end; + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +ind_x = (n_sq_x+1)*(n_sq_y + 1); +ind_y = 1; + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + + +figure(3); +image(Id); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + diff --git a/mr/Ub5/TOOLBOX_calib/extract_grid_manual.m b/mr/Ub5/TOOLBOX_calib/extract_grid_manual.m new file mode 100755 index 0000000..1b1936e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_grid_manual.m @@ -0,0 +1,329 @@ +function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY,xr,yr,click_mode); + +if nargin < 11, + click_mode = 0; +end; + + +if nargin < 10, + xr = []; + yr = []; +end; + + +map = gray(256); + +minI = min(I(:)); +maxI = max(I(:)); + +Id = 255*(I - minI)/(maxI - minI); + +figure(2); +image(Id); +colormap(map); + +if ~isempty(xr); + figure(2); + hold on; + plot(xr,yr,'go'); + hold off; +end; + + +if nargin < 2, + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +need_to_click = 1; + +color_line = 'g'; + +while need_to_click, + + + title('Click on the four extreme corners of the rectangular pattern (first corner = origin)...'); + + disp('Click on the four extreme corners of the rectangular complete pattern (the first clicked corner is the origin)...'); + + x= [];y = []; + figure(2); hold on; + for count = 1:4, + [xi,yi] = ginput4(1); + [xxi] = cornerfinder([xi;yi],I,winty,wintx); + xi = xxi(1); + yi = xxi(2); + figure(2); + plot(xi,yi,'+','color',[ 1.000 0.314 0.510 ],'linewidth',2); + plot(xi + [wintx+.5 -(wintx+.5) -(wintx+.5) wintx+.5 wintx+.5],yi + [winty+.5 winty+.5 -(winty+.5) -(winty+.5) winty+.5],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + x = [x;xi]; + y = [y;yi]; + plot(x,y,'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + end; + plot([x;x(1)],[y;y(1)],'-','color',[ 1.000 0.314 0.510 ],'linewidth',2); + drawnow; + hold off; + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + + % Sort the corners: + x_mean = mean(x); + y_mean = mean(y); + x_v = x - x_mean; + y_v = y - y_mean; + + theta = atan2(-y_v,x_v); + [junk,ind] = sort(theta); + + [junk,ind] = sort(mod(theta-theta(1),2*pi)); + + %ind = ind([2 3 4 1]); + + ind = ind([4 3 2 1]); %-> New: the Z axis is pointing uppward + + + x = x(ind); + y = y(ind); + x1= x(1); x2 = x(2); x3 = x(3); x4 = x(4); + y1= y(1); y2 = y(2); y3 = y(3); y4 = y(4); + + + % Find center: + p_center = cross(cross([x1;y1;1],[x3;y3;1]),cross([x2;y2;1],[x4;y4;1])); + x5 = p_center(1)/p_center(3); + y5 = p_center(2)/p_center(3); + + % center on the X axis: + x6 = (x3 + x4)/2; + y6 = (y3 + y4)/2; + + % center on the Y axis: + x7 = (x1 + x4)/2; + y7 = (y1 + y4)/2; + + % Direction of displacement for the X axis: + vX = [x6-x5;y6-y5]; + vX = vX / norm(vX); + + % Direction of displacement for the X axis: + vY = [x7-x5;y7-y5]; + vY = vY / norm(vY); + + % Direction of diagonal: + vO = [x4 - x5; y4 - y5]; + vO = vO / norm(vO); + + delta = 30; + + + figure(2); image(Id); + colormap(map); + hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text(x6 + delta * vX(1) ,y6 + delta*vX(2),'X'); + set(hx,'color','g','Fontsize',14); + hy=text(x7 + delta*vY(1), y7 + delta*vY(2),'Y'); + set(hy,'color','g','Fontsize',14); + hO=text(x4 + delta * vO(1) ,y4 + delta*vO(2),'O','color','g','Fontsize',14); + hold off; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + if ~click_mode, + + % This way, the user manually enters the number of squares and no more clicks. + % Otherwise, he user is asked to click again. + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + need_to_click = 0; + + end; + + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + need_to_click = 0; + + end; + + color_line = 'r'; + +end; + + +if ~exist('dX')|~exist('dY'), + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=30mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']); + if isempty(dX), dX = 30; end; + if isempty(dY), dY = 30; end; + +end; + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x(1);y(1);1]; +a10 = [x(2);y(2);1]; +a11 = [x(3);y(3);1]; +a01 = [x(4);y(4);1]; + + +% Compute the planar collineation: (return the normalization matrice as well) + +[Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XX = Homo*pts; +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + +% Complete size of the rectangle + +W = n_sq_x*dX; +L = n_sq_y*dY; + + + +if nargin < 6, + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + +else + + xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(2)],kc); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + r2 = sum(XXu.^2); + XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu; + XX(1,:) = fc(1)*XX(1,:)+cc(1); + XX(2,:) = fc(2)*XX(2,:)+cc(2); + +end; + + +Np = (n_sq_x+1)*(n_sq_y+1); + +disp('Corner extraction...'); + +grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + +grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + +ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners +ind_orig = (n_sq_x+1)*n_sq_y + 1; +xorig = grid_pts(1,ind_orig); +yorig = grid_pts(2,ind_orig); +dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); +dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + +ind_x = (n_sq_x+1)*(n_sq_y + 1); +ind_y = 1; + +x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; +y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + + +figure(3); +image(Id); colormap(map); hold on; +plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); +plot(x_box_kk+1,y_box_kk+1,'-b'); +plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); +plot(xorig+1,yorig+1,'*m'); +h = text(xorig+delta*vO(1),yorig+delta*vO(2),'O'); +set(h,'Color','m','FontSize',14); +h2 = text(dxpos(1)+delta*vX(1),dxpos(2)+delta*vX(2),'dX'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)+delta*vY(1),dypos(2)+delta*vY(2),'dY'); +set(h3,'Color','g','FontSize',14); +xlabel('Xc (in camera frame)'); +ylabel('Yc (in camera frame)'); +title('Extracted corners'); +zoom on; +drawnow; +hold off; + + + +Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; +Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; +Zi = zeros(1,Np); + +Xgrid = [Xi;Yi;Zi]; + + +% All the point coordinates (on the image, and in 3D) - for global optimization: + +x = grid_pts; +X = Xgrid; + diff --git a/mr/Ub5/TOOLBOX_calib/extract_parameters.m b/mr/Ub5/TOOLBOX_calib/extract_parameters.m new file mode 100755 index 0000000..71dcbc9 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_parameters.m @@ -0,0 +1,61 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +if ~exist('solution_error') + solution_error = zeros(6*n_ima + 15,1); +end; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:10);%*** + +fc_error = solution_error(1:2); +cc_error = solution_error(3:4); +alpha_c_error = solution_error(5); +kc_error = solution_error(6:10); + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +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;']); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/extract_parameters3D.m b/mr/Ub5/TOOLBOX_calib/extract_parameters3D.m new file mode 100755 index 0000000..841c6ab --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_parameters3D.m @@ -0,0 +1,36 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + + +fc = solution(1:2); +kc = solution(3:6); +cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3); + +% Calibration matrix: + +KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + omckk = solution(4+6*(kk-1) + 3:6*kk + 3); + + Tckk = solution(6*kk+1 + 3:6*kk+3 + 3); + + Rckk = rodrigues(omckk); + + Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hlkk = Hlkk / Hlkk(3,3); + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + + eval(['Hl_' num2str(kk) '=Hlkk;']); + +end; + + diff --git a/mr/Ub5/TOOLBOX_calib/extract_parameters_fisheye.m b/mr/Ub5/TOOLBOX_calib/extract_parameters_fisheye.m new file mode 100755 index 0000000..219be86 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extract_parameters_fisheye.m @@ -0,0 +1,61 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +if ~exist('solution_error') + solution_error = zeros(6*n_ima + 15,1); +end; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:9);%*** + +fc_error = solution_error(1:2); +cc_error = solution_error(3:4); +alpha_c_error = solution_error(5); +kc_error = solution_error(6:9); + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +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;']); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/extrinsic_computation.m b/mr/Ub5/TOOLBOX_calib/extrinsic_computation.m new file mode 100755 index 0000000..84691b0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/extrinsic_computation.m @@ -0,0 +1,186 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +dir; + +fprintf(1,'\n'); +disp('Computation of the extrinsic parameters from an image of a pattern'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); +image_name = input('Image name (full name without extension): ','s'); + +format_image2 = '0'; + +while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; +end; + +ima_name = [image_name '.' format_image2]; + + +%%% READ IN IMAGE: + +if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +if size(I,3)>1, + I = I(:,:,2); +end; + + +%%% EXTRACT GRID CORNERS: + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + +[x_ext,X_ext,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc); + + + +%%% Computation of the Extrinsic Parameters attached to the grid: + +[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c); + + +%%% Reproject the points on the image: + +[x_reproj] = project_points2(X_ext,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +err_reproj = x_ext - x_reproj; + +err_std2 = std(err_reproj')'; + + +Basis = [X_ext(:,[ind_orig ind_x ind_orig ind_y ind_orig ])]; + +VX = Basis(:,2) - Basis(:,1); +VY = Basis(:,4) - Basis(:,1); + +nX = norm(VX); +nY = norm(VY); + +VZ = min(nX,nY) * cross(VX/nX,VY/nY); + +Basis = [Basis VZ]; + +[x_basis] = project_points2(Basis,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +dxpos = (x_basis(:,2) + x_basis(:,1))/2; +dypos = (x_basis(:,4) + x_basis(:,3))/2; +dzpos = (x_basis(:,6) + x_basis(:,5))/2; + + + +figure(2); +image(I); +colormap(gray(256)); +hold on; +plot(x_ext(1,:)+1,x_ext(2,:)+1,'r+'); +plot(x_reproj(1,:)+1,x_reproj(2,:)+1,'yo'); +h = text(x_ext(1,ind_orig)-25,x_ext(2,ind_orig)-25,'O'); +set(h,'Color','g','FontSize',14); +h2 = text(dxpos(1)+1,dxpos(2)-30,'X'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)-30,dypos(2)+1,'Y'); +set(h3,'Color','g','FontSize',14); +h4 = text(dzpos(1)-10,dzpos(2)-20,'Z'); +set(h4,'Color','g','FontSize',14); +plot(x_basis(1,:)+1,x_basis(2,:)+1,'g-','linewidth',2); +title('Image points (+) and reprojected grid points (o)'); +hold off; + + +fprintf(1,'\n\nExtrinsic parameters:\n\n'); +fprintf(1,'Translation vector: Tc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',Tc_ext); +fprintf(1,'Rotation vector: omc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',omc_ext); +fprintf(1,'Rotation matrix: Rc_ext = [ %3.6f \t %3.6f \t %3.6f\n',Rc_ext(1,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f\n',Rc_ext(2,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f ]\n',Rc_ext(3,:)'); +fprintf(1,'Pixel error: err = [ %3.5f \t %3.5f ]\n\n',err_std2); + + + + + +return; + + +% Stores the results: + +kk = 1; + +% Stores location of grid wrt camera: + +eval(['omc_' num2str(kk) ' = omc_ext;']); +eval(['Tc_' num2str(kk) ' = Tc_ext;']); + +% Stores the projected points: + +eval(['y_' num2str(kk) ' = x_reproj;']); +eval(['X_' num2str(kk) ' = X_ext;']); +eval(['x_' num2str(kk) ' = x_ext;']); + + +% Organize the points in a grid: + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/fixallvariables.m b/mr/Ub5/TOOLBOX_calib/fixallvariables.m new file mode 100755 index 0000000..27bd31d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/fixallvariables.m @@ -0,0 +1,19 @@ +% Code that clears all empty or NaN variables + +varlist = whos; + +if ~isempty(varlist), + + Nvar = size(varlist,1); + + for c_var = 1:Nvar, + + var2fix = varlist(c_var).name; + + fixvariable; + + end; + +end; + +clear varlist var2fix Nvar c_var \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/fixvariable.m b/mr/Ub5/TOOLBOX_calib/fixvariable.m new file mode 100755 index 0000000..1118533 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/fixvariable.m @@ -0,0 +1,18 @@ +% Code that clears an empty variable, or a NaN vsriable. +% Does not clear structures, or cells. + +if exist('var2fix')==1, + if eval(['exist(''' var2fix ''') == 1']), + if eval(['isempty(' var2fix ')']), + eval(['clear ' var2fix ]); + else + if eval(['~isstruct(' var2fix ')']), + if eval(['~iscell(' var2fix ')']), + if eval(['isnan(' var2fix '(1))']), + eval(['clear ' var2fix ]); + end; + end; + end; + end; + end; +end; diff --git a/mr/Ub5/TOOLBOX_calib/fov.m b/mr/Ub5/TOOLBOX_calib/fov.m new file mode 100755 index 0000000..425a1e0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/fov.m @@ -0,0 +1,12 @@ +% small program that computes the field of view of a camera (in degrees) + +if ~exist('fc')|~exist('cc')|~exist('nx')|~exist('ny'), + error('Need calibration results to compute FOV (fc,cc,Wcal,Hcal)'); +end; + +FOV_HOR = 180 * ( atan((nx - (cc(1)+.5))/fc(1)) + atan((cc(1)+.5)/fc(1)) )/pi; + +FOV_VER = 180 * ( atan((ny - (cc(2)+.5))/fc(2)) + atan((cc(2)+.5)/fc(2)) )/pi; + +fprintf(1,'Horizontal field of view = %.2f degrees\n',FOV_HOR); +fprintf(1,'Vertical field of view = %.2f degrees\n',FOV_VER); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/ginput2.m b/mr/Ub5/TOOLBOX_calib/ginput2.m new file mode 100755 index 0000000..e7fa17a --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ginput2.m @@ -0,0 +1,207 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 5.18 $ $Date: 1996/11/10 17:48:08 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + + pointer = get(gcf,'pointer'); + set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; %b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/mr/Ub5/TOOLBOX_calib/ginput3.m b/mr/Ub5/TOOLBOX_calib/ginput3.m new file mode 100755 index 0000000..a0b54f2 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ginput3.m @@ -0,0 +1,216 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 5.18 $ $Date: 1996/11/10 17:48:08 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + +pointer = get(gcf,'pointer'); + +set(gcf,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); +%set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; %b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/mr/Ub5/TOOLBOX_calib/ginput4.m b/mr/Ub5/TOOLBOX_calib/ginput4.m new file mode 100755 index 0000000..9a7c14f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ginput4.m @@ -0,0 +1,242 @@ +function [out1,out2,out3] = ginput4(arg1) + +[out1,out2,out3] = ginput(arg1); + +return; + + + +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. +% +% Examples: +% [x,y] = ginput; +% +% [x,y] = ginput(5); +% +% [x, y, button] = ginput(1); +% +% See also GTEXT, UIRESTORE, UISUSPEND, WAITFORBUTTONPRESS. + +% Copyright 1984-2006 The MathWorks, Inc. +% $Revision: 5.32.4.9 $ $Date: 2006/12/20 07:19:10 $ + + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') && ~strcmp(tp,'x') && ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + out1 = trmginput(arg1); + else + out1 = trmginput; + end + elseif nargout == 2 || nargout == 0, + if nargin == 1, + [out1,out2] = trmginput(arg1); + else + [out1,out2] = trmginput; + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + [out1,out2,out3] = trmginput(arg1); + else + [out1,out2,out3] = trmginput; + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if ischar(how_many) ... + || size(how_many,1) ~= 1 || size(how_many,2) ~= 1 ... + || ~(fix(how_many) == how_many) ... + || how_many < 0 + error('MATLAB:ginput:NeedPositiveInt', 'Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('MATLAB:ginput:InvalidArgument', 'Argument must be a positive integer.') + end + end + + % Suspend figure functions + state = uisuspend(fig); + + toolbar = findobj(allchild(fig),'flat','Type','uitoolbar'); + if ~isempty(toolbar) + ptButtons = [uigettool(toolbar,'Plottools.PlottoolsOff'), ... + uigettool(toolbar,'Plottools.PlottoolsOn')]; + ptState = get (ptButtons,'Enable'); + set (ptButtons,'Enable','off'); + end + + %set(fig,'pointer','fullcrosshair'); + set(fig,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); + + fig_units = get(fig,'units'); + char = 0; + + % We need to pump the event queue on unix + % before calling WAITFORBUTTONPRESS + drawnow + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + try + keydown = wfbp; + catch + waserr = 1; + end + if(waserr == 1) + if(ishandle(fig)) + set(fig,'units',fig_units); + uirestore(state); + error('MATLAB:ginput:Interrupted', 'Interrupted'); + else + error('MATLAB:ginput:FigureDeletionPause', 'Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + % We need to compensate for an off-by-one error: + pt = [scrn_pt(1) - loc(1) + 1, scrn_pt(2) - loc(2) + 1]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = 1; + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('MATLAB:ginput:InvalidSelection', 'Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + uirestore(state); + if ~isempty(toolbar) && ~isempty(ptButtons) + set (ptButtons(1),'Enable',ptState{1}); + set (ptButtons(2),'Enable',ptState{2}); + end + set(fig,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +fig = gcf; +current_char = []; + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +try + h=findall(fig,'type','uimenu','accel','C'); % Disabling ^C for edit menu so the only ^C is for + set(h,'accel',''); % interrupting the function. + keydown = waitforbuttonpress; + current_char = double(get(fig,'CurrentCharacter')); % Capturing the character. + if~isempty(current_char) && (keydown == 1) % If the character was generated by the + if(current_char == 3) % current keypress AND is ^C, set 'waserr'to 1 + waserr = 1; % so that it errors out. + end + end + + set(h,'accel','C'); % Set back the accelerator for edit menu. +catch + waserr = 1; +end +drawnow; +if(waserr == 1) + set(h,'accel','C'); % Set back the accelerator if it errored out. + error('MATLAB:ginput:Interrupted', 'Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim.m new file mode 100755 index 0000000..6c51d28 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim.m @@ -0,0 +1,68 @@ +%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('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 ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +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; diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.asv b/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.asv new file mode 100755 index 0000000..18ae1a2 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.asv @@ -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_no_read; % Load the images + click_calib_fisheye_no_read; % 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(2,1);zeros(2,1)]; +end; + + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter_fisheye; + + +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_fisheye; + + 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; + diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.m new file mode 100755 index 0000000..67fe03f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_fisheye_no_read.m @@ -0,0 +1,68 @@ +%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('n_ima'), + data_calib_no_read; % Load the images + click_calib_fisheye_no_read; % 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 ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +if (rosette_calibration) + %%% Special Setting for the Rosette: + est_dist = [ones(2,1);zeros(2,1)]; +end; + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter_fisheye; + +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_fisheye; + 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; diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter.m new file mode 100755 index 0000000..c5b386d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter.m @@ -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 + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +% 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-9)&(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; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_fisheye.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_fisheye.m new file mode 100755 index 0000000..d95de6d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_fisheye.m @@ -0,0 +1,618 @@ +%go_calib_optim_iter_fisheye +% +%Main calibration function. Computes the intrinsic and extrinsic parameters. +%for fisheye cameras. +%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: Fisheye 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_fisheye.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('kc') + if length(kc) > 4, + clear kc; + end; +end; + +if exist('est_dist') + if length(est_dist) > 4, + clear est_dist; + end; +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]; +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'); +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.2; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%%% 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.4; % 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.4; % slow convergence +end; + +if ~exist('fc')& quick_init, + fc = (max(nx,ny) / pi) * ones(2,1); + est_fc = [1;1]; + alpha_smooth = 0.4; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the Fisheye intrinsic parameters\n') + init_intrinsic_param_fisheye; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.4; % slow convergence + est_fc = [1;1]; +end; + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(4,1); + alpha_smooth = 0.4; % 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_fisheye; + + +%%% Initialization of the global parameter vector: + +init_param = [fc;cc;alpha_c;kc;zeros(6,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-6)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:9); + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + %if active_images(kk), + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,f(1),c,k,alpha); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,f,c,k,alpha); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:9,1:9) = JJ3(1:9,1:9) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)'; + + ex3(1:9) = ex3(1:9) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + + %JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + %JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + %AB = sparse(A*B'); + %JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + %JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + %ex3(1:10) = ex3(1:10) + A*exkk(:); + %ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + + % Check if this view is ill-conditioned: + if check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + %end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ3 = JJ3(ind_Jac,ind_Jac); + ex3 = ex3(ind_Jac); + + JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex3; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + + if center_optim & ((param(3)<0)|(param(3)>nx)|(param(4)<0)|(param(4)>ny)), + fprintf(1,'Warning: it appears that the principal point cannot be estimated. Setting center_optim = 0\n'); + center_optim = 0; + cc_current = c; + else + cc_current = param(3:4); + end; + + alpha_current = param(5); + kc_current = param(6:9); + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + + %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!! + %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes) + %% The complete gradient descent method is useful to precisely update the intrinsic parameters. + + + if recompute_extrinsic, + MaxIter2 = 20; + for kk =ind_active, %1:n_ima, + %if active_images(kk), + omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + [omc_current,Tc_current] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond); + if check_cond, + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk); + desactivated_images = [desactivated_images kk]; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + end; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk; + %end; + end; + end; + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:9); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib_fisheye; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:9,1:9) = JJ3(1:9,1:9) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:9,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:9) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); + +JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters_fisheye; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] � [ %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,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f %3.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') + + + +return; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_weak.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_weak.m new file mode 100755 index 0000000..1385d0c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_iter_weak.m @@ -0,0 +1,673 @@ +%%% WARNING!!! This code is not in working condition yet. +%%% AS A RESULT, IT IS NOT SUPPORTED!!! +%%% -Jean-Yves Bouguet + +%go_calib_optim_iter_weak +% +%Main calibration function. Computes the intrinsic andextrinsic parameters +%for the weak perspective camera model +%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 + + + +%%% Little modifications for weak perspective model: +center_optim = 0; % No center estimation for weak perspective model +%est_dist = zeros(5,1); % No distortion for weak perspective model +%est_alpha = 0; % No skew for weak perspective model + + + +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). Default state for weak perspective model'); + 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) - (DEFAULT for weak perspective model) . \n'); + end; + if ~est_dist(2), + fprintf(1,' Fourth order distortion not estimated (est_dist(2)=0) - (DEFAULT for weak perspective model) .\n'); + end; + if ~est_dist(5), + fprintf(1,' Sixth order distortion not estimated (est_dist(5)=0) - (DEFAULT for weak perspective model) .\n'); + end; + if ~prod(double(est_dist(3:4))), + fprintf(1,' Tangential distortion not estimated (est_dist(3:4)~=[1;1]) - (DEFAULT for weak perspective model) .\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 = 1; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%% 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.4; % 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('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(5,1); + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.4; % 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.4; % 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.4; % slow convergence + est_fc = [1;1]; +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; + + +% Compute the average distance of all the points to the camera: +Zave_list = zeros(1,length(ind_active)); +for ii = 1:length(ind_active), + kk = ind_active(ii); + eval(['X_kk = X_' num2str(kk) ';']); + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + Y_kk = rigid_motion(X_kk,omckk,Tckk); + Zave_list(ii) = mean(Y_kk(3,:)); +end; + +Zave = sum(N_points_views .* Zave_list) / sum(N_points_views); + +fprintf(1,'Weak Perspective Camera Calibration - Setting the average depth of the scene at Zave = %.6f\n',Zave); + + +while (change > 1e-9)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:10); + + %%% Make sure that all the Z translations satisfy the average depth constraint: + % Compute the average distance of all the points to the camera: + Zave_list = zeros(1,length(ind_active)); + for ii = 1:length(ind_active), + kk = ind_active(ii); + eval(['X_kk = X_' num2str(kk) ';']); + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + Y_kk = rigid_motion(X_kk,omckk,Tckk); + Zave_list(ii) = mean(Y_kk(3,:)); + end; + + Zave2 = sum(N_points_views .* Zave_list) / sum(N_points_views); + + change_T = Zave2 - Zave; + + for ii = 1:length(ind_active), + kk = ind_active(ii); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + %Tckk2 = Tckk .* [1;1;ratio_T]; + Tckk2 = Tckk - [0;0;change_T]; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk2; + end; + + + % To speed up: pre-allocate the memory for the Jacobian JJ3. + % For that, need to compute the total number of points. + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + + + + + % Compute the size of the Jacobian matrix: + N_points_views_active = N_points_views(ind_active); + + JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + ex3 = zeros(15 + 6*n_ima,1); + + + for kk = ind_active, %1:n_ima, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = N_points_views(kk); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f(1),c,k,alpha,Zave); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,f,c,k,alpha,Zave); + end; + + exkk = x_kk - x; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + + ex3(1:10) = ex3(1:10) + A*exkk(:); + ex3(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = B*exkk(:); + + % Check if this view is ill-conditioned: + if 0, %check_cond, + JJ_kk = B'; %[dxdom dxdT]; + if (cond(JJ_kk)> thresh_cond), + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive. (note: to disactivate this option, set check_cond=0)\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + selected_variables = [est_fc;center_optim*ones(2,1);est_alpha;est_dist;zeros(5,1);reshape(ones(6,1)*active_images,6*n_ima,1)]; + if ~est_aspect_ratio, + if isequal(est_fc,[1;1]) | isequal(est_fc,[1;0]), + selected_variables(2) = 0; + end; + end; + ind_Jac = find(selected_variables)'; + + JJ4 = JJ3(ind_Jac,ind_Jac); + ex4 = ex3(ind_Jac); + + + % Try to make the inversion work: + [U,S,V] = svd(full(JJ4)); + + + + if 0, + s = diag(S); + figure(100); + semilogy(s); + end; + + n_reject = length(ind_active); + + U = U(:,1:end-n_reject); + S = S(1:end-n_reject,1:end-n_reject); + + + JJ2_inv = U * inv(S) * U'; + + %JJ2_inv = inv(JJ4); % not bad for sparse matrices!! + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + param_innov = alpha_smooth2*JJ2_inv*ex4; + + + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + alpha_current = param(5); + kc_current = param(6:10); + + + if ~est_aspect_ratio & isequal(est_fc,[1;1]), + fc_current(2) = fc_current(1); + param(2) = param(1); + end; + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + param_list = [param_list param]; + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +check_active_images; + +solution = param; + + +% Extraction of the paramters for computing the right reprojection error: + +fc = solution(1:2); +cc = solution(3:4); +alpha_c = solution(5); +kc = solution(6:10); + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + Rckk = rodrigues(omckk); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + +end; + + +% Recompute the error (in the vector ex): +comp_error_calib; + +sigma_x = std(ex(:)); + +% Compute the size of the Jacobian matrix: +N_points_views_active = N_points_views(ind_active); + +JJ3 = sparse([],[],[],15 + 6*n_ima,15 + 6*n_ima,126*n_ima + 225); + +for kk = ind_active, + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + eval(['X_kk = X_' num2str(kk) ';']); + + Np = N_points_views(kk); + + %[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave); + + if ~est_aspect_ratio, + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc(1),cc,kc,alpha_c,Zave); + dxdf = repmat(dxdf,[1 2]); + else + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_weak(X_kk,omckk,Tckk,fc,cc,kc,alpha_c,Zave); + end; + + A = [dxdf dxdc dxdalpha dxdk]'; + B = [dxdom dxdT]'; + + JJ3(1:10,1:10) = JJ3(1:10,1:10) + sparse(A*A'); + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = sparse(B*B'); + + AB = sparse(A*B'); + JJ3(1:10,15+6*(kk-1) + 1:15+6*(kk-1) + 6) = AB; + JJ3(15+6*(kk-1) + 1:15+6*(kk-1) + 6,1:10) = (AB)'; + +end; + +JJ3 = JJ3(ind_Jac,ind_Jac); +% Try to make the inversion work: +[U,S,V] = svd(full(JJ3)); +n_reject = length(ind_active); +U = U(:,1:end-n_reject); +S = S(1:end-n_reject,1:end-n_reject); + +JJ2_inv = U * inv(S) * U'; +%JJ2_inv = inv(JJ3); % not bad for sparse matrices!! + +param_error = zeros(6*n_ima+15,1); +param_error(ind_Jac) = 3*sqrt(full(diag(JJ2_inv)))*sigma_x; + +solution_error = param_error; + +if ~est_aspect_ratio & isequal(est_fc,[1;1]), + solution_error(2) = solution_error(1); +end; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters; + +fprintf(1,'done\n'); + + +fprintf(1,'\n\nCalibration results after optimization for weak perspective camera model (with uncertainties):\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ] � [ %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; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_optim_no_read.m b/mr/Ub5/TOOLBOX_calib/go_calib_optim_no_read.m new file mode 100755 index 0000000..057f6b0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_optim_no_read.m @@ -0,0 +1,69 @@ +%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('n_ima'), + data_calib_no_read; % Load the images + click_calib_no_read; % 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 ~exist('rosette_calibration', 'var') + rosette_calibration = 0; +end; + +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; + diff --git a/mr/Ub5/TOOLBOX_calib/go_calib_stereo.m b/mr/Ub5/TOOLBOX_calib/go_calib_stereo.m new file mode 100755 index 0000000..2c3a832 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/go_calib_stereo.m @@ -0,0 +1,385 @@ +% go_calib_stereo.m +% +% Script for Calibrating a stereo rig (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% +% +% Main output variables: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered, that probably +% means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. +% For disabling this process of inconsistent image pairs detection, set the variable 'inconsistent_pairs_detection' to zero + + + +if ~exist('inconsistent_pairs_detection'), + inconsistent_pairs_detection = 1; +end; + + + +if inconsistent_pairs_detection, + %- This threshold is used only to automatically identify non-consistant image pairs (set to Infinity to not reject pairs) + threshold = 50; %1.673; %1e10; %50; +else + threshold = Inf; +end; + + +if ~exist('recompute_intrinsic_right'), + recompute_intrinsic_right = 1; +end; + + +if ~exist('recompute_intrinsic_left'), + recompute_intrinsic_left = 1; +end; + +center_optim_left_st = center_optim_left; +est_alpha_left_st = est_alpha_left; +est_dist_left_st = est_dist_left; +est_fc_left_st = est_fc_left; +est_aspect_ratio_left_st = est_aspect_ratio_left; % just to fix conflicts +center_optim_right_st = center_optim_right; +est_alpha_right_st = est_alpha_right; +est_dist_right_st = est_dist_right; +est_fc_right_st = est_fc_right; +est_aspect_ratio_right_st = est_aspect_ratio_right; % just to fix conflicts + +if ~recompute_intrinsic_left, + fprintf(1,'\nNo recomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 0)\n'); + center_optim_left_st = 0; + est_alpha_left_st = 0; + est_dist_left_st = zeros(5,1); + est_fc_left_st = [0;0]; + est_aspect_ratio_left_st = 1; % just to fix conflicts +else + fprintf(1,'\nRecomputation of the intrinsic parameters of the left camera (recompute_intrinsic_left = 1)\n'); +end; + + +if ~recompute_intrinsic_right, + fprintf(1,'\nNo recomputation of the intrinsic parameters of the right camera (recompute_intrinsic_left = 0)\n'); + center_optim_right_st = 0; + est_alpha_right_st = 0; + est_dist_right_st = zeros(5,1); + est_fc_right_st = [0;0]; + est_aspect_ratio_right_st = 1; % just to fix conflicts +else + fprintf(1,'\nRecomputation of the intrinsic parameters of the right camera (recompute_intrinsic_right = 1)\n'); +end; + +%- Set to zero the entries of the distortion vectors that are not attempted to be estimated. +kc_right = kc_right .* ~~est_dist_right; +kc_left = kc_left .* ~~est_dist_left; + + +active_images = active_images_left & active_images_right; + +history = []; + +fprintf(1,'\nMain stereo calibration optimization procedure - Number of pairs of images: %d\n',length(find(active_images))); +fprintf(1,'Gradient descent iterations: '); + + +MaxIter = 100; +change = 1; +iter = 1; + +while (change > 5e-6)&(iter <= MaxIter), + + + fprintf(1,'%d...',iter); + + % Jacobian: + + J = []; + e = []; + if iter == 1, + e_ref = []; + end; + + param = [fc_left;cc_left;alpha_c_left;kc_left;fc_right;cc_right;alpha_c_right;kc_right;om;T]; + + + for kk = 1:n_ima, + + if active_images(kk), + + % Project the structure onto the left view: + + eval(['Xckk = X_left_' num2str(kk) ';']); + eval(['omckk = omc_left_' num2str(kk) ';']); + eval(['Tckk = Tc_left_' num2str(kk) ';']); + + eval(['xlkk = x_left_' num2str(kk) ';']); + eval(['xrkk = x_right_' num2str(kk) ';']); + + param = [param;omckk;Tckk]; + + % number of points: + Nckk = size(Xckk,2); + + + Jkk = sparse(4*Nckk,20+(1+n_ima)*6); + ekk = zeros(4*Nckk,1); + + + if ~est_aspect_ratio_left, + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left(1),cc_left,kc_left,alpha_c_left); + dxldfl = repmat(dxldfl,[1 2]); + else + [xl,dxldomckk,dxldTckk,dxldfl,dxldcl,dxldkl,dxldalphal] = project_points2(Xckk,omckk,Tckk,fc_left,cc_left,kc_left,alpha_c_left); + end; + + + ekk(1:2*Nckk) = xlkk(:) - xl(:); + + Jkk(1:2*Nckk,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxldomckk); + Jkk(1:2*Nckk,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxldTckk); + + Jkk(1:2*Nckk,1:2) = sparse(dxldfl); + Jkk(1:2*Nckk,3:4) = sparse(dxldcl); + Jkk(1:2*Nckk,5) = sparse(dxldalphal); + Jkk(1:2*Nckk,6:10) = sparse(dxldkl); + + + % Project the structure onto the right view: + + [omr,Tr,domrdomckk,domrdTckk,domrdom,domrdT,dTrdomckk,dTrdTckk,dTrdom,dTrdT] = compose_motion(omckk,Tckk,om,T); + + if ~est_aspect_ratio_right, + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right(1),cc_right,kc_right,alpha_c_right); + dxrdfr = repmat(dxrdfr,[1 2]); + else + [xr,dxrdomr,dxrdTr,dxrdfr,dxrdcr,dxrdkr,dxrdalphar] = project_points2(Xckk,omr,Tr,fc_right,cc_right,kc_right,alpha_c_right); + end; + + + ekk(2*Nckk+1:end) = xrkk(:) - xr(:); + + + dxrdom = dxrdomr * domrdom + dxrdTr * dTrdom; + dxrdT = dxrdomr * domrdT + dxrdTr * dTrdT; + + dxrdomckk = dxrdomr * domrdomckk + dxrdTr * dTrdomckk; + dxrdTckk = dxrdomr * domrdTckk + dxrdTr * dTrdTckk; + + + Jkk(2*Nckk+1:end,1+20:3+20) = sparse(dxrdom); + Jkk(2*Nckk+1:end,4+20:6+20) = sparse(dxrdT); + + + Jkk(2*Nckk+1:end,6*(kk-1)+7+20:6*(kk-1)+7+2+20) = sparse(dxrdomckk); + Jkk(2*Nckk+1:end,6*(kk-1)+7+3+20:6*(kk-1)+7+5+20) = sparse(dxrdTckk); + + Jkk(2*Nckk+1:end,11:12) = sparse(dxrdfr); + Jkk(2*Nckk+1:end,13:14) = sparse(dxrdcr); + Jkk(2*Nckk+1:end,15) = sparse(dxrdalphar); + Jkk(2*Nckk+1:end,16:20) = sparse(dxrdkr); + + + emax = max(abs(ekk)); + + if iter == 1; + e_ref = [e_ref;ekk]; + end; + + + if emax < threshold, + + J = [J;Jkk]; + e = [e;ekk]; + + else + + fprintf(1,'Disabling view %d - Reason: the left and right images are found inconsistent (try help calib_stereo for more information)\n',kk); + + active_images(kk) = 0; + active_images_left(kk) = 0; + active_images_right(kk) = 0; + + end; + + else + + param = [param;NaN*ones(6,1)]; + + end; + + end; + + history = [history param]; + + ind_Jac = find([est_fc_left_st & [1;est_aspect_ratio_left_st];center_optim_left_st*ones(2,1);est_alpha_left_st;est_dist_left_st;est_fc_right_st & [1;est_aspect_ratio_right_st];center_optim_right_st*ones(2,1);est_alpha_right_st;est_dist_right_st;ones(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)]); + + ind_active = find(active_images); + + J = J(:,ind_Jac); + J2 = J'*J; + J2_inv = inv(J2); + + param_update = J2_inv*J'*e; + + + param(ind_Jac) = param(ind_Jac) + param_update; + + fc_left = param(1:2); + cc_left = param(3:4); + alpha_c_left = param(5); + kc_left = param(6:10); + fc_right = param(11:12); + cc_right = param(13:14); + alpha_c_right = param(15); + kc_right = param(16:20); + + + if ~est_aspect_ratio_left_st, + fc_left(2) = fc_left(1); + end; + if ~est_aspect_ratio_right_st, + fc_right(2) = fc_right(1); + end; + + om_old = om; + T_old = T; + + om = param(1+20:3+20); + T = param(4+20:6+20); + + + for kk = 1:n_ima; + + if active_images(kk), + + omckk = param(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_' num2str(kk) ' = omckk;']); + eval(['Tc_left_' num2str(kk) ' = Tckk;']); + + end; + + end; + + change = norm([T;om] - [T_old;om_old])/norm([T;om]); + iter = iter + 1; + +end; + +fprintf(1,'done\n'); + + +history = [history param]; + +inconsistent_images = ~active_images & (active_images_left & active_images_right); + + +%%%--------------------------- Computation of the error of estimation: + +fprintf(1,'Estimation of uncertainties...'); + + +sigma_x = std(e(:)); +param_error = zeros(20 + (1+n_ima)*6,1); +param_error(ind_Jac) = 3*sqrt(full(diag(J2_inv)))*sigma_x; + +for kk = 1:n_ima; + + if active_images(kk), + + omckk_error = param_error(6*(kk-1)+7+20:6*(kk-1)+7+2+20); + Tckk = param_error(6*(kk-1)+7+3+20:6*(kk-1)+7+5+20); + + eval(['omc_left_error_' num2str(kk) ' = omckk;']); + eval(['Tc_left_error_' num2str(kk) ' = Tckk;']); + + else + + eval(['omc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['omc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + eval(['Tc_left_error_' num2str(kk) ' = NaN*ones(3,1);']); + + end; + +end; + +fc_left_error = param_error(1:2); +cc_left_error = param_error(3:4); +alpha_c_left_error = param_error(5); +kc_left_error = param_error(6:10); +fc_right_error = param_error(11:12); +cc_right_error = param_error(13:14); +alpha_c_right_error = param_error(15); +kc_right_error = param_error(16:20); + +if ~est_aspect_ratio_left_st, + fc_left_error(2) = fc_left_error(1); +end; +if ~est_aspect_ratio_right_st, + fc_right_error(2) = fc_right_error(1); +end; + + +om_error = param_error(1+20:3+20); +T_error = param_error(4+20:6+20); + + +KK_left = [fc_left(1) fc_left(1)*alpha_c_left cc_left(1);0 fc_left(2) cc_left(2); 0 0 1]; +KK_right = [fc_right(1) fc_right(1)*alpha_c_right cc_right(1);0 fc_right(2) cc_right(2); 0 0 1]; + + +R = rodrigues(om); + +fprintf(1,'done\n'); + +fprintf(1,'\n\n\nStereo calibration parameters after optimization:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); + + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') +%fprintf(1,'\n\nSuggested threshold = %s\n\n',) + diff --git a/mr/Ub5/TOOLBOX_calib/ima_read_calib.m b/mr/Ub5/TOOLBOX_calib/ima_read_calib.m new file mode 100755 index 0000000..da1ceea --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ima_read_calib.m @@ -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; + diff --git a/mr/Ub5/TOOLBOX_calib/ima_read_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/ima_read_calib_no_read.m new file mode 100755 index 0000000..c4a7102 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/ima_read_calib_no_read.m @@ -0,0 +1,130 @@ + +if ~exist('calib_name')|~exist('format_image'), + data_calib_no_read; + return; +end; + +check_directory; + +if ~exist('n_ima'), + data_calib_no_read; + 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; + +% Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: +for kk = 1:n_ima, + if (exist(['I_' num2str(kk)])==1), + clear(['I_' num2str(kk)]); + end; +end; + + +fprintf(1,'\nChecking directory content for the calibration images (no global image loading in memory efficient mode)\n'); + +one_image_read = 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,'Found images: '); + end; + + if exist(ima_name), + + fprintf(1,'%d...',i); + + + if ~one_image_read + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + + if size(I,1)~=480, + small_calib_image = 1; + else + small_calib_image = 0; + end; + + [Hcal,Wcal] = size(I); % size of the calibration image + + [ny,nx] = size(I); + + one_image_read = 1; + + end; + + + else + + images_read(i) = 0; + + end; + + end; + + i = i+1; + +end; + + +ind_read = find(images_read); + + +if ~(exist('map')==1), map = gray(256); end; + +active_images = images_read; + +fprintf(1,'\ndone\n'); diff --git a/mr/Ub5/TOOLBOX_calib/init_intrinsic_param.m b/mr/Ub5/TOOLBOX_calib/init_intrinsic_param.m new file mode 100755 index 0000000..ad6ab23 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/init_intrinsic_param.m @@ -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); diff --git a/mr/Ub5/TOOLBOX_calib/init_intrinsic_param_fisheye.m b/mr/Ub5/TOOLBOX_calib/init_intrinsic_param_fisheye.m new file mode 100755 index 0000000..203326b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/init_intrinsic_param_fisheye.m @@ -0,0 +1,82 @@ +%init_intrinsic_param_fisheye +% +%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: Fisheye 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)); + +check_active_images; + +% initial guess for principal point and distortion: + +if ~exist('nx'), [ny,nx] = size(I); end; + +f_init = (max(nx,ny) / pi) * ones(2,1); +c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image +k_init = [0;0;0;0]; % initialize to zero (no distortion) + +if ~est_aspect_ratio, + f_init(1) = (f_init(1)+f_init(2))/2; + f_init(2) = f_init(1); +end; + +alpha_init = 0; + +% 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,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); diff --git a/mr/Ub5/TOOLBOX_calib/inverse_motion.m b/mr/Ub5/TOOLBOX_calib/inverse_motion.m new file mode 100755 index 0000000..5d605e2 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/inverse_motion.m @@ -0,0 +1,45 @@ +function [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); + +% This function computes the inverse motion corresponding to (om,T) + + +om2 = -om; +dom2dom = -eye(3); +dom2dT = zeros(3,3); + + +[R,dRdom] = rodrigues(om); +Rinv = R'; +dRinvdR = zeros(9,9); +dRinvdR([1 4 7],[1 2 3]) = eye(3); +dRinvdR([2 5 8],[4 5 6]) = eye(3); +dRinvdR([3 6 9],[7 8 9]) = eye(3); +dRinvdom = dRinvdR * dRdom; + +Tt = Rinv * T; +[dTtdRinv,dTtdT] = dAB(Rinv,T); + +T2 = -Tt; + +dT2dom = - dTtdRinv * dRinvdom; +dT2dT = - dTtdT; + + +return; + +% Test of the Jacobians: + +om = randn(3,1); +T = 10*randn(3,1); +[om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); + +dom = randn(3,1) / 100000; +dT = randn(3,1) / 100000; + +[om3r,T3r] = inverse_motion(om+dom,T+dT); + +om3p = om2 + dom2dom*dom + dom2dT*dT; +T3p = T2 + dT2dom*dom + dT2dT*dT; + +%norm(om3r - om2) / norm(om3r - om3p) %-> Leads to infinity, since the opreation is linear! +norm(T3r - T2) / norm(T3r - T3p) diff --git a/mr/Ub5/TOOLBOX_calib/is3D.m b/mr/Ub5/TOOLBOX_calib/is3D.m new file mode 100755 index 0000000..ab00b3d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/is3D.m @@ -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); + diff --git a/mr/Ub5/TOOLBOX_calib/load_image.m b/mr/Ub5/TOOLBOX_calib/load_image.m new file mode 100755 index 0000000..9797f21 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/load_image.m @@ -0,0 +1,41 @@ +function I = load_image(kk , calib_name , format_image , type_numbering , image_numbers , N_slots), + + +if ~type_numbering, + number_ext = num2str(image_numbers(kk)); +else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); +end; + + +ima_name = [calib_name number_ext '.' format_image]; + +if ~exist(ima_name), + + fprintf(1,'Image %s not found!!!\n',ima_name); + I = NaN; + +else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + +end; diff --git a/mr/Ub5/TOOLBOX_calib/load_stereo_calib_files.m b/mr/Ub5/TOOLBOX_calib/load_stereo_calib_files.m new file mode 100755 index 0000000..3618186 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/load_stereo_calib_files.m @@ -0,0 +1,245 @@ + +dir('*mat'); + +fprintf(1,'Loading of the individual left and right camera calibration files\n'); + +calib_file_name_left = input('Name of the left camera calibration file ([]=Calib_Results_left.mat): ','s'); + +if isempty(calib_file_name_left), + calib_file_name_left = 'Calib_Results_left.mat'; +end; + + +calib_file_name_right = input('Name of the right camera calibration file ([]=Calib_Results_right.mat): ','s'); + +if isempty(calib_file_name_right), + calib_file_name_right = 'Calib_Results_right.mat'; +end; + + +if (exist(calib_file_name_left)~=2)|(exist(calib_file_name_right)~=2), + fprintf(1,'Error: left and right calibration files do not exist.\n'); + return; +end; + + +fprintf(1,'Loading the left camera calibration result file %s...\n',calib_file_name_left); + +clear calib_name + +load(calib_file_name_left); + +fc_left = fc; +cc_left = cc; +kc_left = kc; +alpha_c_left = alpha_c; +fc_left_error = fc_error; +cc_left_error = cc_error; +kc_left_error = kc_error; +alpha_c_left_error = alpha_c_error; +KK_left = KK; + +if exist('calib_name'), + calib_name_left = calib_name; + format_image_left = format_image; + type_numbering_left = type_numbering; + image_numbers_left = image_numbers; + N_slots_left = N_slots; +else + calib_name_left = ''; + format_image_left = ''; + type_numbering_left = ''; + image_numbers_left = ''; + N_slots_left = ''; +end; + + +X_left = []; + + +om_left_list = []; +T_left_list = []; + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_left = [X_left Xckk]; + + om_left_list = [om_left_list omckk]; + + T_left_list = [T_left_list Tckk]; + + end; +end; + + + +fprintf(1,'Loading the right camera calibration result file %s...\n',calib_file_name_right); + +clear calib_name + +load(calib_file_name_right); + +fc_right = fc; +cc_right = cc; +kc_right = kc; +alpha_c_right = alpha_c; +KK_right = KK; +fc_right_error = fc_error; +cc_right_error = cc_error; +kc_right_error = kc_error; +alpha_c_right_error = alpha_c_error; + +if exist('calib_name'), + calib_name_right = calib_name; + format_image_right = format_image; + type_numbering_right = type_numbering; + image_numbers_right = image_numbers; + N_slots_right = N_slots; +else + calib_name_right = ''; + format_image_right = ''; + type_numbering_right = ''; + image_numbers_right = ''; + N_slots_right = ''; +end; + +X_right = []; + +om_right_list = []; +T_right_list = []; + + +for kk = 1:n_ima, + + if active_images(kk), + + eval(['Xkk = X_' num2str(kk) ';']); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Rckk = Rc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + N = size(Xkk,2); + + Xckk = Rckk * Xkk + Tckk*ones(1,N); + + X_right = [X_right Xckk]; + + om_right_list = [om_right_list omckk]; + T_right_list = [T_right_list Tckk]; + + end; +end; + + + + +om_ref_list = []; +T_ref_list = []; +for ii = 1:size(om_left_list,2), + % Align the structure from the first view: + R_ref = rodrigues(om_right_list(:,ii)) * rodrigues(om_left_list(:,ii))'; + T_ref = T_right_list(:,ii) - R_ref * T_left_list(:,ii); + om_ref = rodrigues(R_ref); + om_ref_list = [om_ref_list om_ref]; + T_ref_list = [T_ref_list T_ref]; +end; + + +% Robust estimate of the initial value for rotation and translation between the two views: +om = median(om_ref_list,2); +T = median(T_ref_list,2); + + + + +if 0, + figure(10); + plot3(X_right(1,:),X_right(2,:),X_right(3,:),'bo'); + hold on; + [Xr2] = rigid_motion(X_left,om,T); + plot3(Xr2(1,:),Xr2(2,:),Xr2(3,:),'r+'); + hold off; + drawnow; +end; + + +R = rodrigues(om); + + + +% Re-optimize now over all the set of extrinsic unknows (global optimization) and intrinsic parameters: + +load(calib_file_name_left); % Calib_Results_left; + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_left_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_left_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['omc_left_' num2str(kk) ' = omc_' num2str(kk) ';']); + eval(['Rc_left_' num2str(kk) ' = Rc_' num2str(kk) ';']); + eval(['Tc_left_' num2str(kk) ' = Tc_' num2str(kk) ';']); + end; +end; + +center_optim_left = center_optim; +est_alpha_left = est_alpha; +est_dist_left = est_dist; +est_fc_left = est_fc; +est_aspect_ratio_left = est_aspect_ratio; +active_images_left = active_images; + + +load(calib_file_name_right); + +for kk = 1:n_ima, + if active_images(kk), + eval(['X_right_' num2str(kk) ' = X_' num2str(kk) ';']); + eval(['x_right_' num2str(kk) ' = x_' num2str(kk) ';']); + end; +end; + +center_optim_right = center_optim; +est_alpha_right = est_alpha; +est_dist_right = est_dist; +est_fc_right = est_fc; +est_aspect_ratio_right = est_aspect_ratio; +active_images_right = active_images; + + +active_images = active_images_left & active_images_right; + + + +fprintf(1,'\n\n\nStereo calibration parameters after loading the individual calibration files:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ]\n',om); +fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ]\n',T); + + diff --git a/mr/Ub5/TOOLBOX_calib/loading_calib.m b/mr/Ub5/TOOLBOX_calib/loading_calib.m new file mode 100755 index 0000000..a0f50d2 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/loading_calib.m @@ -0,0 +1,10 @@ +if ~exist('Calib_Results.mat'), + fprintf(1,'\nCalibration file Calib_Results.mat not found!\n'); + return; +end; + +fprintf(1,'\nLoading calibration results from Calib_Results.mat\n'); + +load Calib_Results + +fprintf(1,'done\n'); diff --git a/mr/Ub5/TOOLBOX_calib/loading_stereo_calib.m b/mr/Ub5/TOOLBOX_calib/loading_stereo_calib.m new file mode 100755 index 0000000..018c903 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/loading_stereo_calib.m @@ -0,0 +1,7 @@ +if exist('Calib_Results_stereo.mat')~=2, + fprintf(1,'\nStereo calibration file Calib_Results_stereo.mat not found!\n'); + return; +end; + +fprintf(1,'Loading stereo calibration results from Calib_Results_stereo.mat...\n'); +load('Calib_Results_stereo.mat'); diff --git a/mr/Ub5/TOOLBOX_calib/loadinr.m b/mr/Ub5/TOOLBOX_calib/loadinr.m new file mode 100755 index 0000000..91b6f89 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/loadinr.m @@ -0,0 +1,52 @@ +%LOADINR Load an INRIMAGE format file +% +% LOADINR(filename, im) +% +% Load an INRIA image format file and return it as a matrix +% +% SEE ALSO: saveinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1996 + +function im = loadinr(fname, im) + + fid = fopen(fname, 'r'); + + s = fgets(fid); + if strcmp(s(1:12), '#INRIMAGE-4#') == 0, + error('not INRIMAGE format'); + end + + % not very complete, only looks for the X/YDIM keys + while 1, + s = fgets(fid); + n = length(s) - 1; + if s(1) == '#', + break + end + if strcmp(s(1:5), 'XDIM='), + cols = str2num(s(6:n)); + end + if strcmp(s(1:5), 'YDIM='), + rows = str2num(s(6:n)); + end + if strcmp(s(1:4), 'CPU='), + if strcmp(s(5:n), 'sun') == 0, + error('not sun data ordering'); + end + end + + end + disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)]) + + % now the binary data + fseek(fid, 256, 'bof'); + [im count] = fread(fid, [cols rows], 'float32'); + im = im'; + if count ~= (rows*cols), + error('file too short'); + end + fclose(fid); diff --git a/mr/Ub5/TOOLBOX_calib/loadpgm.m b/mr/Ub5/TOOLBOX_calib/loadpgm.m new file mode 100755 index 0000000..dfa8b61 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/loadpgm.m @@ -0,0 +1,89 @@ +%LOADPGM Load a PGM image +% +% I = loadpgm(filename) +% +% Returns a matrix containing the image loaded from the PGM format +% file filename. Handles ASCII (P2) and binary (P5) PGM file formats. +% +% If the filename has no extension, and open fails, a '.pgm' will +% be appended. +% +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadpgm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.pgm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + if magic(1) == 'P', + if magic(2) == '2', + %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols rows])'; + elseif magic(2) == '5', + %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols rows], fmt)'; + else + disp('Not a PGM file'); + end + end + fclose(fid); diff --git a/mr/Ub5/TOOLBOX_calib/loadppm.m b/mr/Ub5/TOOLBOX_calib/loadppm.m new file mode 100755 index 0000000..c527b61 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/loadppm.m @@ -0,0 +1,115 @@ +%LOADPPM Load a PPM image +% +% I = loadppm(filename) +% +% Returns a matrix containing the image loaded from the PPM format +% file filename. Handles ASCII (P3) and binary (P6) PPM file formats. +% +% If the filename has no extension, and open fails, a '.ppm' and +% '.pnm' extension will be tried. +% +% SEE ALSO: saveppm loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadppm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.ppm'], 'r'); + end + if fid < 0, + fid = fopen([file '.pnm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + + % assume a carriage return only: + + c = fread(fid,1,'char'); + + % bug: because the image might be starting with special characters! + %while 1 + % c = fread(fid,1,'char'); + % if c == '#', + % fgetl(fid); + % elseif ~any(c == white) + % fseek(fid, -1, 'cof'); % unputc() + % break; + % end + %end + if magic(1) == 'P', + if magic(2) == '3', + %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols*3 rows]); + elseif magic(2) == '6', + %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols*3 rows], fmt); + else + disp('Not a PPM file'); + end + end + % + % now the matrix has interleaved columns of R, G, B + % + I = I'; + size(I); + R = I(:,1:3:(cols*3)); + G = I(:,2:3:(cols*3)); + B = I(:,3:3:(cols*3)); + fclose(fid); + + + I = zeros(rows,cols,3); + I(:,:,1) = R; + I(:,:,2) = G; + I(:,:,3) = B; + I = uint8(I); + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/manual_corner_extraction.m b/mr/Ub5/TOOLBOX_calib/manual_corner_extraction.m new file mode 100755 index 0000000..d3503d8 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/manual_corner_extraction.m @@ -0,0 +1,141 @@ +%% This code allows complete manual reselection of every corner in the +%% images. +%% This tool is specifically useful in the case of highly distorted images. +%% +%% Use it when in standard mode. +%% In memory efficient mode, use manual_corner_extraction_no_read.m + + +if ~exist('n_ima'), + fprintf(1,'No image data available\n'); + return; +end; + +check_active_images; + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +if ~exist(['I_' num2str(ind_active(1))]), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +fprintf(1,'\nManual re-extraction of the grid corners on the images\n'); + +q_converge = input('Do you want to try to automatically find the closest corner? - only works with ckecker board corners ([]=yes, other = no) ','s'); + +if isempty(q_converge), + q_converge = 1; + fprintf(1,'Automatic refinement of the corner location after manual mouse click\n'); + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); +else + q_converge = 0; + fprintf(1,'No attempt to refine the corner location after manual mouse click\n'); +end; + + + + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + eval(['I = I_' num2str(kk) ';']); + + eval(['x = x_' num2str(kk) ';']); + + Np = size(x,2); + + + figure(2); + image(I); + colormap(map); + hold on; + hx = plot(x(1,:)+1,x(2,:)+1,'r+'); + hcp = plot(x(1,1)+1,x(2,1)+1,'co'); + hold off; + + for np = 1:Np, + + set(hcp,'Xdata',x(1,np)+1,'Ydata',x(2,np)+1); + + + title(['Click on corner #' num2str(np) ' out of ' num2str(Np) ' (right button: keep point unchanged)']); + + [xi,yi,b] = ginput4(1); + + if b==1, + xxi = [xi;yi]; + if q_converge, + [xxi] = cornerfinder(xxi,I,winty,wintx); + end; + x(1,np) = xxi(1) - 1; + x(2,np) = xxi(2) - 1; + set(hx,'Xdata',x(1,:)+1,'Ydata',x(2,:)+1); + end; + + end; + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= x;']); + + else + + if ~exist(['omc_' num2str(kk)]), + + 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; + + +end; + +fprintf(1,'\ndone\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/manual_corner_extraction_no_read.m b/mr/Ub5/TOOLBOX_calib/manual_corner_extraction_no_read.m new file mode 100755 index 0000000..b75d574 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/manual_corner_extraction_no_read.m @@ -0,0 +1,175 @@ +%% This code allows complete manual reselection of every corner in the +%% images. +%% This tool is specifically useful in the case of highly distorted images. +%% +%% Use it when in memory efficient mode. +%% In standard mode, use manual_corner_extraction.m + + +if ~exist('n_ima'), + fprintf(1,'No image data available\n'); + return; +end; + +check_active_images; + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +%if ~exist(['I_' num2str(ind_active(1))]), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% disp('Cannot extract corners without images'); +% return; +% end; +%end; + +fprintf(1,'\nManual re-extraction of the grid corners on the images\n'); + +q_converge = input('Do you want to try to automatically find the closest corner? - only works with ckecker board corners ([]=yes, other = no)','s'); + +if isempty(q_converge), + q_converge = 1; + fprintf(1,'Automatic refinement of the corner location after manual mouse click\n'); + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); +else + q_converge = 0; + fprintf(1,'No attempt to refine the corner location after manual mouse click\n'); +end; + + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if exist(ima_name), + + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + + eval(['x = x_' num2str(kk) ';']); + + Np = size(x,2); + + + figure(2); + image(I); + colormap(map); + hold on; + hx = plot(x(1,:)+1,x(2,:)+1,'r+'); + hcp = plot(x(1,1)+1,x(2,1)+1,'co'); + hold off; + + for np = 1:Np, + + set(hcp,'Xdata',x(1,np)+1,'Ydata',x(2,np)+1); + + + title(['Click on corner #' num2str(np) ' out of ' num2str(Np) ' (right button: keep point unchanged)']); + + [xi,yi,b] = ginput4(1); + + if b==1, + xxi = [xi;yi]; + if q_converge, + [xxi] = cornerfinder(xxi,I,winty,wintx); + end; + x(1,np) = xxi(1) - 1; + x(2,np) = xxi(2) - 1; + set(hx,'Xdata',x(1,:)+1,'Ydata',x(2,:)+1); + end; + + end; + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= x;']); + + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + + + else + + if ~exist(['omc_' num2str(kk)]), + + 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; + + +end; + +fprintf(1,'\ndone\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/mean_std_robust.m b/mr/Ub5/TOOLBOX_calib/mean_std_robust.m new file mode 100755 index 0000000..0d18a62 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/mean_std_robust.m @@ -0,0 +1,7 @@ +function [m,s] = mean_std_robust(x); + +x = x(:); + +m = median(x); + +s = median(abs(x - m))*1.4836; diff --git a/mr/Ub5/TOOLBOX_calib/merge_calibration_sets.m b/mr/Ub5/TOOLBOX_calib/merge_calibration_sets.m new file mode 100755 index 0000000..dc84190 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/merge_calibration_sets.m @@ -0,0 +1,85 @@ + +set1 = load(data_set1); % part1\Calib_Results; +set2 = load(data_set2); % part2\Calib_Results; + +shift = set1.n_ima; + +for kk = 1:set1.n_ima + +eval(['X_' num2str(kk) ' = set1.X_' num2str(kk) ';']); + +eval(['dX_' num2str(kk) ' = set1.dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk) ' = set1.dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk) ' = set1.wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk) ' = set1.winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk) ' = set1.x_' num2str(kk) ';']); + +if isfield(set1,'y') + eval(['y_' num2str(kk) ' = set1.y_' num2str(kk) ';']); +else + eval(['y_' num2str(kk) ' = [NaN];']); +end; + +eval(['n_sq_x_' num2str(kk) ' = set1.n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk) ' = set1.n_sq_y_' num2str(kk) ';']); + + +if isfield(set1,['omc_' num2str(kk+shift)]) + eval(['omc_' num2str(kk+shift) ' = set1.omc_' num2str(kk) ';']); + eval(['Tc_' num2str(kk+shift) ' = set1.Tc_' num2str(kk) ';']); +else + eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); + eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); +end; + +end; + +for kk = 1:set2.n_ima + +eval(['X_' num2str(kk+shift) ' = set2.X_' num2str(kk) ';']); + + +eval(['dX_' num2str(kk+shift) ' = set2.dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk+shift) ' = set2.dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk+shift) ' = set2.wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk+shift) ' = set2.winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk+shift) ' = set2.x_' num2str(kk) ';']); + +if isfield(set2,'y') + eval(['y_' num2str(kk) ' = set2.y_' num2str(kk) ';']); +else + eval(['y_' num2str(kk) ' = [NaN];']); +end; + +eval(['n_sq_x_' num2str(kk+shift) ' = set2.n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk+shift) ' = set2.n_sq_y_' num2str(kk) ';']); + + + +if isfield(set2,['omc_' num2str(kk+shift)]) + eval(['omc_' num2str(kk+shift) ' = set2.omc_' num2str(kk) ';']); + eval(['Tc_' num2str(kk+shift) ' = set2.Tc_' num2str(kk) ';']); +else + eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); + eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); +end; + +end; + + +fc = set2.fc; +kc = set2.kc; +cc = set2.cc; +alpha_c = set2.alpha_c; +KK = set2.KK; +inv_KK = set2.inv_KK; + + +n_ima = set1.n_ima + set2.n_ima; +active_images = [set1.active_images set2.active_images]; + +no_image = 1; diff --git a/mr/Ub5/TOOLBOX_calib/merge_two_datasets.m b/mr/Ub5/TOOLBOX_calib/merge_two_datasets.m new file mode 100755 index 0000000..dd3ac02 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/merge_two_datasets.m @@ -0,0 +1,68 @@ +fprintf(1,'Script that merges two "Cabib_Results.mat" data sets of the same camera into a single dataset\n') + +dir; + + +cont = 1; +while cont + data_set1 = input('Filename of the first dataset (with complete path if necessary): ','s'); + cont = ((exist(data_set1)~=2)); + if cont, + fprintf(1,'File not found. Try again.\n'); + end; +end; +cont = 1; +while cont + data_set2 = input('Filename of the second dataset (with complete path if necessary): ','s'); + cont = ((exist(data_set2)~=2)); + if cont, + fprintf(1,'File not found. Try again.\n'); + end; +end; + + +load(data_set1); % part1\Calib_Results; + +shift = n_ima; + +load(data_set2); % part2\Calib_Results; + +active_images2 = active_images; +n_ima2 = n_ima; + + +for kk = 1:n_ima + +eval(['X_' num2str(kk+shift) ' = X_' num2str(kk) ';']); + + +eval(['dX_' num2str(kk+shift) ' = dX_' num2str(kk) ';']); +eval(['dY_' num2str(kk+shift) ' = dY_' num2str(kk) ';']); + +eval(['wintx_' num2str(kk+shift) ' = wintx_' num2str(kk) ';']); +eval(['winty_' num2str(kk+shift) ' = winty_' num2str(kk) ';']); + +eval(['x_' num2str(kk+shift) ' = x_' num2str(kk) ';']); +eval(['y_' num2str(kk+shift) ' = y_' num2str(kk) ';']); + +eval(['n_sq_x_' num2str(kk+shift) ' = n_sq_x_' num2str(kk) ';']); +eval(['n_sq_y_' num2str(kk+shift) ' = n_sq_y_' num2str(kk) ';']); + + +eval(['omc_' num2str(kk+shift) ' = omc_' num2str(kk) ';']); +eval(['Tc_' num2str(kk+shift) ' = Tc_' num2str(kk) ';']); + +end; + +load(data_set1); % part1\Calib_Results; + +n_ima = n_ima + n_ima2; +active_images = [active_images active_images2]; + +no_image = 1; + +% Recompute the error (in the vector ex): +comp_error_calib; + +fprintf('The two calibration datasets are now merged. You are now ready to run calibration. \n'); + diff --git a/mr/Ub5/TOOLBOX_calib/mosaic.m b/mr/Ub5/TOOLBOX_calib/mosaic.m new file mode 100755 index 0000000..37c916c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/mosaic.m @@ -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'); + diff --git a/mr/Ub5/TOOLBOX_calib/mosaic_no_read.m b/mr/Ub5/TOOLBOX_calib/mosaic_no_read.m new file mode 100755 index 0000000..ad013d8 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/mosaic_no_read.m @@ -0,0 +1,118 @@ + + +if ~exist('n_ima'), + data_calib_no_read; +end; + + +check_active_images; + +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; + +ny2 = length(1:n_col:ny); +nx2 = length(1:n_col:nx); + +%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 (kk_c <= n_ima), + + if ~type_numbering, + number_ext = num2str(image_numbers(kk_c)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk_c)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + + if (exist(ima_name)) & (kk_c <= n_ima), + + if active_images(kk_c), + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + %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; + + 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'); + diff --git a/mr/Ub5/TOOLBOX_calib/normalize.m b/mr/Ub5/TOOLBOX_calib/normalize.m new file mode 100755 index 0000000..c293f86 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/normalize.m @@ -0,0 +1,47 @@ +function [xn] = normalize(x_kk,fc,cc,kc,alpha_c) + +%normalize +% +%[xn] = normalize(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; diff --git a/mr/Ub5/TOOLBOX_calib/normalize2.m b/mr/Ub5/TOOLBOX_calib/normalize2.m new file mode 100755 index 0000000..d3d5604 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/normalize2.m @@ -0,0 +1,73 @@ +function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c), + +%normalize +% +%[xn] = normalize(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: + +k1 = kc(1); +k2 = kc(2); +k3 = kc(5); +p1 = kc(3); +p2 = kc(4); + +N = size(x_kk,2); + +% 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)]; + + +v1 = - x_distort(1,:) / fc(1); +v2 = - x_distort(2,:) / fc(1); + +dx_distortdfc = zeros(2*N,2); +dx_distortdfc(1:2:end,1) = v1'; +dx_distortdfc(2:2:end,2) = v2'; + +v1 = - x_distort(1,:) / fc(1); +v2 = - x_distort(2,:) / fc(1); + +dx_distortdcc = zeros(2*N,2); +dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1); +dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1); + +% Second: undo skew +x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); + +dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2) dx_distortdfc(:,2)]; +dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2) dx_distortdcc(:,2)]; + +dx_distort2dalpha_c = zeros(2*N,1); +dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)'; + +x = x_distort; % 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 = (x_distort - delta_x)./(ones(2,1)*k_radial); + +end; + + +xn = x; + + +dxdk = zeros(2*N,5); % Approximation (no time) +dxdf = dx_distort2dfc; +dxdc = dx_distort2dcc; +dxdalpha = dx_distort2dalpha_c; diff --git a/mr/Ub5/TOOLBOX_calib/normalize_pixel.m b/mr/Ub5/TOOLBOX_calib/normalize_pixel.m new file mode 100755 index 0000000..467fe14 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/normalize_pixel.m @@ -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; diff --git a/mr/Ub5/TOOLBOX_calib/normalize_pixel_fisheye.m b/mr/Ub5/TOOLBOX_calib/normalize_pixel_fisheye.m new file mode 100755 index 0000000..e3dd632 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/normalize_pixel_fisheye.m @@ -0,0 +1,40 @@ +function [xn] = normalize_pixel_fisheye(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: Fisheye distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) + +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,:); + +% Third: Compensate for lens distortion: +xn = comp_fisheye_distortion(x_distort,kc); + diff --git a/mr/Ub5/TOOLBOX_calib/pattern.eps b/mr/Ub5/TOOLBOX_calib/pattern.eps new file mode 100755 index 0000000..bcdd8af --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/pattern.eps Binary files differ diff --git a/mr/Ub5/TOOLBOX_calib/pgmread.m b/mr/Ub5/TOOLBOX_calib/pgmread.m new file mode 100755 index 0000000..c96ccb7 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/pgmread.m @@ -0,0 +1,26 @@ +function img = pgmread(filename) +% function img = pgmread(filename) +% this is my version of pgmread for the pgm file created by XV. +% +% this program also corrects for the shifts in the image from pm file. + +fid = fopen(filename,'r'); +fscanf(fid, 'P5\n'); +cmt = '#'; +while findstr(cmt, '#'), + cmt = fgets(fid); + if length(findstr(cmt, '#')) ~= 1, + YX = sscanf(cmt, '%d %d'); + y = YX(1); x = YX(2); + end +end + +%fgets(fid); + +%img = fscanf(fid,'%d',size); +%img = img'; + +img = fread(fid,[y,x],'uint8'); +img = img'; +fclose(fid); + diff --git a/mr/Ub5/TOOLBOX_calib/point_distribution.m b/mr/Ub5/TOOLBOX_calib/point_distribution.m new file mode 100755 index 0000000..1b72409 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/point_distribution.m @@ -0,0 +1,45 @@ +% Point Distribution + +colors = 'brgkcm'; + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +figure(6); + +for kk=1:n_ima + + if exist(['x_' num2str(kk)]), + + if active_images(kk) & eval(['~isnan(x_' num2str(kk) '(1,1))']), + + eval(['plot(x_' num2str(kk) '(1,:),x_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + + end; + +end; + +axis('equal'); + +axis([0 nx 0 ny]); + +title1=pwd; +title1=strrep(title1,'_','\_'); + +title({'Point Distribution in Images',title1}); + +xlabel('x'); + +ylabel('y'); diff --git a/mr/Ub5/TOOLBOX_calib/project2_oulu.m b/mr/Ub5/TOOLBOX_calib/project2_oulu.m new file mode 100755 index 0000000..c5c4a34 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project2_oulu.m @@ -0,0 +1,53 @@ +function [x] = project2_oulu(X,R,T,f,t,k) +%PROJECT Subsidiary to calib + +% (c) Pietro Perona -- March 24, 1994 +% California Institute of Technology +% Pasadena, CA +% +% Renamed because project exists in matlab 5.2!!! +% Now uses the more elaborate intrinsic model from Oulu + + + +[m,n] = size(X); + +Y = R*X + T*ones(1,n); +Z = Y(3,:); + +f = f(:); %% make a column vector +if length(f)==1, + f = [f f]'; +end; + +x = (Y(1:2,:) ./ (ones(2,1) * Z)) ; + + +radius_2 = x(1,:).^2 + x(2,:).^2; + +if length(k) > 1, + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2)); + + if length(k) < 4, + + delta_x = zeros(2,n); + + else + + delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ; + k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)]; + + end; + + +else + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2)); + + delta_x = zeros(2,n); + +end; + + +x = (x .* radial_distortion + delta_x).* (f * ones(1,n)) + t*ones(1,n); diff --git a/mr/Ub5/TOOLBOX_calib/project_points.m b/mr/Ub5/TOOLBOX_calib/project_points.m new file mode 100755 index 0000000..ccbac4c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project_points.m @@ -0,0 +1,282 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) + +%project_points.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) +% +%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) +% +%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) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion, the right terms correspond to tangential distortion +% +%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*xx + 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 < 6, + k = zeros(4,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; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + + +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); + + +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,:); + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + + + +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,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom; +dcdistdT = k(1) * dr2dT+ k(2) * dr4dT; +dcdistdk = [ r2' r4' zeros(n,2)]; + + +xd1 = x .* (ones(2,1)*cdist); + +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,4); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; + + + +% 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); + +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,4); +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'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Pixel coordinates: +if length(f)>1, + xp = xd2 .* (f * ones(1,n)) + c*ones(1,n); + coeff = reshape(f*ones(1,n),2*n,1); + dxpdom = (coeff*ones(1,3)) .* dxd2dom; + dxpdT = (coeff*ones(1,3)) .* dxd2dT; + dxpdk = (coeff*ones(1,5)) .* dxd2dk; + dxpdf = zeros(2*n,2); + dxpdf(1:2:end,1) = xd2(1,:)'; + dxpdf(2:2:end,2) = xd2(2,:)'; +else + xp = f * xd2 + c*ones(1,n); + dxpdom = f * dxd2dom; + dxpdT = f * dxd2dT; + dxpdk = f * dxd2dk; + dxpdf = xd2(:); +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + + + +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(4,1); + + +[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k); + + +% Test on om: OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points(X,om2,T,f,c,k); + +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_points(X,om,T2,f,c,k); + +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_points(X,om,T,f2,c,k); + +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_points(X,om,T,f,c2,k); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(4)*randn(4,1); +k2 = k + dk; + +[x2] = project_points(X,om,T,f,c,k2); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/mr/Ub5/TOOLBOX_calib/project_points2.m b/mr/Ub5/TOOLBOX_calib/project_points2.m new file mode 100755 index 0000000..24918ec --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project_points2.m @@ -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) diff --git a/mr/Ub5/TOOLBOX_calib/project_points3.m b/mr/Ub5/TOOLBOX_calib/project_points3.m new file mode 100755 index 0000000..e44742b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project_points3.m @@ -0,0 +1,375 @@ +function [xp,dxpdX,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points3(X,om,T,f,c,k,alpha) + +%[xp,dxpdX,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points3(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +%Same as project_points2, with the derivative with respect to the shape X (dxpdX) included. +%This matrix is the most hideous thing to compute. +% +%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) +% dxpdX: Derivative of xp with respect to X (sparse (2N)x(3N) matrix) +% 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_motion2.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); + + +% Rigid motion: + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + +% The derivative of Y with respect to X -> just a series of R matrices (needed for later) +R = rodrigues(om); +dYdX = repmat(R,1,n); %[reshape(R(1,:)'*ones(1,n),3*n,1)';reshape(R(2,:)'*ones(1,n),3*n,1)';reshape(R(3,:)'*ones(1,n),3*n,1)']; + +%keyboard; + +% Pinehole projection: + +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); + + +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,:); + +AA_temp = (ones(3,1)*reshape(ones(3,1)*inv_Z,n*3,1)') .* dYdX; % product of R and inv_Z + +dxdX = [AA_temp(1,:) - (reshape(ones(3,1)*x(1,:),3*n,1)'.* AA_temp(3,:)); +AA_temp(2,:) - (reshape(ones(3,1)*x(2,:),3*n,1)'.* AA_temp(3,:))]; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +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,:); + +dr2dX = sum(2*([reshape(ones(3,1)*x(1,:),3*n,1)';reshape(ones(3,1)*x(2,:),3*n,1)']).*dxdX); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + +dr4dX = 2*reshape(ones(3,1)*r2,3*n,1)' .* dr2dX; + + +r6 = r2.^3; + +dr6dom = 3*(r4'*ones(1,3)) .* dr2dom; +dr6dT = 3*(r4'*ones(1,3)) .* dr2dT; +dr6dX = 2*reshape(ones(3,1)*r4,3*n,1)' .* dr2dX; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +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']; +dcdistdX = k(1) * dr2dX + k(2) * dr4dX + k(5) * dr6dX; + + +xd1 = x .* (ones(2,1)*cdist); + +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; + +dxd1dX = [dcdistdX .* (reshape(ones(3,1)*x(1,:),3*n,1)');dcdistdX .* (reshape(ones(3,1)*x(2,:),3*n,1)')] + (ones(2,1)*reshape(ones(3,1)*cdist,3*n,1)').*dxdX; + + + +% 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); + +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'; + + +ddelta_xdX = [2*k(3)*(dxdX(1,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)') + dxdX(2,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)')) + ... + k(4) * (6 * (dxdX(1,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)')) + 2 * (dxdX(2,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)'))) ; +k(3) * (6* (dxdX(2,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)')) + 2*(dxdX(1,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)'))) + ... + 2* k(4)* (dxdX(1,:) .* (reshape(ones(3,1)*x(2,:),3*n,1)') + dxdX(2,:) .* (reshape(ones(3,1)*x(1,:),3*n,1)'))]; + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; +dxd2dX = dxd1dX + ddelta_xdX ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +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,:)'; + + +dxd3dX = [dxd2dX(1,:) + alpha*dxd2dX(2,:); dxd2dX(2,:)]; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + 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,:)'; + dxpdX2 = [f(1)*dxd3dX(1,:);f(2)*dxd3dX(2,:)]; +else + xp = f * xd3 + c*ones(1,n); + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); + dxpdX2 = f * dxd3dX; +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +% Make a sparse matrix out of the Jacobian matrix dxpdX (to make it valid for matrix +% multiplication): + +II = reshape(reshape([1:2:2*n 2:2:2*n]'*ones(1,3),n,6)',6*n,1); +JJ = reshape(ones(2,1)*[1:3*n],6*n,1); +dxpdX = sparse(II,JJ,dxpdX2(:),2*n,3*n, 6*n); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n) + [zeros(2,n);50*ones(1,n)]; +om = 0.1*randn(3,1); +T = 0.1*(10*randn(3,1) + [0;0;40]); +f = 300 + 200*rand(2,1); +c = 1000*rand(2,1); +k = 0.5*randn(5,1); +alpha = 0.01*randn(1,1); + +[x,dxdX,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points3(X,om,T,f,c,k,alpha); + + +% Test on X: + +dX = 0.0001 *randn(3,n); +X2 = X + dX; + +[x2] = project_points2(X2,om,T,f,c,k,alpha); + +x_pred = x + reshape(dxdX * dX(:),2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on om: + +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.00001 * 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) diff --git a/mr/Ub5/TOOLBOX_calib/project_points_fisheye.m b/mr/Ub5/TOOLBOX_calib/project_points_fisheye.m new file mode 100755 index 0000000..8e4548d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project_points_fisheye.m @@ -0,0 +1,330 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha) + +%project_points2.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_fisheye(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane of a fisheye camera. +% +%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 fisheye coefficients (5x1 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)x5 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, +%call theta = atan(r), +%Fisheye distortion -> theta_d = theta * (1 + k(1)*theta^2 + k(2)*theta^4 + k(3)*theta^6 + k(4)*theta^8) +% +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = (theta_d / r) * x +%yy = (theta_d / r) * y +% +%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(4,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 fisheye 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; + +% Radial distance: +r = sqrt(r2); +if nargout > 1, + drdr2 = ones(1,length(r)); + drdr2(r>1e-8) = 1 ./ (2*r(r>1e-8)); + + drdom = [ (drdr2').*dr2dom(:,1) (drdr2').*dr2dom(:,2) (drdr2').*dr2dom(:,3) ]; + drdT = [ (drdr2').*dr2dT(:,1) (drdr2').*dr2dT(:,2) (drdr2').*dr2dT(:,3) ]; +end; + +% Angle of the incoming ray: +theta = atan(r); +if nargout > 1, + dthetadr = 1 ./ (1 + r2); + + dthetadom = [ (dthetadr').*drdom(:,1) (dthetadr').*drdom(:,2) (dthetadr').*drdom(:,3) ]; + dthetadT = [ (dthetadr').*drdT(:,1) (dthetadr').*drdT(:,2) (dthetadr').*drdT(:,3) ]; +end; + +% Add the fisheye distortion: + +theta2 = theta.^2; +theta3 = theta2.*theta; +theta4 = theta2.^2; +theta5 = theta4.*theta; +theta6 = theta3.^2; +theta7 = theta6.*theta; +theta8 = theta4.*theta4; +theta9 = theta8.*theta; + +% Fisheye distortion -> theta_d = theta * (1 + k(1)*theta2 + k(2)*theta4 + k(3)*theta6 + k(4)*theta8) + +theta_d = theta + k(1)*theta3 + k(2)*theta5 + k(3)*theta7 + k(4)*theta9; + +if nargout > 1, + dtheta_ddtheta = 1 + 3*k(1)*theta2 + 5*k(2)*theta4 + 7*k(3)*theta6 + 9*k(4)*theta8; + dtheta_ddom = [ (dtheta_ddtheta').*dthetadom(:,1) (dtheta_ddtheta').*dthetadom(:,2) (dtheta_ddtheta').*dthetadom(:,3) ]; + dtheta_ddT = [ (dtheta_ddtheta').*dthetadT(:,1) (dtheta_ddtheta').*dthetadT(:,2) (dtheta_ddtheta').*dthetadT(:,3) ]; + dtheta_ddk = [theta3' theta5' theta7' theta9']; +end; + +% ratio: +inv_r = ones(1,length(r)); +inv_r(r>1e-8) = 1./r(r>1e-8); + +cdist = ones(1,length(r)); +cdist(r > 1e-8) = theta_d(r > 1e-8) ./ r(r > 1e-8); + +if nargout > 1, + dcdistdom = [ ((inv_r').*(dtheta_ddom(:,1) - (cdist').*drdom(:,1))) ((inv_r').*(dtheta_ddom(:,2) - (cdist').*drdom(:,2))) ((inv_r').*(dtheta_ddom(:,3) - (cdist').*drdom(:,3))) ]; + dcdistdT = [ ((inv_r').*(dtheta_ddT(:,1) - (cdist').*drdT(:,1))) ((inv_r').*(dtheta_ddT(:,2) - (cdist').*drdT(:,2))) ((inv_r').*(dtheta_ddT(:,3) - (cdist').*drdT(:,3))) ]; + dcdistdk = [ (inv_r'.*dtheta_ddk(:,1)) (inv_r'.*dtheta_ddk(:,2)) (inv_r'.*dtheta_ddk(:,3)) (inv_r'.*dtheta_ddk(:,4)) ]; +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,4); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; +end; + +% No tangential distortion: +xd2 = xd1; +if nargout > 1, + dxd2dom = dxd1dom; + dxd2dT = dxd1dT; + dxd2dk = dxd1dk; +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,4); + 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,4)) .* 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(4,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points_fisheye(X,om,T,f,c,k,alpha); + + +% Test on om: not OK + +dom = 0.00000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points_fisheye(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: not OK + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points_fisheye(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_points_fisheye(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_points_fisheye(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.00001 * norm(k)*randn(4,1); +k2 = k + dk; + +[x2] = project_points_fisheye(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_points_fisheye(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/mr/Ub5/TOOLBOX_calib/project_points_weak.m b/mr/Ub5/TOOLBOX_calib/project_points_weak.m new file mode 100755 index 0000000..b29b8ef --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/project_points_weak.m @@ -0,0 +1,349 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha,Zave] = project_points_weak(X,om,T,f,c,k,alpha,Zave) + +%project_points_weak.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points_weak(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. Weak perspective model used. +% +%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) +% Zave: The average distance of the structure to the camera (opt). +% Default: computed average Z of the structure X in the camera reference frame (if not there) +% +%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 weak perspective projection coordinates of P is [a;b] where a=x/Zave and b=y/Zave. +%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); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + +if ~exist('Zave'), + Zave = mean(Y(3,:)); +else + if Zave == 0, + Zave = mean(Y(3,:)); + end; +end; + +if 0 + inv_Z = 1./Y(3,:); +else + inv_Z = repmat(1/Zave,[1 n]); +end; + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)); + + + +if 0, + + bb = (-x(1,:) .* inv_Z)'*ones(1,3); + cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + 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,:); + +else + + + dxdom = zeros(2*n,3); + dxdom(1:2:end,:) = dYdom(1:3:end,:)/Zave; + dxdom(2:2:end,:) = dYdom(2:3:end,:)/Zave; + + dxdT = zeros(2*n,3); + dxdT(1:2:end,:) = dYdT(1:3:end,:)/Zave; + dxdT(2:2:end,:) = dYdT(2:3:end,:)/Zave; + +end; + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +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,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +r6 = r2.^3; + +dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom; +dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; + +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']; + + +xd1 = x .* (ones(2,1)*cdist); + +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; + + + +% 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); + +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'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +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,:)'; + + + +% Pixel coordinates: +if length(f)>1, + xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + 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,:)'; +else + xp = f * xd3 + c*ones(1,n); + dxpdom = f * dxd3dom; + dxpdT = f * dxd3dT; + dxpdk = f * dxd3dk; + dxpdalpha = f .* dxd3dalpha; + dxpdf = xd3(:); +end; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); X(3,:) = X(3,:) + 100; +om = zeros(3,1); +T = zeros(3,1); +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_points_weak(X,om,T,f,c,k,alpha); + + +% Test on om: OK + +dom = 0.0000001 *randn(3,1); +om2 = om + dom; + +[x2] = project_points_weak(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.000000001 *randn(3,1); +T2 = T + dT; + +[x2] = project_points_weak(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_points_weak(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_points_weak(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_points_weak(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_points_weak(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/mr/Ub5/TOOLBOX_calib/projectedGrid.m b/mr/Ub5/TOOLBOX_calib/projectedGrid.m new file mode 100755 index 0000000..561a7d0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/projectedGrid.m @@ -0,0 +1,24 @@ +function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny); + +% new formalism using homographies + +a00 = [P1;1]; +a10 = [P2;1]; +a11 = [P3;1]; +a01 = [P4;1]; + +% Compute the planart collineation: + +[H] = compute_collineation (a00, a10, a11, a01); + + +% Build the grid using the planar collineation: + +x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1); +y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1); + +pts = [x_l(:) y_l(:) ones(nx*ny,1)]'; + +XX = H*pts; + +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); diff --git a/mr/Ub5/TOOLBOX_calib/projector_calib.m b/mr/Ub5/TOOLBOX_calib/projector_calib.m new file mode 100755 index 0000000..11a83cd --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/projector_calib.m @@ -0,0 +1,93 @@ + +% load camera results (for setting active images, n_ima,...) +load camera_results; + + +% Projection settings: + +wintx = 12; %8; +winty = 12; %8; +nx = 1024; +ny = 768; +dX = 32; +dY = 32; +dXoff=511.5; +dYoff=383.5; + + +proj_name = 'proj'; %input('Basename projector calibration images (without number nor suffix): ','s'); +camera_name = 'cam'; + + +xr_list = NaN*ones(1,n_ima); +yr_list = NaN*ones(1,n_ima); + + +ind_proc = ind_active; + + + +DEBUG = 1; +%ind_ima_proj = [18]; + +for i = ind_proc, + + projector_marker; + +end; + + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + + + +recompute_corner = 0; + +if recompute_corner & ~exist(['xproj_' num2str(ind_proc(1))]), + if exist('projector_data'), + load projector_data; + else + recompute_corner = 0; + disp('WARNING: Cannot recompute corners. Data need to be extracted at least once'); + end; +end; + +if ~recompute_corner, + disp('Manual extraction mode'); +else + disp('Automatic recomputation of the corners'); +end; + +% extract the projector corners: + + +for kk = ind_proc, + + projector_ima_corners + +end; + + +string_save = 'save projector_data '; + +for kk = ind_active, + string_save = [string_save ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_save); + + + + +return; + + +i = 18; + +figure(4) +image(I_29); +colormap(gray(256)); +hold on; +plot(x_29(1,:)+1,x_29(2,:)+1,'r+','markersize',13,'linewidth',3) +hold off; +axis image +axis off; diff --git a/mr/Ub5/TOOLBOX_calib/projector_ima_corners.m b/mr/Ub5/TOOLBOX_calib/projector_ima_corners.m new file mode 100755 index 0000000..6b18cf9 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/projector_ima_corners.m @@ -0,0 +1,61 @@ + + eval(['Ip = Ip_' num2str(kk) ';']); + eval(['In = In_' num2str(kk) ';']); + + xr = xr_list(kk); + yr = yr_list(kk); + + fprintf(1,'Processing image %d...',kk); + + if ~recompute_corner, + [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1); + xproj = x; + else + eval(['xproj = xproj_' num2str(kk) ';']); + x = cornerfinder(xproj+1,Ip,winty,wintx); + xproj = x - 1; + end; + + Np_proj = size(x,2); + + figure(2); + image(Ip); + hold on; + plot(xproj(1,:)+1,xproj(2,:)+1,'r+'); + %title('Click on your reference point'); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + hold off; + + %disp('Click on your reference point...'); + + %[xr,yr] = ginput2(1); + + xr = xr_list(kk); + yr = yr_list(kk); + + err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2)); + ind_ref = find(err == min(err)); + + ref_pt = xproj(:,ind_ref); + + figure(2); + hold on; + plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off; + title(['Image ' num2str(kk)]); + drawnow; + + + if ~recompute_corner, + + off_x = mod(ind_ref-1,n_sq_x+1); + off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1)); + + x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj)); + + eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image + + end; + + eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image + diff --git a/mr/Ub5/TOOLBOX_calib/projector_marker.m b/mr/Ub5/TOOLBOX_calib/projector_marker.m new file mode 100755 index 0000000..2c34c95 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/projector_marker.m @@ -0,0 +1,146 @@ + 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_namep = [proj_name number_ext 'p.' format_image]; + ima_namen = [proj_name number_ext 'n.' format_image]; + ima_namer = [proj_name number_ext 'r.' format_image]; + ima_nameb = [camera_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ip = double(loadppm(ima_namep)); + In = double(loadppm(ima_namen)); + Ir = double(loadppm(ima_namer)); + Ib = double(loadppm(ima_nameb)); + else + Ip = double(loadpgm(ima_namep)); + In = double(loadpgm(ima_namen)); + Ir = double(loadpgm(ima_namer)); + Ib = double(loadpgm(ima_nameb)); + end; + else + if format_image(1) == 'r', + Ip = readras(ima_namep); + In = readras(ima_namen); + Ir = readras(ima_namer); + Ib = readras(ima_nameb); + else + Ip = double(imread(ima_namep)); + In = double(imread(ima_namen)); + Ir = double(imread(ima_namer)); + Ib = double(imread(ima_nameb)); + end; + end; + + + if size(Ip,3)>1, + Ip = 0.299 * Ip(:,:,1) + 0.5870 * Ip(:,:,2) + 0.114 * Ip(:,:,3); + In = 0.299 * In(:,:,1) + 0.5870 * In(:,:,2) + 0.114 * In(:,:,3); + Ir = 0.299 * Ir(:,:,1) + 0.5870 * Ir(:,:,2) + 0.114 * Ir(:,:,3); + Ib = 0.299 * Ib(:,:,1) + 0.5870 * Ib(:,:,2) + 0.114 * Ib(:,:,3); + end; + + %IIp = Ip - In; + + %Ip2 = Ip - Ib; + %In2 = In - Ib; + + %imax = max(IIp(:)); + %imin = min(IIp(:)); + + %IIp = 255*(IIp - imin)/(imax - imin); + + %indplus = find(IIp >= 255/2); + %indminus = find(IIp < 255/2); + + %IIp(indplus) = 255*ones(length(indplus),1); + %IIp(indminus) = zeros(length(indminus),1); + + delta_I = Ip - In; + + %IIp = 255*(1 + exp(-delta_I/2)).^(-1); + + + IIp = (Ip >= In)*255; + + IIp = conv2(conv2(IIp,[1/4 1/2 1/4],'same'),[1/4 1/2 1/4]','same'); + + if 0, + figure(4); + image(IIp); + colormap(gray(256)); + axis off; + end; + + + eval(['Ip_' num2str(i) ' = IIp;']); + eval(['In_' num2str(i) ' = 255 - IIp;']); + + + + + I_marker2 = Ib-Ir; + + + if DEBUG, + + Imax = max(I_marker2(:)); + Imin = min(I_marker2(:)); + + I_marker_out = 255*(I_marker2 - Imin)/(Imax - Imin); + + figure(3); + image(I_marker_out); + colormap(gray(256)); + + [xr,yr] = ginput(1); + + xr_list(i) = xr; + yr_list(i) = yr; + + else + + I_marker = I_marker2 >50; + I_marker = eliminate_boundary(I_marker); + I_marker = eliminate_boundary(I_marker); + [ymm,xmm] = find(I_marker); + + if length(xmm)<10, + fprintf(1,'WARNING!! No marker in image %d!!!!\n',i); + else + xr = mean(xmm); + yr = mean(ymm); + xr_list(i) = xr; + yr_list(i) = yr; + end; + end; + + figure(2); + image(IIp); + colormap(gray(256)); + hold on; + plot(xr,yr,'go'); + hold off; + title(['Image ' num2str(i)]); + drawnow; + + if ~DEBUG + waitforbuttonpress; + end; + + + + end; diff --git a/mr/Ub5/TOOLBOX_calib/readras.m b/mr/Ub5/TOOLBOX_calib/readras.m new file mode 100755 index 0000000..37e9c49 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/readras.m @@ -0,0 +1,87 @@ +function [X, map] = readras(filename, ys, ye, xs, xe); +%READRAS Read an image file in sun raster format. +% READRAS('imagefile.ras') reads a "sun.raster" image file. +% [X, map] = READRAS('imagefile.ras') returns both the image and a +% color map, so that +% [X, map] = readras('imagefile.ras'); +% image(X) +% colormap(map) +% axis('equal') +% will display the result with the proper colors. +% NOTE: readras cannot deal with complicated color maps. +% In fact, Matlab doesn't quite allow to work with colormaps +% with more than 64 entries. +% + +%% +%% (C) Thomas K. Leung 3/30/93. +%% California Institute of Technology. +%% Modified by Andrea Mennucci to deal with color images +%% + +% PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if(strcmp(lower(suffix), 'ras')) % raster file format % + fp = fopen(filename, 'rb'); + if(fp<0) error(['Cannot open ' filename '.']), end + + %Read and crack the 32-byte header + fseek(fp, 4, -1); + + width = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + height = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + depth = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + length = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + type = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = maplen / 3; + + if maptype == 2 % RMT_RAW + map = fread(fp, [maplen, 3], 'uchar')/255; +% if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end; + elseif maptype == 1 % RMT_EQUAL_RGB + map(:,1) = fread(fp, [maplen], 'uchar'); + map(:,2) = fread(fp, [maplen], 'uchar'); + map(:,3) = fread(fp, [maplen], 'uchar'); + %maxmap = max(max(map)); + map = map/255; + if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end; + else % RMT_NONE + map = []; + end +% if maplen>64, +% map=[map',zeros(3,256-maplen)]'; +% end; + + % Read the image + + if rem(width,2) == 1 + Xt = fread(fp, [width+1, height], 'uchar'); + X = Xt(1:width, :)'; + else + Xt = fread(fp, [width, height], 'uchar'); + X = Xt'; + end + X = X + 1; + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end + + +if nargin == 5 + + X = X(ys:ye, xs:xe); + +end \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/recomp_corner_calib.m b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib.m new file mode 100755 index 0000000..7c39693 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib.m @@ -0,0 +1,130 @@ +% Re-select te corners after calibration + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + + +if n_ima == 0, + + fprintf(1,'No image data available\n'); + +else + +if ~exist(['I_' num2str(ind_active(1))]), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + if isempty(q_auto), + + eval(['I = I_' num2str(kk) ';']); + + eval(['y = y_' num2str(kk) ';']); + + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= xc - 1;']); + + else + + fprintf(1,'\n'); + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + click_ima_calib; + + end; + + else + + if ~exist(['omc_' num2str(kk)]), + + 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; + + +end; + +% Recompute the error: + +comp_error_calib; + +fprintf(1,'\ndone\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_fisheye_no_read.m b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_fisheye_no_read.m new file mode 100755 index 0000000..d77bb52 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_fisheye_no_read.m @@ -0,0 +1,141 @@ +% Re-select te corners after calibration + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +if n_ima == 0, + fprintf(1,'No image data available\n'); + return; +end + +%if ~exist(['I_' num2str(ind_active(1))]), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% disp('Cannot extract corners without images'); +% return; +% end; +%end; + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +if ~dont_ask, + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 20) = '); + if isempty(wintx), wintx = 20; end; + wintx = round(wintx); + winty = input('winty ([] = 20) = '); + if isempty(winty), winty = 20; end; + winty = round(winty); +else + wintx = 20; + winty = 20; +end; + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +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; + +if ~dont_ask, + q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); +else + q_auto = []; +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + if active_images(kk), + fprintf(1,'%d...',kk); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + ima_name = [calib_name number_ext '.' format_image]; + + if exist(ima_name), + if format_image(1) == 'p'; + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + [ny,nx,junk] = size(I); + if isempty(q_auto), + eval(['y = y_' num2str(kk) ';']); + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + eval(['x_' num2str(kk) '= xc - 1;']); + else + fprintf(1,'\n'); + fprintf(1,'\nProcessing image %d...\n',kk); + click_calib_fisheye_no_read; + end; + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + else + if ~exist(['omc_' num2str(kk)]), + 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; +end; + +% Recompute the error: +comp_error_calib_fisheye; +fprintf(1,'\ndone\n'); diff --git a/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_no_read.m new file mode 100755 index 0000000..3e982cc --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_no_read.m @@ -0,0 +1,128 @@ +% Re-select te corners after calibration +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +if n_ima == 0, + fprintf(1,'No image data available\n'); + return; +end + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if dont_ask, + wintx = 20; + winty = 20; +else + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 20) = '); + if isempty(wintx), wintx = 20; end; + wintx = round(wintx); + winty = input('winty ([] = 20) = '); + if isempty(winty), winty = 20; end; + winty = round(winty); +end; + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +if dont_ask, + ima_numbers = []; +else + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +if dont_ask, + q_auto = []; +else + q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); +end; + +fprintf(1,'Processing image '); + +for kk = ima_proc; + if active_images(kk), + fprintf(1,'%d...',kk); + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + ima_name = [calib_name number_ext '.' format_image]; + if exist(ima_name), + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + [ny,nx,junk] = size(I); + if isempty(q_auto), + eval(['y = y_' num2str(kk) ';']); + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + eval(['x_' num2str(kk) '= xc - 1;']); + else + fprintf(1,'\n'); + fprintf(1,'\nProcessing image %d...\n',kk); + click_ima_calib_no_read; + end; + else + fprintf(1,'Image %s not found!!!...',ima_name); + end; + else + if ~exist(['omc_' num2str(kk)]), + 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; +end; +% Recompute the error: +comp_error_calib; +fprintf(1,'\ndone\n'); diff --git a/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_saddle_points.m b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_saddle_points.m new file mode 100755 index 0000000..10d3f05 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/recomp_corner_calib_saddle_points.m @@ -0,0 +1,61 @@ +fprintf(1,'Recomputation of the image corners using Lucchese''s saddle point detector\n'); + +q = input('This detector only works for X junctions (checker board corners). Is this the case here? ([]=yes, other=no)','s'); + + +if isempty(q), + + ima_proc = 1:n_ima; + + fprintf(1,'Processing image '); + + for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + + eval(['I = I_' num2str(kk) ';']); + + eval(['y = x_' num2str(kk) ';']); + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + + xc = cornerfinder_saddle_point(y+1,I,winty,wintx); % the four corners + + eval(['x_' num2str(kk) '= xc - 1;']); + + else + + if ~exist(['omc_' num2str(kk)]), + + 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; + + end; + + % Recompute the error: + + comp_error_calib; + + fprintf(1,'\ndone\n'); + +else + + fprintf(1,'No recomputation done (The points are not locally saddle points)\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/rect.m b/mr/Ub5/TOOLBOX_calib/rect.m new file mode 100755 index 0000000..54d0e0c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rect.m @@ -0,0 +1,126 @@ +function [Irec] = rect(I,R,f,c,k,alpha,KK_new); + + +if nargin < 5, + k = [0;0;0;0;0]; + if nargin < 4, + c = [0;0]; + if nargin < 3, + f = [1;1]; + if nargin < 2, + R = eye(3); + if nargin < 1, + error('ERROR: Need an image to rectify'); + end; + end; + end; + end; +end; + + +if nargin < 7, + if nargin < 6, + KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; + else + KK_new = alpha; % the 6th argument is actually KK_new + end; + alpha = 0; +end; + + + +% Note: R is the motion of the points in space +% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. + + +if ~exist('KK_new'), + KK_new = [f(1) alpha*f(1) c(1);0 f(2) c(2);0 0 1]; +end; + + +[nr,nc] = size(I); + +Irec = 255*ones(nr,nc); + +[mx,my] = meshgrid(1:nc, 1:nr); +px = reshape(mx',nc*nr,1); +py = reshape(my',nc*nr,1); + +rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; + + +% Rotation: (or affine transformation): + +rays2 = R'*rays; + +x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; + + +% Add distortion: +xd = apply_distortion(x,k); + + +% Reconvert in pixels: + +px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); +py2 = f(2)*xd(2,:)+c(2); + + +% Interpolate between the closest pixels: + +px_0 = floor(px2); + + +py_0 = floor(py2); +py_1 = py_0 + 1; + + +good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); + +px2 = px2(good_points); +py2 = py2(good_points); +px_0 = px_0(good_points); +py_0 = py_0(good_points); + +alpha_x = px2 - px_0; +alpha_y = py2 - py_0; + +a1 = (1 - alpha_y).*(1 - alpha_x); +a2 = (1 - alpha_y).*alpha_x; +a3 = alpha_y .* (1 - alpha_x); +a4 = alpha_y .* alpha_x; + +ind_lu = px_0 * nr + py_0 + 1; +ind_ru = (px_0 + 1) * nr + py_0 + 1; +ind_ld = px_0 * nr + (py_0 + 1) + 1; +ind_rd = (px_0 + 1) * nr + (py_0 + 1) + 1; + +ind_new = (px(good_points)-1)*nr + py(good_points); + + + +Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd); + + + +return; + + +% Convert in indices: + +fact = 3; + +[XX,YY]= meshgrid(1:nc,1:nr); +[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); + +%tic; +Iinterp = interp2(XX,YY,I,XXi,YYi); +%toc + +[nri,nci] = size(Iinterp); + + +ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; +ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; + +good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); diff --git a/mr/Ub5/TOOLBOX_calib/rect_index.m b/mr/Ub5/TOOLBOX_calib/rect_index.m new file mode 100755 index 0000000..69928e3 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rect_index.m @@ -0,0 +1,123 @@ +function [Irec,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(I,R,f,c,k,alpha,KK_new); + + +if nargin < 5, + k = [0;0;0;0;0]; + if nargin < 4, + c = [0;0]; + if nargin < 3, + f = [1;1]; + if nargin < 2, + R = eye(3); + if nargin < 1, + error('ERROR: Need an image to rectify'); + %break; + end; + end; + end; + end; +end; + + +if nargin < 7, + if nargin < 6, + KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; + else + KK_new = alpha; % the 6th argument is actually KK_new + end; + alpha = 0; +end; + + + +% Note: R is the motion of the points in space +% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. + + +if ~exist('KK_new'), + KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1]; +end; + + +[nr,nc] = size(I); + +Irec = 255*ones(nr,nc); + +[mx,my] = meshgrid(1:nc, 1:nr); +px = reshape(mx',nc*nr,1); +py = reshape(my',nc*nr,1); + +rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; + + +% Rotation: (or affine transformation): + +rays2 = R'*rays; + +x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; + + +% Add distortion: +xd = apply_distortion(x,k); + + +% Reconvert in pixels: + +px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); +py2 = f(2)*xd(2,:)+c(2); + + +% Interpolate between the closest pixels: + +px_0 = floor(px2); +py_0 = floor(py2); + + +good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); + +px2 = px2(good_points); +py2 = py2(good_points); +px_0 = px_0(good_points); +py_0 = py_0(good_points); + +alpha_x = px2 - px_0; +alpha_y = py2 - py_0; + +a1 = (1 - alpha_y).*(1 - alpha_x); +a2 = (1 - alpha_y).*alpha_x; +a3 = alpha_y .* (1 - alpha_x); +a4 = alpha_y .* alpha_x; + +ind_1 = px_0 * nr + py_0 + 1; +ind_2 = (px_0 + 1) * nr + py_0 + 1; +ind_3 = px_0 * nr + (py_0 + 1) + 1; +ind_4 = (px_0 + 1) * nr + (py_0 + 1) + 1; + +ind_new = (px(good_points)-1)*nr + py(good_points); + + +Irec(ind_new) = a1 .* I(ind_1) + a2 .* I(ind_2) + a3 .* I(ind_3) + a4 .* I(ind_4); + + + +return; + + +% Convert in indices: + +fact = 3; + +[XX,YY]= meshgrid(1:nc,1:nr); +[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); + +%tic; +Iinterp = interp2(XX,YY,I,XXi,YYi); +%toc + +[nri,nci] = size(Iinterp); + + +ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; +ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; + +good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); diff --git a/mr/Ub5/TOOLBOX_calib/rectify_stereo_pair.m b/mr/Ub5/TOOLBOX_calib/rectify_stereo_pair.m new file mode 100755 index 0000000..4fda19f --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rectify_stereo_pair.m @@ -0,0 +1,192 @@ +% rectify_stereo_pair.m +% +% Script file that rectifies a set of stereo images after stereo calibration +% This script loads the stereo calibration file Calib_Results_stereo generated by calib_stereo.m +% Therefore, type help calib_stereo for more information. + + +if ~exist('fc_right')|~exist('cc_right')|~exist('kc_right')|~exist('alpha_c_right')|~exist('fc_left')|~exist('cc_left')|~exist('kc_left')|~exist('alpha_c_left')|~exist('om')|~exist('T') + + if exist('Calib_Results_stereo.mat')~=2, + fprintf(1,'No stereo calibration data.\n'); + return; + else + fprintf(1,'\nLoading the stereo calibration file Calib_Results_stereo.mat.\n'); + load Calib_Results_stereo; % Load the stereo calibration result + end; + +end; + + + +fprintf(1,'\nCalculating the rotation to be applied to the right and left images in order to bring the epipolar lines aligned with the horizontal scan lines, and in correspondence...\n\n'); + +R = rodrigues(om); + +% Bring the 2 cameras in the same orientation by rotating them "minimally": +r_r = rodrigues(-om/2); +r_l = r_r'; +t = r_r * T; + +% Rotate both cameras so as to bring the translation vector in alignment with the (1;0;0) axis: +if abs(t(1)) > abs(t(2)), + type_stereo = 0; + uu = [1;0;0]; % Horizontal epipolar lines +else + type_stereo = 1; + uu = [0;1;0]; % Vertical epipolar lines +end; +if dot(uu,t)<0, + uu = -uu; % Swtich side of the vector +end; +ww = cross(t,uu); +ww = ww/norm(ww); +ww = acos(abs(dot(t,uu))/(norm(t)*norm(uu)))*ww; +R2 = rodrigues(ww); + + +% Global rotations to be applied to both views: +R_R = R2 * r_r; +R_L = R2 * r_l; + + +% The resulting rigid motion between the two cameras after image rotations (substitutes of om, R and T): +R_new = eye(3); +om_new = zeros(3,1); +T_new = R_R*T; + + + +% Computation of the *new* intrinsic parameters for both left and right cameras: + +% Vertical focal length *MUST* be the same for both images (here, we are trying to find a focal length that retains as much information contained in the original distorted images): +if kc_left(1) < 0, + fc_y_left_new = fc_left(2) * (1 + kc_left(1)*(nx^2 + ny^2)/(4*fc_left(2)^2)); +else + fc_y_left_new = fc_left(2); +end; +if kc_right(1) < 0, + fc_y_right_new = fc_right(2) * (1 + kc_right(1)*(nx^2 + ny^2)/(4*fc_right(2)^2)); +else + fc_y_right_new = fc_right(2); +end; +fc_y_new = min(fc_y_left_new,fc_y_right_new); + + +% For simplicity, let's pick the same value for the horizontal focal length as the vertical focal length (resulting into square pixels): +fc_left_new = round([fc_y_new;fc_y_new]); +fc_right_new = round([fc_y_new;fc_y_new]); + +% Select the new principal points to maximize the visible area in the rectified images + +cc_left_new = [(nx-1)/2;(ny-1)/2] - mean(project_points2([normalize_pixel([0 nx-1 nx-1 0; 0 0 ny-1 ny-1],fc_left,cc_left,kc_left,alpha_c_left);[1 1 1 1]],rodrigues(R_L),zeros(3,1),fc_left_new,[0;0],zeros(5,1),0),2); +cc_right_new = [(nx-1)/2;(ny-1)/2] - mean(project_points2([normalize_pixel([0 nx-1 nx-1 0; 0 0 ny-1 ny-1],fc_right,cc_right,kc_right,alpha_c_right);[1 1 1 1]],rodrigues(R_R),zeros(3,1),fc_right_new,[0;0],zeros(5,1),0),2); + + +% For simplivity, set the principal points for both cameras to be the average of the two principal points. +if ~type_stereo, + %-- Horizontal stereo + cc_y_new = (cc_left_new(2) + cc_right_new(2))/2; + cc_left_new = [cc_left_new(1);cc_y_new]; + cc_right_new = [cc_right_new(1);cc_y_new]; +else + %-- Vertical stereo + cc_x_new = (cc_left_new(1) + cc_right_new(1))/2; + cc_left_new = [cc_x_new;cc_left_new(2)]; + cc_right_new = [cc_x_new;cc_right_new(2)]; +end; + + +% Of course, we do not want any skew or distortion after rectification: +alpha_c_left_new = 0; +alpha_c_right_new = 0; +kc_left_new = zeros(5,1); +kc_right_new = zeros(5,1); + + +% The resulting left and right camera matrices: +KK_left_new = [fc_left_new(1) fc_left_new(1)*alpha_c_left_new cc_left_new(1);0 fc_left_new(2) cc_left_new(2); 0 0 1]; +KK_right_new = [fc_right_new(1) fc_right_new(1)*alpha_c_right cc_right_new(1);0 fc_right_new(2) cc_right_new(2); 0 0 1]; + +% The sizes of the images are the same: +nx_right_new = nx; +ny_right_new = ny; +nx_left_new = nx; +ny_left_new = ny; + +% Save the resulting extrinsic and intrinsic paramters into a file: +fprintf(1,'Saving the *NEW* set of intrinsic and extrinsic parameters corresponding to the images *AFTER* rectification under Calib_Results_stereo_rectified.mat...\n\n'); +save Calib_Results_stereo_rectified om_new R_new T_new fc_left_new cc_left_new kc_left_new alpha_c_left_new KK_left_new fc_right_new cc_right_new kc_right_new alpha_c_right_new KK_right_new nx_right_new ny_right_new nx_left_new ny_left_new + + + +% Let's rectify the entire set of calibration images: + +fprintf(1,'Pre-computing the necessary data to quickly rectify the images (may take a while depending on the image resolution, but needs to be done only once - even for color images)...\n\n'); + +% Pre-compute the necessary indices and blending coefficients to enable quick rectification: +[Irec_junk_left,ind_new_left,ind_1_left,ind_2_left,ind_3_left,ind_4_left,a1_left,a2_left,a3_left,a4_left] = rect_index(zeros(ny,nx),R_L,fc_left,cc_left,kc_left,alpha_c_left,KK_left_new); +[Irec_junk_left,ind_new_right,ind_1_right,ind_2_right,ind_3_right,ind_4_right,a1_right,a2_right,a3_right,a4_right] = rect_index(zeros(ny,nx),R_R,fc_right,cc_right,kc_right,alpha_c_right,KK_right_new); + +clear Irec_junk_left + +if 0, + %% Test of rectification for 2 images: + + % left image: + I = double(rgb2gray(imread('left.jpg'))); + I2 = 255*ones(ny,nx); + I2(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left)); + imwrite(uint8(I2),gray(256),'left_rect.jpg','jpg'); + + % right image: + I = double(rgb2gray(imread('right.jpg'))); + I2 = 255*ones(ny,nx); + I2(ind_new_right) = uint8(a1_right .* I(ind_1_right) + a2_right .* I(ind_2_right) + a3_right .* I(ind_3_right) + a4_right .* I(ind_4_right)); + imwrite(uint8(I2),gray(256),'right_rect.jpg','jpg'); + +end; + + + + +% Loop around all the frames now: (if there are images to rectify) + +if ~isempty(calib_name_left)&~isempty(calib_name_right), + + fprintf(1,'Rectifying all the images (this should be fast)...\n\n'); + + % Rectify all the images: (This is fastest way to proceed: precompute the set of image indices, and blending coefficients before actual image warping!) + + for kk = find(active_images); + + % Left image: + + I = load_image(kk,calib_name_left,format_image_left,type_numbering_left,image_numbers_left,N_slots_left); + + fprintf(1,'Image warping...\n'); + + I2 = 255*ones(ny,nx); + I2(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left)); + + write_image(I2,kk,[calib_name_left '_rectified'],format_image_left,type_numbering_left,image_numbers_left,N_slots_left ), + + fprintf(1,'\n'); + + % Right image: + + I = load_image(kk,calib_name_right,format_image_right,type_numbering_right,image_numbers_right,N_slots_right); + + fprintf(1,'Image warping...\n'); + + I2 = 255*ones(ny,nx); + I2(ind_new_right) = uint8(a1_right .* I(ind_1_right) + a2_right .* I(ind_2_right) + a3_right .* I(ind_3_right) + a4_right .* I(ind_4_right)); + + write_image(I2,kk,[calib_name_right '_rectified'],format_image_right,type_numbering_right,image_numbers_right,N_slots_right ); + + fprintf(1,'\n'); + + end; + +end; + diff --git a/mr/Ub5/TOOLBOX_calib/reproject_calib.m b/mr/Ub5/TOOLBOX_calib/reproject_calib.m new file mode 100755 index 0000000..0eb7dc0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/reproject_calib.m @@ -0,0 +1,216 @@ +%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +% Color code for each image: + +colors = 'brgkcm'; + +% Reproject the patterns on the images, and compute the pixel errors: + +% Reload the images if necessary +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + +if n_ima ~= 0, +if ~no_image, + if ~exist(['I_' num2str(ind_active(1)) ]'), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + fprintf(1,'WARNING: Do not show the original images\n'); %return; + end; + end; +else + no_image_file = 1; +end; +end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if (~dont_ask)&(length(ind_active)>1), + ima_numbers = input('Number(s) of image(s) to show ([] = all images) = '); +else + ima_numbers = []; +end; + + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +figure(5); +for kk = ima_proc, %1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; + end; +end; +hold off; +axis('equal'); +title('Reprojection error (in pixel)'); +xlabel('x'); +ylabel('y'); +drawnow; +if n_ima==0, + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); +end; + + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + + +no_grid = 0; + +for kk = ima_proc, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if exist(['I_' num2str(kk)]), + eval(['I = I_' num2str(kk) ';']); + else + I = 255*ones(ny,nx); + end; + + + + figure(5+kk); + image(I); hold on; + colormap(gray(256)); + + + if ~no_grid, + + eval(['x_kk = x_' num2str(kk) ';']); + + N_kk = size(x_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)])|~exist(['n_sq_y_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + end; + + if ~no_grid, + + % plot more things on the figure (to help the user): + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + ind_ori = (Ny - 1) * Nx + 1; + ind_X = Nx*Ny; + ind_Y = 1; + ind_XY = Nx; + + xo = x_kk(1,ind_ori); + yo = x_kk(2,ind_ori); + + xX = x_kk(1,ind_X); + yX = x_kk(2,ind_X); + + xY = x_kk(1,ind_Y); + yY = x_kk(2,ind_Y); + + xXY = x_kk(1,ind_XY); + yXY = x_kk(2,ind_XY); + + uu = cross(cross([xo;yo;1],[xXY;yXY;1]),cross([xX;yX;1],[xY;yY;1])); + xc = uu(1)/uu(3); + yc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xY;yY;1]),cross([xX;yX;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xX;yX;1]),cross([xc;yc;1],bbb)); + xXc = uu(1)/uu(3); + yXc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xX;yX;1]),cross([xY;yY;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xY;yY;1]),cross([xc;yc;1],bbb)); + xYc = uu(1)/uu(3); + yYc = uu(2)/uu(3); + + uX = [xXc - xc;yXc - yc]; + uY = [xYc - xc;yYc - yc]; + uO = [xo - xc;yo - yc]; + + uX = uX / norm(uX); + uY = uY / norm(uY); + uO = uO / norm(uO); + + delta = 30; + + plot([xo;xX]+1,[yo;yX]+1,'g-','linewidth',2); + plot([xo;xY]+1,[yo;yY]+1,'g-','linewidth',2); + text(xXc + delta * uX(1) +1 ,yXc + delta * uX(2)+1,'X','color','g','Fontsize',14); + text(xYc + delta * uY(1)+1 ,yYc + delta * uY(2)+1,'Y','color','g','Fontsize',14,'HorizontalAlignment','center'); + text(xo + delta * uO(1) +1,yo + delta * uO(2)+1,'O','color','g','Fontsize',14); + + end; + + + title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']); + eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']); + eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']); + eval(['quiver(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),1,''' colors(rem(kk-1,6)+1) ''');']); + zoom on; + axis([1 nx 1 ny]); + hold off; + drawnow; + + + set(5+kk,'color',[1 1 1]); + set(5+kk,'Name',num2str(kk),'NumberTitle','off'); + + + + end; + end; +end; + +if n_ima ~= 0, +err_std = std(ex')'; +fprintf(1,'Pixel error: err = [%3.5f %3.5f] (all active images)\n\n',err_std); +end; diff --git a/mr/Ub5/TOOLBOX_calib/reproject_calib_no_read.m b/mr/Ub5/TOOLBOX_calib/reproject_calib_no_read.m new file mode 100755 index 0000000..7e1cfc6 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/reproject_calib_no_read.m @@ -0,0 +1,248 @@ +%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +% Color code for each image: + +colors = 'brgkcm'; + +% Reproject the patterns on the images, and compute the pixel errors: + +% Reload the images if necessary +if n_ima ~= 0, +if ~exist(['omc_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n'); + return; +end; +end; + +%if ~no_image, +% if ~exist(['I_' num2str(ind_active(1)) ]'), +% n_ima_save = n_ima; +% active_images_save = active_images; +% ima_read_calib; +% n_ima = n_ima_save; +% active_images = active_images_save; +% check_active_images; +% if no_image_file, +% fprintf(1,'WARNING: Do not show the original images\n'); %return; +% end; +% end; +%else +% no_image_file = 1; +%end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if (~dont_ask)&(length(ind_active)>1), + ima_numbers = input('Number(s) of image(s) to show ([] = all images) = '); +else + ima_numbers = []; +end; + + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +figure(5); +for kk = ima_proc, %1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; + end; +end; +hold off; +axis('equal'); +title('Reprojection error (in pixel)'); +xlabel('x'); +ylabel('y'); +drawnow; +if n_ima==0, + text(.5,.5,'No image data available','fontsize',24,'horizontalalignment' ,'center'); +end; + + +set(5,'color',[1 1 1]); +set(5,'Name','error','NumberTitle','off'); + + +no_grid = 0; + +for kk = ima_proc, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if ~exist(ima_name), + + I = 255*ones(ny,nx); + + else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [ny,nx,junk] = size(I); + + end; + + + + + + figure(5+kk); + image(I); hold on; + colormap(gray(256)); + + + if ~no_grid, + + eval(['x_kk = x_' num2str(kk) ';']); + + N_kk = size(x_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)])|~exist(['n_sq_y_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + end; + + if ~no_grid, + + % plot more things on the figure (to help the user): + + Nx = n_sq_x+1; + Ny = n_sq_y+1; + + ind_ori = (Ny - 1) * Nx + 1; + ind_X = Nx*Ny; + ind_Y = 1; + ind_XY = Nx; + + xo = x_kk(1,ind_ori); + yo = x_kk(2,ind_ori); + + xX = x_kk(1,ind_X); + yX = x_kk(2,ind_X); + + xY = x_kk(1,ind_Y); + yY = x_kk(2,ind_Y); + + xXY = x_kk(1,ind_XY); + yXY = x_kk(2,ind_XY); + + uu = cross(cross([xo;yo;1],[xXY;yXY;1]),cross([xX;yX;1],[xY;yY;1])); + xc = uu(1)/uu(3); + yc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xY;yY;1]),cross([xX;yX;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xX;yX;1]),cross([xc;yc;1],bbb)); + xXc = uu(1)/uu(3); + yXc = uu(2)/uu(3); + + bbb = cross(cross([xo;yo;1],[xX;yX;1]),cross([xY;yY;1],[xXY;yXY;1])); + uu = cross(cross([xo;yo;1],[xY;yY;1]),cross([xc;yc;1],bbb)); + xYc = uu(1)/uu(3); + yYc = uu(2)/uu(3); + + uX = [xXc - xc;yXc - yc]; + uY = [xYc - xc;yYc - yc]; + uO = [xo - xc;yo - yc]; + + uX = uX / norm(uX); + uY = uY / norm(uY); + uO = uO / norm(uO); + + delta = 30; + + plot([xo;xX]+1,[yo;yX]+1,'g-','linewidth',2); + plot([xo;xY]+1,[yo;yY]+1,'g-','linewidth',2); + text(xXc + delta * uX(1) +1 ,yXc + delta * uX(2)+1,'X','color','g','Fontsize',14); + text(xYc + delta * uY(1)+1 ,yYc + delta * uY(2)+1,'Y','color','g','Fontsize',14,'HorizontalAlignment','center'); + text(xo + delta * uO(1) +1,yo + delta * uO(2)+1,'O','color','g','Fontsize',14); + + end; + + + title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']); + eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']); + eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']); + zoom on; + axis([1 nx 1 ny]); + hold off; + drawnow; + + + set(5+kk,'color',[1 1 1]); + set(5+kk,'Name',num2str(kk),'NumberTitle','off'); + + end; + end; +end; + +if n_ima ~= 0, +err_std = std(ex')'; +fprintf(1,'Pixel error: err = [%3.5f %3.5f] (all active images)\n\n',err_std); +end; diff --git a/mr/Ub5/TOOLBOX_calib/rigid_motion.m b/mr/Ub5/TOOLBOX_calib/rigid_motion.m new file mode 100755 index 0000000..a4a5527 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rigid_motion.m @@ -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; + + + + diff --git a/mr/Ub5/TOOLBOX_calib/rodrigues.m b/mr/Ub5/TOOLBOX_calib/rodrigues.m new file mode 100755 index 0000000..9a1447b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rodrigues.m @@ -0,0 +1,273 @@ +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)] + [M(2,1), M(3,2), M(3,1)])/2; + syn = ((mvec > eps) - (mvec < -eps)); % 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] + +%% 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) + +%% Another test case where norm(om)=pi from Chen Feng (June 27th, 2014) +R = [-0.950146567583153 -6.41765854280073e-05 0.311803617668748; ... + -6.41765854277654e-05 -0.999999917385145 -0.000401386434914383; ... + 0.311803617668748 -0.000401386434914345 0.950146484968298]; +om = rodrigues(R) +norm(om) - pi + +%% Another test case where norm(om)=pi from 余成义 (July 1st, 2014) +R = [-0.999920129411407 -6.68593208347372e-05 -0.0126384464118876; ... + 9.53007036072085e-05 -0.999997464662094 -0.00224979713751896; ... + -0.0126382639492467 -0.00225082189773293 0.999917600647740]; +om = rodrigues(R) +norm(om) - pi + + + + diff --git a/mr/Ub5/TOOLBOX_calib/rotation.m b/mr/Ub5/TOOLBOX_calib/rotation.m new file mode 100755 index 0000000..87ee2fe --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/rotation.m @@ -0,0 +1,23 @@ +function [] = rotation(st); + +if nargin < 1, + st= 1; +end; + + +fig = gcf; + +ax = gca; + +vv = get(ax,'view'); + +az = vv(1); +el = vv(2); + +for azi = az:-abs(st):az-360, + + set(ax,'view',[azi el]); + figure(fig); + drawnow; + +end; diff --git a/mr/Ub5/TOOLBOX_calib/run_error_analysis.m b/mr/Ub5/TOOLBOX_calib/run_error_analysis.m new file mode 100755 index 0000000..095e17e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/run_error_analysis.m @@ -0,0 +1,65 @@ +%%% Program that launchs the complete + +for N_ima_active = 1:30, + + error_analysis; + +end; + + + +return; + + +f = []; +f_std = []; + +c = []; +c_std = []; + +k = []; +k_std = []; + +NN = 30; + +for rr = 1:NN, + + load(['Calib_Accuracies_' num2str(rr)]); + + [m1,s1] = mean_std_robust(fc_list(1,:)); + [m2,s2] = mean_std_robust(fc_list(2,:)); + + f = [f [m1;m2]]; + f_std = [f_std [s1;s2]]; + + [m1,s1] = mean_std_robust(cc_list(1,:)); + [m2,s2] = mean_std_robust(cc_list(2,:)); + + c = [c [m1;m2]]; + c_std = [c_std [s1;s2]]; + + [m1,s1] = mean_std_robust(kc_list(1,:)); + [m2,s2] = mean_std_robust(kc_list(2,:)); + [m3,s3] = mean_std_robust(kc_list(3,:)); + [m4,s4] = mean_std_robust(kc_list(4,:)); + + k = [k [m1;m2;m3;m4]]; + k_std = [k_std [s1;s2;s3;s4]]; + +end; + +figure(1); +errorbar([1:NN;1:NN]',f',f_std'); +figure(2); +errorbar([1:NN;1:NN]',c',c_std'); +figure(3); +errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std'); + +figure(4); +semilogy(f_std'); + +figure(5); +semilogy(c_std'); + +figure(6); +semilogy(k_std'); diff --git a/mr/Ub5/TOOLBOX_calib/saveinr.m b/mr/Ub5/TOOLBOX_calib/saveinr.m new file mode 100755 index 0000000..a176e39 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saveinr.m @@ -0,0 +1,46 @@ +%SAVEINR Write an INRIMAGE format file +% +% SAVEINR(filename, im) +% +% Saves the specified image array in a INRIA image format file. +% +% SEE ALSO: loadinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + +% Peter Corke 1996 + +function saveinr(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + + % build the header + hdr = []; + s = sprintf('#INRIMAGE-4#{\n'); + hdr = [hdr s]; + s = sprintf('XDIM=%d\n',c); + hdr = [hdr s]; + s = sprintf('YDIM=%d\n',r); + hdr = [hdr s]; + s = sprintf('ZDIM=1\n'); + hdr = [hdr s]; + s = sprintf('VDIM=1\n'); + hdr = [hdr s]; + s = sprintf('TYPE=float\n'); + hdr = [hdr s]; + s = sprintf('PIXSIZE=32\n'); + hdr = [hdr s]; + s = sprintf('SCALE=2**0\n'); + hdr = [hdr s]; + s = sprintf('CPU=sun\n#'); + hdr = [hdr s]; + + % make it 256 bytes long and write it + hdr256 = zeros(1,256); + hdr256(1:length(hdr)) = hdr; + fwrite(fid, hdr256, 'uchar'); + + % now the binary data + fwrite(fid, im', 'float32'); + fclose(fid) diff --git a/mr/Ub5/TOOLBOX_calib/savepgm.m b/mr/Ub5/TOOLBOX_calib/savepgm.m new file mode 100755 index 0000000..0cee75d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/savepgm.m @@ -0,0 +1,22 @@ +%SAVEPGM Write a PGM format file +% +% SAVEPGM(filename, im) +% +% Saves the specified image array in a binary (P5) format PGM image file. +% +% SEE ALSO: loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function savepgm(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + fprintf(fid, 'P5\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + fwrite(fid, im', 'uchar'); + fclose(fid); diff --git a/mr/Ub5/TOOLBOX_calib/saveppm.m b/mr/Ub5/TOOLBOX_calib/saveppm.m new file mode 100755 index 0000000..ece092b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saveppm.m @@ -0,0 +1,45 @@ +%SAVEPPM Write a PPM format file +% +% SAVEPPM(filename, I) +% +% Saves the specified red, green and blue planes in a binary (P6) +% format PPM image file. +% +% SEE ALSO: loadppm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function saveppm(fname, I) + +I = double(I); + +if size(I,3) == 1, + R = I; + G = I; + B = I; +else + R = I(:,:,1); + G = I(:,:,2); + B = I(:,:,3); +end; + +%keyboard; + + fid = fopen(fname, 'w'); + [r,c] = size(R'); + fprintf(fid, 'P6\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + R = R'; + G = G'; + B = B'; + im = [R(:) G(:) B(:)]; + %im = reshape(im,r,c*3); + im = im'; + %im = im(:); + fwrite(fid, im, 'uchar'); + fclose(fid); + diff --git a/mr/Ub5/TOOLBOX_calib/saving_calib.m b/mr/Ub5/TOOLBOX_calib/saving_calib.m new file mode 100755 index 0000000..6f9b6c2 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_calib.m @@ -0,0 +1,220 @@ + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('est_alpha'), + est_alpha = 0; +end; + +if ~exist('center_optim'), + center_optim = 1; +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('est_dist'), + if length(est_dist) == 4, + est_dist = [est_dist ; 0]; + end; +end; + +if exist('kc'), + if length(kc) == 4; + kc = [kc;0]; + end; +end; + +if ~exist('fc_error'), + fc_error = NaN*ones(2,1); +end; + +if ~exist('kc_error'), + kc_error = NaN*ones(5,1); +end; + +if ~exist('cc_error'), + cc_error = NaN*ones(2,1); +end; + +if ~exist('alpha_c_error'), + alpha_c_error = NaN; +end; + +check_active_images; + +if ~exist('solution_init'), solution_init = []; end; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('solution'), + solution = []; +end; + +if ~exist('param_list'), + param_list = solution; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + +if ~exist('Hcal'), + Hcal = []; +end; +if ~exist('Wcal'), + Wcal = []; +end; +if ~exist('map'), + map = []; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + if ~exist(['y_' num2str(kk)]), + eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + end; + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; + if ~exist(['Tc_' num2str(kk)]), + eval(['Tc_' num2str(kk) ' = [NaN;NaN;NaN];']); + end; + if ~exist(['omc_' num2str(kk)]), + eval(['omc_' num2str(kk) ' = [NaN;NaN;NaN];']); + eval(['Rc_' num2str(kk) ' = NaN * ones(3,3);']); + end; + if ~exist(['omc_error_' num2str(kk)]), + eval(['omc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Tc_error_' num2str(kk)]), + eval(['Tc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Rc_' num2str(kk)]), + eval(['Rc_' num2str(kk) ' = rodrigues(omc_' num2str(kk) ');']); + end; +end; + +if ~exist('small_calib_image'), + small_calib_image = 0; +end; + +if ~exist('check_cond'), + check_cond = 1; +end; + +if ~exist('MaxIter'), + MaxIter = 30; +end; + +save_name = 'Calib_Results'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile'), + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'Calib_Results' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('Calib_Results.mat',[save_name '.mat']); + disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']); + if exist('Calib_Results.m')==2, + copyfile('Calib_Results.m',[save_name '.m']); + disp(['Copying the current Calib_Results.m file to ' save_name '.m']); + end; + cont_save = 1; + else + disp('The file Calib_Results.mat is about to be changed.'); + cont_save = input('Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'Calib_Results'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' 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; + + else + + fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' 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; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + saving_calib_ascii; + + fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/saving_calib_ascii.m b/mr/Ub5/TOOLBOX_calib/saving_calib_ascii.m new file mode 100755 index 0000000..1478082 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_calib_ascii.m @@ -0,0 +1,74 @@ +if ~exist('save_name'), + save_name = 'Calib_Results'; +end; + +fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) + + +fid = fopen([ save_name '.m'],'wt'); + +fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); +fprintf(fid,'%%\n'); +fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); +fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); +fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); +fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Focal length:\n'); +fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point:\n'); +fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient:\n'); +fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients:\n'); +fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Focal length uncertainty:\n'); +fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point uncertainty:\n'); +fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); +fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); +fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Image size:\n'); +fprintf(fid,'nx = %d;\n',nx); +fprintf(fid,'ny = %d;\n',ny); +fprintf(fid,'\n'); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); +fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); +fprintf(fid,'\n'); +fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); +fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); +fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); +fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); +fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); +fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Extrinsic parameters:\n'); +fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); +fprintf(fid,'\n'); +for kk = 1:n_ima, + fprintf(fid,'%%-- Image #%d:\n',kk); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); + fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); + if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), + eval(['omckk_error = omc_error_' num2str(kk) ';']); + eval(['Tckk_error = Tc_error_' num2str(kk) ';']); + fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); + fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); + end; + fprintf(fid,'\n'); +end; + +fclose(fid); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/saving_calib_ascii_fisheye.m b/mr/Ub5/TOOLBOX_calib/saving_calib_ascii_fisheye.m new file mode 100755 index 0000000..19e7b36 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_calib_ascii_fisheye.m @@ -0,0 +1,74 @@ +if ~exist('save_name'), + save_name = 'Calib_Results'; +end; + +fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) + + +fid = fopen([ save_name '.m'],'wt'); + +fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); +fprintf(fid,'%%\n'); +fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); +fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); +fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); +fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Focal length:\n'); +fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point:\n'); +fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient:\n'); +fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients:\n'); +fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Focal length uncertainty:\n'); +fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Principal point uncertainty:\n'); +fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); +fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); +fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Image size:\n'); +fprintf(fid,'nx = %d;\n',nx); +fprintf(fid,'ny = %d;\n',ny); +fprintf(fid,'\n'); +fprintf(fid,'\n'); +fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); +fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); +fprintf(fid,'\n'); +fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); +fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); +fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); +fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); +fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); +fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); +fprintf(fid,'\n\n'); +fprintf(fid,'%%-- Extrinsic parameters:\n'); +fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); +fprintf(fid,'\n'); +for kk = 1:n_ima, + fprintf(fid,'%%-- Image #%d:\n',kk); + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); + fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); + if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), + eval(['omckk_error = omc_error_' num2str(kk) ';']); + eval(['Tckk_error = Tc_error_' num2str(kk) ';']); + fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); + fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); + end; + fprintf(fid,'\n'); +end; + +fclose(fid); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/saving_calib_fisheye.m b/mr/Ub5/TOOLBOX_calib/saving_calib_fisheye.m new file mode 100755 index 0000000..7584444 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_calib_fisheye.m @@ -0,0 +1,220 @@ + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('est_alpha'), + est_alpha = 0; +end; + +if ~exist('center_optim'), + center_optim = 1; +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('est_dist'), +% if length(est_dist) == 4, +% est_dist = [est_dist ; 0]; +% end; +%end; + +%if exist('kc'), +% if length(kc) == 4; +% kc = [kc;0]; +% end; +%end; + +if ~exist('fc_error'), + fc_error = NaN*ones(2,1); +end; + +if ~exist('kc_error'), + kc_error = NaN*ones(4,1); +end; + +if ~exist('cc_error'), + cc_error = NaN*ones(2,1); +end; + +if ~exist('alpha_c_error'), + alpha_c_error = NaN; +end; + +check_active_images; + +if ~exist('solution_init'), solution_init = []; end; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('solution'), + solution = []; +end; + +if ~exist('param_list'), + param_list = solution; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + +if ~exist('Hcal'), + Hcal = []; +end; +if ~exist('Wcal'), + Wcal = []; +end; +if ~exist('map'), + map = []; +end; + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + if ~exist(['y_' num2str(kk)]), + eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + end; + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; + if ~exist(['Tc_' num2str(kk)]), + eval(['Tc_' num2str(kk) ' = [NaN;NaN;NaN];']); + end; + if ~exist(['omc_' num2str(kk)]), + eval(['omc_' num2str(kk) ' = [NaN;NaN;NaN];']); + eval(['Rc_' num2str(kk) ' = NaN * ones(3,3);']); + end; + if ~exist(['omc_error_' num2str(kk)]), + eval(['omc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Tc_error_' num2str(kk)]), + eval(['Tc_error_' num2str(kk) ' = NaN*ones(3,1);']); + end; + if ~exist(['Rc_' num2str(kk)]), + eval(['Rc_' num2str(kk) ' = rodrigues(omc_' num2str(kk) ');']); + end; +end; + +if ~exist('small_calib_image'), + small_calib_image = 0; +end; + +if ~exist('check_cond'), + check_cond = 1; +end; + +if ~exist('MaxIter'), + MaxIter = 30; +end; + +save_name = 'Calib_Results'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile'), + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'Calib_Results' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('Calib_Results.mat',[save_name '.mat']); + disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']); + if exist('Calib_Results.m')==2, + copyfile('Calib_Results.m',[save_name '.m']); + disp(['Copying the current Calib_Results.m file to ' save_name '.m']); + end; + cont_save = 1; + else + disp('The file Calib_Results.mat is about to be changed.'); + cont_save = input('Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'Calib_Results'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' 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; + + else + + fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active est_alpha est_dist est_aspect_ratio est_fc fc kc cc alpha_c fc_error kc_error cc_error alpha_c_error err_std ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY wintx_default winty_default no_image check_cond MaxIter']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ' H_' 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; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + saving_calib_ascii_fisheye; + + fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/saving_calib_no_results.m b/mr/Ub5/TOOLBOX_calib/saving_calib_no_results.m new file mode 100755 index 0000000..4995687 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_calib_no_results.m @@ -0,0 +1,117 @@ + +if ~exist('n_ima'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('dX'), + dX = []; +end; +if ~exist('dY'), + dY = dX; +end; + + +if ~exist('wintx_default')|~exist('winty_default'), + wintx_default = max(round(nx/128),round(ny/96)); + winty_default = wintx_default; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +if ~exist('err_std'), + err_std = [NaN;NaN]; +end; + +for kk = 1:n_ima, + + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; +end; + +save_name = 'camera_data'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + if exist('copyfile')==2, + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old' num2str(pfn)]; + save_name = [ 'camera_data' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('camera_data.mat',[save_name '.mat']); + disp(['Copying the current camera_calib_data.mat file to ' save_name '.mat']); + cont_save = 1; + else + disp('The file camera_data.mat is about to be changed.'); + cont_save = input(1,'Do you want to continue? ([]=no,other=yes) ','s'); + cont_save = ~isempty(cont_save); + end; +else + cont_save = 1; +end; + + +if cont_save, + + save_name = 'camera_data'; + + if exist('calib_name'), + + fprintf(1,['\nSaving calibration data under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name nx ny 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; + + else + + fprintf(1,['\nSaving calibration data under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' active_images ind_active wintx winty n_ima nx ny 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; + + end; + + + + %fprintf(1,'To load later click on Load\n'); + + eval(string_save); + + fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/saving_stereo_calib.m b/mr/Ub5/TOOLBOX_calib/saving_stereo_calib.m new file mode 100755 index 0000000..612c805 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/saving_stereo_calib.m @@ -0,0 +1,12 @@ + +fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat...\n'); + +string_save = 'save Calib_Results_stereo om R T recompute_intrinsic_right recompute_intrinsic_left calib_name_left format_image_left type_numbering_left image_numbers_left N_slots_left calib_name_right format_image_right type_numbering_right image_numbers_right N_slots_right fc_left cc_left kc_left alpha_c_left KK_left fc_right cc_right kc_right alpha_c_right KK_right active_images dX dY nx ny n_ima active_images_right active_images_left inconsistent_images center_optim_left est_alpha_left est_dist_left est_fc_left est_aspect_ratio_left center_optim_right est_alpha_right est_dist_right est_fc_right est_aspect_ratio_right history param param_error sigma_x om_error T_error fc_left_error cc_left_error kc_left_error alpha_c_left_error fc_right_error cc_right_error kc_right_error alpha_c_right_error'; + +for kk = 1:n_ima, + if active_images(kk), + string_save = [string_save ' X_left_' num2str(kk) ' omc_left_' num2str(kk) ' Tc_left_' num2str(kk) ' omc_left_error_' num2str(kk) ' Tc_left_error_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk)]; + end; +end; +eval(string_save); + diff --git a/mr/Ub5/TOOLBOX_calib/scanner_calibration_script.m b/mr/Ub5/TOOLBOX_calib/scanner_calibration_script.m new file mode 100755 index 0000000..78a905d --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/scanner_calibration_script.m @@ -0,0 +1,466 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%-- Main 3D Scanner Calibration Script +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +clear; + +%-------------------------------------------------------------------------- +%-- STEP 1: Calibration of the camera independently of the projector: +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 1: Calibration of the camera \n'); + +%% Load the corner coordinates in all the 30 images +load camera_data; + +%% Show one example image: +I_cam1 = imread('cam01.bmp'); + +figure(10); +image(I_cam1); +hold on; +plot(x_1(1,:)+1,x_1(2,:)+1,'r+'); +title('Example camera calibration image: cam01.bmp') +hold off; +drawnow; + +figure(11); +plot3(X_1(1,:),X_1(2,:),X_1(3,:),'r+'); +xlabel('X'); +ylabel('Y'); +zlabel('Z'); +title('3D coordinates of the corresponding points (camera calibration)...') +drawnow; + +% Setup the camera model before calibration: +est_dist = [1;0;0;0;0]; %- A first order distortion model is enough +est_alpha = 0; % No Skew needed +center_optim = 1; % Estimate the principal point + +% Run the main calibration routine: +go_calib_optim; + +% Show the camera location with respect to the patterns: +ext_calib; + +% Saving the camera calibration results under camera_results.mat: +saving_calib; +copyfile('Calib_Results.mat','camera_results.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +%-- Save the camera calibration results in side variables: +n_ima_cam = n_ima; + +fc_cam = fc; +cc_cam = cc; +kc_cam = kc; +alpha_c_cam = alpha_c; +fc_error_cam = fc_error; +cc_error_cam = cc_error; +kc_error_cam = kc_error; +alpha_c_error_cam = alpha_c_error; + +est_fc_cam = est_fc; +est_dist_cam = est_dist; +est_alpha_cam = est_alpha; +center_optim_cam = center_optim; +nx_cam = nx; +ny_cam = ny; +active_images_cam = active_images; +ind_active_cam = ind_active; + + +X_1_cam = X_1; +x_1_cam = x_1; +omc_1_cam = omc_1; +Rc_1 = rodrigues(omc_1); +Rc_1_cam = Rc_1; +Tc_1_cam = Tc_1; +omc_error_1_cam = omc_error_1; +Tc_error_1_cam = Tc_error_1; + +dX_cam = dX; +dY_cam = dY; + + +clear fc cc kc alpha_c + + +%%% Saving the calibration solution (for further refinement) +param = solution; +param_cam = param([1:10 16:end]); + + + + + +%-------------------------------------------------------------------------- +%-- STEP 2: Calibration of the projector having done the camera calibration: +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 2: Calibration of the projector (having done projector calibration)...\n'); + +% Load the projector data: + +load projector_data; % load the projector corners (previously saved) + +% Show how an example of data: +I_proj1 = imread('proj01n.bmp'); + +figure(20); +image(I_proj1); +hold on; +plot(xproj_1(1,:)+1,xproj_1(2,:)+1,'r+'); +title('Corner locations in image proj01n.bmp') +hold off; +drawnow; + +figure(21); +plot(x_proj_1(1,:)+1,x_proj_1(2,:)+1,'r+'); +title('Corner locations in the projector image plane'); +xlabel('x (in projector image)'); +ylabel('y (in projector image)'); +drawnow; + + + + +% Start projector calibration making use of the information from camera +% calibration: + +X_proj = []; % 3D coordinates of the points +x_proj = []; % 2D coordinates of the points in the projector image +n_ima_proj = []; + +for kk = ind_active, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc_cam,cc_cam,kc_cam,alpha_c_cam); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); + n_ima_proj = [n_ima_proj kk*ones(1,Np_proj)]; +end; + + +figure(22); +plot(x_proj(1,:)+1,x_proj(2,:)+1,'r+'); +title('Complete set of points in the projector image') +xlabel('x (in projector image)'); +ylabel('y (in projector image)'); +drawnow; + +figure(23); +plot3(X_proj(1,:),X_proj(2,:),X_proj(3,:),'r+'); +axis equal +title('3D coordinates of the projector points (computed using the camera calibration results)') +xlabel('Xc (camera reference frame)'); +ylabel('Yc (camera reference frame)'); +zlabel('Zc (camera reference frame)'); +drawnow; + + +% Projector image size: (may or may not be available) +nx = 1024; +ny = 768; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; + +% Do estimate distortion: +est_dist = [1 0 0 0 0]'; %ones(5,1); +est_alpha = 0; +center_optim = 1; + + +% Run the main projector calibration routine: +go_calib_optim; + + + +%%% Saving the calibration solution (for further refinement) +param = solution; +param_proj = param([1:10 16:end]); + + + +% Shows the extrinsic parameters: +dX = 30; +dY = 30; +ext_calib; + +% Reprojection on the original images: +dont_ask = 1; +reproject_calib; +dont_ask = 0; + +saving_calib; +copyfile('Calib_Results.mat','projector_results.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +%-- Projector parameters: + +fc_proj = fc; +cc_proj = cc; +kc_proj = kc; +alpha_c_proj = alpha_c; +fc_error_proj = fc_error; +cc_error_proj = cc_error; +kc_error_proj = kc_error; +alpha_c_error_proj = alpha_c_error; + +est_fc_proj = est_fc; +est_dist_proj = est_dist; +est_alpha_proj = est_alpha; +center_optim_proj = center_optim; +nx_proj = nx; +ny_proj = ny; +active_images_proj = active_images; +ind_active_proj = ind_active; + +% Position of the global structure wrt the projector: +T_proj = Tc_1; +om_proj = omc_1; +R_proj = rodrigues(om_proj); +T_error_proj = Tc_error_1; +om_error_proj = omc_error_1; + + +%-- Restore the camera calibration information (previously saved in local variables) +n_ima = n_ima_cam; +X_1 = X_1_cam; +x_1 = x_1_cam; +no_image = 0; +dX = dX_cam; +dY = dY_cam; + +omc_1 = omc_1_cam; +Rc_1 = Rc_1_cam; +Tc_1 = Tc_1_cam; +omc_error_1 = omc_error_1_cam; +Tc_error_1 = Tc_error_1_cam; + + +%----------------------- Retrieve calibration results: + +% Intrinsic parameters: + +% Projector: +fp = fc_proj; +cp = cc_proj; +kp = kc_proj; +alpha_p = alpha_c_proj; +fp_error = fc_error_proj; +cp_error = cc_error_proj; +kp_error = kc_error_proj; +alpha_p_error = alpha_c_error_proj; + +% Camera: +fc = fc_cam; +cc = cc_cam; +kc = kc_cam; +alpha_c = alpha_c_cam; +fc_error = fc_error_cam; +cc_error = cc_error_cam; +kc_error = kc_error_cam; +alpha_c_error = alpha_c_error_cam; + +% Extrinsic parameters: + +% Relative position of projector and camera: +T = T_proj; +om = om_proj; +R = R_proj; +T_error = T_error_proj; +om_error = om_error_proj; + + +% Relative prosition of camera wrt world (assuming first pattern as reference -- arbitrary): +omc = omc_1_cam; +Rc = Rc_1_cam; +Tc = Tc_1_cam; + +% Relative position of projector wrt world (assuming first pattern as reference -- arbitrary): +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + + +fprintf(1,'Saving the scanner calibration results in calib_cam_proj.mat...\n'); + +saving_string = 'save calib_cam_proj R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp n_ima active_images_cam active_images_proj ind_active_cam ind_active_proj T_error om_error fc_error cc_error kc_error alpha_c_error fp_error cp_error kp_error alpha_p_error est_fc_cam est_dist_cam est_alpha_cam center_optim_cam est_fc_proj est_dist_proj est_alpha_proj center_optim_proj param_cam param_proj nx_cam ny_cam nx_proj ny_proj'; + +for kk = 1:n_ima, + saving_string = [saving_string ' X_' num2str(kk) ' x_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' omc_error_' num2str(kk) ' Tc_error_' num2str(kk) ]; +end; + +eval(saving_string); + + + +%-------------------------------------------------------------------------- +%-- STEP 3: Global optimization (optimize over all parameters, camera and projector): +%-------------------------------------------------------------------------- + +fprintf(1,'STEP 3: Global optimization...This step may take a while...\n'); + +string_global = 'global n_ima'; +for kk = 1:n_ima, + string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; +end; +eval(string_global); + + +param = [param_cam([1:4 6 11:end]);param_proj([1:4 6 11:end])]; + + +param_init = param; + + +options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; +param = leastsq('error_cam_proj3',param,options); + + +%options = optimset('Display','iter','MaxFunEvals',100,'MaxIter',50); +%param = lsqnonlin('error_cam_proj3',param,options); + + + +%-- Retrive the parameters: + +fc = param(1:2); +cc = param(3:4); +alpha_c = 0; +kc = [param(5);zeros(4,1)]; + +for kk = 1:n_ima, + omckk = param(kk*6:kk*6+2); + Tckk = param(kk*6+3:kk*6+5); + Rckk = rodrigues(omckk); + eval(['omc_' num2str(kk) '= omckk;']); + eval(['Tc_' num2str(kk) '= Tckk;']); + eval(['Rc_' num2str(kk) '= Rckk;']); +end; + +fp = param((1:2)+n_ima * 6 + 5); +cp = param((3:4)+n_ima * 6 + 5); +alpha_p = 0; +kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; + +om = param((6:8)+n_ima * 6 + 5); +T = param((9:11)+n_ima * 6 + 5); +R = rodrigues(om); + +omc = omc_1; +Rc = Rc_1; +Tc = Tc_1; + +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + + +%-- Re-create the parameters: + +param_cam = [param(1:4);0;param(5);zeros(4,1);param(6:5+6*n_ima)]; +param_proj = [param((1:4)+5+6*n_ima);0;param(5+5+6*n_ima);zeros(4,1);param((6:11)+5+6*n_ima)]; + + +fprintf(1,'Saving the optimized scanner calibration results in calib_cam_proj_optim.mat...\n'); + +saving_string = 'save calib_cam_proj_optim R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp n_ima active_images_cam active_images_proj ind_active_cam ind_active_proj est_fc_cam est_dist_cam est_alpha_cam center_optim_cam est_fc_proj est_dist_proj est_alpha_proj center_optim_proj param_cam param_proj param_init nx_cam ny_cam nx_proj ny_proj'; + +for kk = 1:n_ima, + saving_string = [saving_string ' X_' num2str(kk) ' x_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ]; +end; + +eval(saving_string); + + +% Save the optimal camera parameters: + +no_image = 0; + +nx = nx_cam; +ny = ny_cam; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','camera_results_optim.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + + +omc_1_cam = omc_1; +Rc_1_cam = Rc_1; +Tc_1_cam = Tc_1; + +% Save the optimal projector parameters: + + +X_proj = []; +x_proj = []; + +for kk = 1:n_ima, + eval(['xproj = xproj_' num2str(kk) ';']); + xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + Np_proj = size(xproj,2); + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + +fc = fp; +cc = cp; +alpha_c = alpha_p; +kc = kp; + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; +omc_1 = om; +Tc_1 = T; +Rc_1 = R; + +% Image size: (may or may not be available) +nx = nx_proj; +ny = ny_proj; + +% No calibration image is available (only the corner coordinates) +no_image = 1; + +comp_error_calib; + +saving_calib; +copyfile('Calib_Results.mat','projector_results_optim.mat'); +delete('Calib_Results.mat'); +delete('Calib_Results.m'); + +n_ima = n_ima_cam; +X_1 = X_1_cam; +x_1 = x_1_cam; +no_image = 0; +dX = dX_cam; +dY = dY_cam; + +omc_1 = omc_1_cam; +Rc_1 = Rc_1_cam; +Tc_1 = Tc_1_cam; \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/scanning_script.m b/mr/Ub5/TOOLBOX_calib/scanning_script.m new file mode 100755 index 0000000..a3cee7e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/scanning_script.m @@ -0,0 +1,75 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%-- Main 3D Scanning Script +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +if exist('calib_cam_proj_optim.mat')~=2, + error('The scanner calibration file does not exist. Make sure to go to the directory where the scanning images and the calibration file are located (folder scanning_example)'); +end; + +% Loading the scanner calibration parameters: +fprintf(1,'Loading the scanner calibration data...\n'); +load calib_cam_proj_optim; + + +% Choose a dataset (scan #20 for example) +ind_view = 20; +stripe_image = ['strip' sprintf('%.4d',ind_view) ]; + +if exist([stripe_image '_pat00p.bmp'])~=2, + error('The scanning images cannot be found. Make sure to go to the directory where the scanning images are located (folder scanning_example)'); +end; + + +% Compute the projector coordinates at every pixel in the camera image: +fprintf(1,'Computing the subpixel projector coordinates at every pixel in the camera image...\n'); +[xc,xp,nx,ny] = ComputeStripes(stripe_image,1); + + +% Triangulate the 3D geometry: +fprintf(1,'Triangulating the 3D geometry...\n'); +[Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); + + +% Meshing the points: +Thresh_connect = 15; +N_smoothing = 1; +fprintf(1,'Computing the 3D surface mesh...\n'); +[Xc2,tri2,xc2,xp2,dc2,xc_texture,nc2,conf_nc2,Nn2] = Meshing(Xc,xc,xp,Thresh_connect,N_smoothing,om,T,nx,ny,fc,cc,kc,alpha_c,fp,cp,kp,alpha_p); + + +% Display the 3D mesh: +figure(7); +h = trimesh(tri2,Xc2(1,:),Xc2(3,:),-Xc2(2,:)); +set(h,'EdgeColor', 'b'); +xlabel('X'); +ylabel('Y'); +zlabel('Z'); +axis('equal'); +rotate3d on; +view(0.5,12); +axis equal + + + +% Save the mesh in a VRML file: +TT = [0;0;0]; +wwn = [1;0;0]; +nw = pi; +fieldOfView = 3*atan((ny/2)/fc(2)); + +filename_VRML = ['mesh' sprintf('%.4d',ind_view) '.wrl']; + +fprintf(1,'Saving Geometry in the VRML file %s...(use Cosmo Player to visualize the mesh)\n',filename_VRML); + +file = fopen(filename_VRML,'wt'); +fprintf(file ,'#VRML V2.0 utf8\n'); +fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; nw; fieldOfView]); +%fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; 0; atan((ny/2)/fc(2))]); +fprintf(file ,'Transform { children [ Shape { appearance Appearance { material Material { }} geometry IndexedFaceSet { coord Coordinate { point [\n'); +fprintf(file,'%.3f %.3f %.3f,\n',Xc2); +fprintf(file ,']} coordIndex [\n'); +fprintf(file,'%d,%d,%d,-1,\n',tri2'-1); +fprintf(file ,']}}]}\n'); +fprintf(file,']} '); +fclose(file) ; diff --git a/mr/Ub5/TOOLBOX_calib/script_fit_distortion.m b/mr/Ub5/TOOLBOX_calib/script_fit_distortion.m new file mode 100755 index 0000000..c5e5430 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/script_fit_distortion.m @@ -0,0 +1,39 @@ + + satis_distort = 0; + + disp(['Estimated focal: ' num2str(f_g) ' pixels']); + + while ~satis_distort, + + k_g = input('Guess for distortion factor kc ([]=0): '); + + if isempty(k_g), k_g = 0; end; + + xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu; + XX(1,:) = f_g*XX(1,:)+c_g(1); + XX(2,:) = f_g*XX(2,:)+c_g(2); + + figure(2); + image(I); + colormap(map); + zoom on; + hold on; + %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro'); + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be on the grid corners...'); + hold off; + + satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); + + satis_distort = ~isempty(satis_distort); + + + end; + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/script_fit_distortion_fisheye.m b/mr/Ub5/TOOLBOX_calib/script_fit_distortion_fisheye.m new file mode 100755 index 0000000..ef216de --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/script_fit_distortion_fisheye.m @@ -0,0 +1,65 @@ + + satis_distort = 0; + + while ~satis_distort, + + k_g_save = k_g; + + k_g = input(['Guess for distortion factor kc ([]=' num2str(k_g_save) '): ']); + + if isempty(k_g), k_g = k_g_save; end; + + +x_n = (x - c_g(1))/f_g; +y_n = (y - c_g(2))/f_g; + +[x_pn] = comp_fisheye_distortion([x_n' ; y_n'],[k_g;0;0;0]); + + +% Compute the inside points through computation of the planar homography (collineation) + +a00 = [x_pn(1,1);x_pn(2,1);1]; +a10 = [x_pn(1,2);x_pn(2,2);1]; +a11 = [x_pn(1,3);x_pn(2,3);1]; +a01 = [x_pn(1,4);x_pn(2,4);1]; + +% Compute the planar collineation: (return the normalization matrix as well) +[Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + +% Build the grid using the planar collineation: + +x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; +y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; +pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + +XXpn = Homo*pts; +XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); + +XX = apply_fisheye_distortion(XXpn,[k_g;0;0;0]); + +XX(1,:) = f_g*XX(1,:) + c_g(1); +XX(2,:) = f_g*XX(2,:) + c_g(2); + + + + + + + + figure(2); + image(I); + colormap(map); + zoom on; + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be on the grid corners...'); + hold off; + + satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); + + satis_distort = ~isempty(satis_distort); + + + end; + \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/show_calib_results.m b/mr/Ub5/TOOLBOX_calib/show_calib_results.m new file mode 100755 index 0000000..e2e306b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/show_calib_results.m @@ -0,0 +1,57 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist('alpha_c'), + alpha_c = 0; + est_alpha = 0; +end; + +if length(kc) == 4; + kc = [kc;0]; +end; + +if ~exist('est_dist'), + est_dist = (kc~=0); +else + if length(est_dist) == 4; + est_dist = [est_dist;0]; + end; +end; + +if ~exist('err_std'), + comp_error_calib; +end; + + +if ~exist('fc_error'), + + fprintf(1,'\n\nCalibration results:\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 axes = %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]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n\n\n'); + +else + + fprintf(1,'\n\nCalibration results (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]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n',err_std); + fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') + +end; diff --git a/mr/Ub5/TOOLBOX_calib/show_calib_results_fisheye.m b/mr/Ub5/TOOLBOX_calib/show_calib_results_fisheye.m new file mode 100755 index 0000000..bae5cc4 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/show_calib_results_fisheye.m @@ -0,0 +1,57 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist('alpha_c'), + alpha_c = 0; + est_alpha = 0; +end; + +%if length(kc) == 4; +% kc = [kc;0]; +%end; + +%if ~exist('est_dist'), +% est_dist = (kc~=0); +%else +% if length(est_dist) == 4; +% est_dist = [est_dist;0]; +% end; +%end; + +if ~exist('err_std'), + comp_error_calib_fisheye; +end; + + +if ~exist('fc_error'), + + fprintf(1,'\n\nCalibration results:\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 axes = %3.5f degrees\n',[alpha_c],90 - atan(alpha_c)*180/pi); + fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',[kc]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n\n\n'); + +else + + fprintf(1,'\n\nCalibration results (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,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f %3.5f ]\n',[kc;kc_error]); + if n_ima ~= 0, + fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n',err_std); + end; + fprintf(1,'\n',err_std); + fprintf(1,'Note: The numerical errors are approximately three times the standard deviations (for reference).\n\n\n') + +end; diff --git a/mr/Ub5/TOOLBOX_calib/show_stereo_calib_results.m b/mr/Ub5/TOOLBOX_calib/show_stereo_calib_results.m new file mode 100755 index 0000000..16cfb24 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/show_stereo_calib_results.m @@ -0,0 +1,28 @@ + +fprintf(1,'\nStereo calibration parameters:\n'); + + +fprintf(1,'\n\nIntrinsic parameters of left camera:\n\n'); +fprintf(1,'Focal Length: fc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_left;fc_left_error]); +fprintf(1,'Principal point: cc_left = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_left;cc_left_error]); +fprintf(1,'Skew: alpha_c_left = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_left;alpha_c_left_error],90 - atan(alpha_c_left)*180/pi,atan(alpha_c_left_error)*180/pi); +fprintf(1,'Distortion: kc_left = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_left;kc_left_error]); + + +fprintf(1,'\n\nIntrinsic parameters of right camera:\n\n'); +fprintf(1,'Focal Length: fc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[fc_right;fc_right_error]); +fprintf(1,'Principal point: cc_right = [ %3.5f %3.5f ] � [ %3.5f %3.5f ]\n',[cc_right;cc_right_error]); +fprintf(1,'Skew: alpha_c_right = [ %3.5f ] � [ %3.5f ] => angle of pixel axes = %3.5f � %3.5f degrees\n',[alpha_c_right;alpha_c_right_error],90 - atan(alpha_c_right)*180/pi,atan(alpha_c_right_error)*180/pi); +fprintf(1,'Distortion: kc_right = [ %3.5f %3.5f %3.5f %3.5f %5.5f ] � [ %3.5f %3.5f %3.5f %3.5f %5.5f ]\n',[kc_right;kc_right_error]); + + +fprintf(1,'\n\nExtrinsic parameters (position of right camera wrt left camera):\n\n'); +if ~exist('om_error')|~exist('T_error'), + fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ]\n',[om]); + fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ]\n',[T]); +else + fprintf(1,'Rotation vector: om = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[om;om_error]); + fprintf(1,'Translation vector: T = [ %3.5f %3.5f %3.5f ] � [ %3.5f %3.5f %3.5f ]\n',[T;T_error]); +end; + +fprintf(1,'\n\nNote: The numerical errors are approximately three times the standard deviations (for reference).\n\n') diff --git a/mr/Ub5/TOOLBOX_calib/show_window.m b/mr/Ub5/TOOLBOX_calib/show_window.m new file mode 100755 index 0000000..55e053c --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/show_window.m @@ -0,0 +1,77 @@ +function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) + + +if ~exist('cell_list'), + error('No description of the functions'); +end; + +if ~exist('fig_number'), + fig_number = 1; +end; +if ~exist('title_figure'), + title_figure = ''; +end; +if ~exist('x_size'), + x_size = 85; +end; +if ~exist('y_size'), + y_size = 14; +end; +if ~exist('gap_x'), + gap_x = 0; +end; +if ~exist('font_name'), + font_name = 'clean'; +end; +if ~exist('font_size'), + font_size = 8; +end; + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +[n_row,n_col] = size(cell_list); + +fig_size_x = x_size*n_col+(n_col+1)*gap_x; +fig_size_y = y_size*n_row+(n_row+1)*gap_x; + +set(fig_number,'Units','points', ... + 'BackingStore','off', ... + 'Color',[0.8 0.8 0.8], ... + 'MenuBar','none', ... + 'Resize','off', ... + 'Name',title_figure, ... +'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... +'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); + +h_mat = zeros(n_row,n_col); + +posx = zeros(n_row,n_col); +posy = zeros(n_row,n_col); + +for i=n_row:-1:1, + for j = n_col:-1:1, + posx(i,j) = gap_x+(j-1)*(x_size+gap_x); + posy(i,j) = fig_size_y - i*(gap_x+y_size); + end; +end; + +for i=n_row:-1:1, + for j = n_col:-1:1, + if ~isempty(cell_list{i,j}), + if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), + h_mat(i,j) = uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',cell_list{i,j}{2}, ... + 'ListboxTop',0, ... + 'Position',[posx(i,j) posy(i,j) x_size y_size], ... + 'String',cell_list{i,j}{1}, ... + 'fontsize',font_size,... + 'fontname',font_name,... + 'Tag','Pushbutton1'); + end; + end; + end; +end; + +%------ END PROTECTED REGION ----------------% diff --git a/mr/Ub5/TOOLBOX_calib/skew3.m b/mr/Ub5/TOOLBOX_calib/skew3.m new file mode 100755 index 0000000..449a13b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/skew3.m @@ -0,0 +1,27 @@ +function [S,dSdT] = skew3(T); + +S = [ 0 -T(3) T(2) + T(3) 0 -T(1) + -T(2) T(1) 0 ]; + +dSdT = [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]; + +return; + + +% Test of Jacobian: + +T1 = randn(3,1); + +dT = 0.001*randn(3,1); + +T2 = T1 + dT; + +[S1,dSdT] = skew3(T1); +[S2] = skew3(T2); + +S2app = S1; +S2app(:) = S2app(:) + dSdT*dT; + + +norm(S1 - S2) / norm(S2app - S2) diff --git a/mr/Ub5/TOOLBOX_calib/small_test_script.m b/mr/Ub5/TOOLBOX_calib/small_test_script.m new file mode 100755 index 0000000..97ddf31 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/small_test_script.m @@ -0,0 +1,65 @@ +% This is small script that demonstrate the computation of extrinsic +% parameters using 3D structures. +% This test was build from data provided by Daniel Small (thank you Daniel!) + + +%-- Image points (in pixels): + +x = [479.5200 236.0800 + 608.4100 415.3700 + 461.0000 40.0000 + 451.4800 308.7000 + 373.9900 314.8900 + 299.3200 319.1300 + 231.5500 321.3700 + 443.7300 282.9200 + 378.3600 288.3000 + 314.6900 292.7400 + 255.4700 296.2300]'; + + +% 3D world coordinates: + +X = [ 0 0 0 + 54.0000 0 0 + 0 0 40.5000 + 27.0000 -8.4685 -2.3750 + 27.0000 -18.4685 -2.3750 + 27.0000 -28.4685 -2.3750 + 27.0000 -38.4685 -2.3750 + 17.0000 -8.4685 -2.3750 + 17.0000 -18.4685 -2.3750 + 17.0000 -28.4685 -2.3750 + 17.0000 -38.4685 -2.3750]'; + + +%------------ Intrinsic parameters: +%--- focal: +fc = [ 395.0669 357.1178 ]'; +%--- Principal point: +cc = [ 380.5387 230.5278 ]'; +%--- Distortion coefficients: +kc = [-0.2601 0.0702 -0.0019 -0.0003 0]'; +%--- Skew coefficient: +alpha_c = 0; + +%----- Computation of the pose of the object in space +%----- (or the rigid motion between world reference frame and camera ref. frame) +[om,T,R] = compute_extrinsic(x,X,fc,cc,kc,alpha_c); + +%--- Try to reproject the structure to see if the computed pose makes sense: +x2 = project_points2(X_1,omckk,Tckk,fc,cc,kc,alpha_c); + + +% Graphical output: +figure(2); +plot(x(1,:),x(2,:),'r+'); +hold on; +plot(x2(1,:),x2(2,:),'go'); +hold off; +axis('equal'); +axis('image'); +title('red crosses: data, green circles: reprojected structure -- IT WORKS!!!'); +xlabel('x'); +ylabel('y'); + diff --git a/mr/Ub5/TOOLBOX_calib/smooth_images.m b/mr/Ub5/TOOLBOX_calib/smooth_images.m new file mode 100755 index 0000000..e570b56 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/smooth_images.m @@ -0,0 +1,94 @@ +% Anisotropically diffuse the calibration image +% to enhance the corner detection + + +fprintf(1,'Anisotropic diffusion of the images for corner enhancement (the images have to be loaded in memory)\n'); + + +ker = [1/4 1/2 1/4]; +ker2 = conv2(ker,ker); +ker2 = conv2(ker2,ker); +ker2 = conv2(ker2,ker); + + + +if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; +end; + +check_active_images; + +format_image2 = format_image; +if format_image2(1) == 'j', + format_image2 = 'bmp'; +end; + +for kk = 1:n_ima, + + if exist(['I_' num2str(kk)]), + + %fprintf(1,'%d...',kk); + + eval(['I = I_' num2str(kk) ';']); + + + % Compute the sigI automatically: + [nn,xx] = hist(I(:),50); + nn = conv2(nn,ker2,'same'); + + max_nn = max(nn); + + + localmax = [0 (nn(2:end-1)>=nn(3:end)) & (nn(2:end-1) > nn(1:end-2)) 0] .* (nn >= max_nn/5); + + %plot(xx,nn); + %hold on; + %plot(xx,nn .* localmax,'r' ); + %hold off; + + localmax_ind = find(localmax); + nn_local_max = nn(localmax_ind); + + % order the picks: + [a,b] = sort(-nn_local_max); + + localmax_ind = localmax_ind(b); + nn_local_max = nn_local_max(b); + + sig_I = abs(xx(localmax_ind(1)) - xx(localmax_ind(2)))/4.25; + + + + + I2 = anisdiff(I,sig_I,30); + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_smth' number_ext '.' format_image2]; + + fprintf(1,['Saving smoothed image under ' ima_name2 '...\n']); + + if format_image2(1) == 'p', + if format_images2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + end; + +end; + +fprintf(1,'\ndone\n'); \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/startup.m b/mr/Ub5/TOOLBOX_calib/startup.m new file mode 100755 index 0000000..e12abf9 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/startup.m @@ -0,0 +1,2 @@ +% Main camera calibration toolbox: +format compact diff --git a/mr/Ub5/TOOLBOX_calib/stereo_gui.m b/mr/Ub5/TOOLBOX_calib/stereo_gui.m new file mode 100755 index 0000000..0197d3e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/stereo_gui.m @@ -0,0 +1,155 @@ +% stereo_gui +% Stereo Camera Calibration Toolbox (two cameras, internal and external calibration): +% +% It is assumed that the two cameras (left and right) have been calibrated with the pattern at the same 3D locations, and the same points +% on the pattern (select the same grid points). Therefore, in particular, the same number of images were used to calibrate both cameras. +% The two calibration result files must have been saved under two seperate data files (Calib_Results_left.mat and Calib_Results_right.mat) +% prior to running this toolbox. To generate the two files, run the classic Camera Calibration toolbox calib.m. +% +% INPUT: Calib_Results_left.mat, Calib_Results_right.mat -> Generated by the standard calibration toolbox on the two cameras individually +% OUTPUT: Calib_Results_stereo.mat -> The saved result after global stereo calibration (after running stereo calibration, and hitting Save stereo calib results) +% +% Main result variables stored in Calib_Results_stereo.mat: +% om, R, T: relative rotation and translation of the right camera wrt the left camera +% fc_left, cc_left, kc_left, alpha_c_left, KK_left: New intrinsic parameters of the left camera +% fc_right, cc_right, kc_right, alpha_c_right, KK_right: New intrinsic parameters of the right camera +% +% Both sets of intrinsic parameters are equivalent to the classical {fc,cc,kc,alpha_c,KK} described online at: +% http://www.vision.caltech.edu/bouguetj/calib_doc/parameters.html +% +% Note: If you do not want to recompute the intinsic parameters, through stereo calibration you may want to set +% recompute_intrinsic_right and recompute_intrinsic_left to zero, prior to running stereo calibration. Default: 1 +% +% Definition of the extrinsic parameters: R and om are related through the rodrigues formula (R=rodrigues(om)). +% Consider a point P of coordinates XL and XR in the left and right camera reference frames respectively. +% XL and XR are related to each other through the following rigid motion transformation: +% XR = R * XL + T +% R and T (or equivalently om and T) fully describe the relative displacement of the two cameras. +% +% +% If the Warning message "Disabling view kk - Reason: the left and right images are found inconsistent" is encountered during stereo calibration, +% that probably means that for the kkth pair of images, the left and right images are found to have captured the calibration pattern at two +% different locations in space. That means that the two views are not consistent, and therefore cannot be used for stereo calibration. +% When capturing your images, make sure that you do not move the calibration pattern between capturing the left and the right images. +% The pattwern can (and should) be moved in space only between two sets of (left,right) images. +% Another reason for inconsistency is that you selected a different set of points on the pattern when running the separate calibrations +% (leading to the two files Calib_Results_left.mat and Calib_Results_left.mat). Make sure that the same points are selected in the +% two separate calibration. In other words, the points need to correspond. + +% (c) Jean-Yves Bouguet - Intel Corporation +% October 25, 2001 -- Last updated June 14, 2004 + +function stereo_gui, + + +cell_list = {}; + + +%-------- Begin editable region -------------% +%-------- Begin editable region -------------% + + +fig_number = 1; + +title_figure = 'Stereo Camera Calibration Toolbox'; + +cell_list{1,1} = {'Load left and right calibration files','load_stereo_calib_files;'}; +cell_list{1,2} = {'Run stereo calibration','go_calib_stereo;'}; +cell_list{2,1} = {'Show Extrinsics of stereo rig','ext_calib_stereo;'}; +cell_list{2,2} = {'Show Intrinsic parameters','show_stereo_calib_results;'}; +cell_list{3,1} = {'Save stereo calib results','saving_stereo_calib;'}; +cell_list{3,2} = {'Load stereo calib results','loading_stereo_calib;'}; +cell_list{4,1} = {'Rectify the calibration images','rectify_stereo_pair;'}; +cell_list{4,2} = {'Exit',['disp(''Bye. To run again, type stereo_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; + + +show_window(cell_list,fig_number,title_figure,150,14); + + +%-------- End editable region -------------% +%-------- End editable region -------------% + + + + + + +%------- DO NOT EDIT ANYTHING BELOW THIS LINE -----------% + +function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) + + +if ~exist('cell_list'), + error('No description of the functions'); +end; + +if ~exist('fig_number'), + fig_number = 1; +end; +if ~exist('title_figure'), + title_figure = ''; +end; +if ~exist('x_size'), + x_size = 85; +end; +if ~exist('y_size'), + y_size = 14; +end; +if ~exist('gap_x'), + gap_x = 0; +end; +if ~exist('font_name'), + font_name = 'clean'; +end; +if ~exist('font_size'), + font_size = 8; +end; + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +[n_row,n_col] = size(cell_list); + +fig_size_x = x_size*n_col+(n_col+1)*gap_x; +fig_size_y = y_size*n_row+(n_row+1)*gap_x; + +set(fig_number,'Units','points', ... + 'BackingStore','off', ... + 'Color',[0.8 0.8 0.8], ... + 'MenuBar','none', ... + 'Resize','off', ... + 'Name',title_figure, ... +'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... +'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); + +h_mat = zeros(n_row,n_col); + +posx = zeros(n_row,n_col); +posy = zeros(n_row,n_col); + +for i=n_row:-1:1, + for j = n_col:-1:1, + posx(i,j) = gap_x+(j-1)*(x_size+gap_x); + posy(i,j) = fig_size_y - i*(gap_x+y_size); + end; +end; + +for i=n_row:-1:1, + for j = n_col:-1:1, + if ~isempty(cell_list{i,j}), + if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), + h_mat(i,j) = uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',cell_list{i,j}{2}, ... + 'ListboxTop',0, ... + 'Position',[posx(i,j) posy(i,j) x_size y_size], ... + 'String',cell_list{i,j}{1}, ... + 'fontsize',font_size,... + 'fontname',font_name,... + 'Tag','Pushbutton1'); + end; + end; + end; +end; + +%------ END PROTECTED REGION ----------------% diff --git a/mr/Ub5/TOOLBOX_calib/stereo_triangulation.m b/mr/Ub5/TOOLBOX_calib/stereo_triangulation.m new file mode 100755 index 0000000..8b3f351 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/stereo_triangulation.m @@ -0,0 +1,73 @@ +function [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), + +% [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), +% +% Function that computes the position of a set on N points given the left and right image projections. +% The cameras are assumed to be calibrated, intrinsically, and extrinsically. +% +% Input: +% xL: 2xN matrix of pixel coordinates in the left image +% xR: 2xN matrix of pixel coordinates in the right image +% om,T: rotation vector and translation vector between right and left cameras (output of stereo calibration) +% fc_left,cc_left,...: intrinsic parameters of the left camera (output of stereo calibration) +% fc_right,cc_right,...: intrinsic parameters of the right camera (output of stereo calibration) +% +% Output: +% +% XL: 3xN matrix of coordinates of the points in the left camera reference frame +% XR: 3xN matrix of coordinates of the points in the right camera reference frame +% +% Note: XR and XL are related to each other through the rigid motion equation: XR = R * XL + T, where R = rodrigues(om) +% For more information, visit http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html +% +% +% (c) Jean-Yves Bouguet - Intel Corporation - April 9th, 2003 + + + +%--- Normalize the image projection according to the intrinsic parameters of the left and right cameras +xt = normalize_pixel(xL,fc_left,cc_left,kc_left,alpha_c_left); +xtt = normalize_pixel(xR,fc_right,cc_right,kc_right,alpha_c_right); + +%--- Extend the normalized projections in homogeneous coordinates +xt = [xt;ones(1,size(xt,2))]; +xtt = [xtt;ones(1,size(xtt,2))]; + +%--- Number of points: +N = size(xt,2); + +%--- Rotation matrix corresponding to the rigid motion between left and right cameras: +R = rodrigues(om); + + +%--- Triangulation of the rays in 3D space: + +u = R * xt; + +n_xt2 = dot(xt,xt); +n_xtt2 = dot(xtt,xtt); + +T_vect = repmat(T, [1 N]); + +DD = n_xt2 .* n_xtt2 - dot(u,xtt).^2; + +dot_uT = dot(u,T_vect); +dot_xttT = dot(xtt,T_vect); +dot_xttu = dot(u,xtt); + +NN1 = dot_xttu.*dot_xttT - n_xtt2 .* dot_uT; +NN2 = n_xt2.*dot_xttT - dot_uT.*dot_xttu; + +Zt = NN1./DD; +Ztt = NN2./DD; + +X1 = xt .* repmat(Zt,[3 1]); +X2 = R'*(xtt.*repmat(Ztt,[3,1]) - T_vect); + + +%--- Left coordinates: +XL = 1/2 * (X1 + X2); + +%--- Right coordinates: +XR = R*XL + T_vect; + diff --git a/mr/Ub5/TOOLBOX_calib/undistort_image.m b/mr/Ub5/TOOLBOX_calib/undistort_image.m new file mode 100755 index 0000000..b0029f7 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/undistort_image.m @@ -0,0 +1,206 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +%%% Compute the new KK matrix to fit as much data in the image (in order to +%%% accomodate large distortions: +r2_extreme = (nx^2/(4*fc(1)^2) + ny^2/(4*fc(2)^2)); +dist_amount = 1; %(1+kc(1)*r2_extreme + kc(2)*r2_extreme^2); +fc_new = dist_amount * fc; + +KK_new = [fc_new(1) alpha_c*fc_new(1) cc(1);0 fc_new(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest), + quest = 0; +end; + +if ~quest, + + if n_ima == 0, + fprintf(1,'No image data available\n'); + return; + end; + + if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + end; + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j', + format_image2 = 'bmp'; + end; + + for kk = 1:n_ima, + + if exist(['I_' num2str(kk)]), + + eval(['I = I_' num2str(kk) ';']); + [I2] = rect(I,eye(3),fc,cc,kc,KK_new); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']); + + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + + end; + + end; + + fprintf(1,'done\n'); + +else + + dir; + fprintf(1,'\n'); + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + if size(I,3)>1, + I = I(:,:,2); + end; + + + if (size(I,1)>ny)|(size(I,2)>nx), + I = I(1:ny,1:nx); + end; + + + %% SHOW THE ORIGINAL IMAGE: + + figure(2); + image(I); + colormap(gray(256)); + title('Original image (with distortion) - Stored in array I'); + drawnow; + + + %% UNDISTORT THE IMAGE: + + fprintf(1,'Computing the undistorted image...') + + [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK_new); + + fprintf(1,'done\n'); + + figure(3); + image(I2); + colormap(gray(256)); + title('Undistorted image - Stored in array I2'); + drawnow; + + + %% SAVE THE IMAGE IN FILE: + + ima_name2 = [image_name '_rect.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']); + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/undistort_image_color.m b/mr/Ub5/TOOLBOX_calib/undistort_image_color.m new file mode 100755 index 0000000..570b1cc --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/undistort_image_color.m @@ -0,0 +1,174 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c') + fprintf(1,'No intrinsic camera parameters available.\n'); + return +end + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that corrects distorted images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n') + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest) + quest = 0; +end + +if ~quest + if ~exist(['I_' num2str(ind_active(1))]) + ima_read_calib; + end + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j' + format_image2 = 'bmp'; + end + + for kk = 1:n_ima + if exist(['I_' num2str(kk)]) + eval(['I = I_' num2str(kk) ';']); + [I2] = rect(I,eye(3),fc,cc,kc,KK); + + if ~type_numbering + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']) + + if format_image2(1) == 'p' + if format_image2(2) == 'p' + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end + else + if format_image2(1) == 'r' + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end + end + end + end + + fprintf(1,'done\n') +else + dir + fprintf(1,'\n') + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0' + format_image2 = input('Image format ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm''): ','s'); + + if isempty(format_image2) + format_image2 = 'ras'; + end + + if lower(format_image2(1)) == 'm' + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b' + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't' + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p' + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j' + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r' + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end + end + end + end + end + end + end + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + if format_image2(1) == 'p' + if format_image2(2) == 'p' + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end + else + if format_image2(1) == 'r' + I = readras(ima_name); + else + I = double(imread(ima_name)); + end + end + + if (size(I,1)>ny)|(size(I,2)>nx) + I = I(1:ny,1:nx); + end + + %% SHOW THE ORIGINAL IMAGE: + figure(2); + image(uint8(I)); + title('Original image (with distortion) - Stored in array I') + drawnow; + + %% UNDISTORT THE IMAGE: + fprintf(1,'Computing the undistorted image...') + + [Ipart_1] = rect(I(:,:,1),eye(3),fc,cc,kc,alpha_c,KK); + [Ipart_2] = rect(I(:,:,2),eye(3),fc,cc,kc,alpha_c,KK); + [Ipart_3] = rect(I(:,:,3),eye(3),fc,cc,kc,alpha_c,KK); + + I2 = ones(ny, nx,3); + I2(:,:,1) = Ipart_1; + I2(:,:,2) = Ipart_2; + I2(:,:,3) = Ipart_3; + + fprintf(1,'done\n') + + figure(3); + image(uint8(I2)); + title('Undistorted image - Stored in array I2') + drawnow; + + %% SAVE THE IMAGE IN FILE: + ima_name2 = [image_name '_rect_color.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']) + + if format_image2(1) == 'p' + if format_image2(2) == 'p' + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end + else + if format_image2(1) == 'r' + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),ima_name2,format_image2); + end + end + + fprintf(1,'done\n') +end \ No newline at end of file diff --git a/mr/Ub5/TOOLBOX_calib/undistort_image_no_read.m b/mr/Ub5/TOOLBOX_calib/undistort_image_no_read.m new file mode 100755 index 0000000..23aaa46 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/undistort_image_no_read.m @@ -0,0 +1,232 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest), + quest = 0; +end; + +if ~quest, + + %if ~exist(['I_' num2str(ind_active(1))]), + %ima_read_calib; + %end; + + if n_ima == 0, + fprintf(1,'No image data available\n'); + return; + end; + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j', + format_image2 = 'bmp'; + end; + + for kk = 1:n_ima, + + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + + if ~exist(ima_name), + + fprintf(1,'Image %s not found!!!\n',ima_name); + + else + + fprintf(1,'Loading image %s...\n',ima_name); + + if format_image(1) == 'p', + if format_image(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + + if size(I,3)>1, + I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); + end; + + [I2] = rect(I,eye(3),fc,cc,kc,KK); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']); + + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + + end; + + end; + + fprintf(1,'done\n'); + +else + + dir; + fprintf(1,'\n'); + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + + while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + + ima_name = [image_name '.' format_image2]; + + + %%% READ IN IMAGE: + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + if size(I,3)>1, + I = I(:,:,2); + end; + + + if (size(I,1)>ny)|(size(I,2)>nx), + I = I(1:ny,1:nx); + end; + + + %% SHOW THE ORIGINAL IMAGE: + + figure(2); + image(I); + colormap(gray(256)); + title('Original image (with distortion) - Stored in array I'); + drawnow; + + + %% UNDISTORT THE IMAGE: + + fprintf(1,'Computing the undistorted image...') + + [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK); + + fprintf(1,'done\n'); + + figure(3); + image(I2); + colormap(gray(256)); + title('Undistorted image - Stored in array I2'); + drawnow; + + + %% SAVE THE IMAGE IN FILE: + + ima_name2 = [image_name '_rect.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']); + + if format_image2(1) == 'p', + if format_image2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + fprintf(1,'done\n'); + +end; diff --git a/mr/Ub5/TOOLBOX_calib/undistort_sequence.m b/mr/Ub5/TOOLBOX_calib/undistort_sequence.m new file mode 100755 index 0000000..f11787b --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/undistort_sequence.m @@ -0,0 +1,161 @@ +%%% INPUT THE IMAGE FILE NAME: + +graphout = 0; + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available. Maybe, need to load Calib_Results.mat\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts a whole sequence of images (works with bmp only so far... needs some debugging)'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); +disp('After undistortion, the intrinsic parameters fc, cc, alpha_c remain unchanged. The distortion coefficient vector kc is zero'); + +dir; + +fprintf(1,'\n'); + +seq_name = input('Basename of sequence images (without number nor suffix): ','s'); + +format_image_seq = '0'; + +while format_image_seq == '0', + format_image_seq = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + if isempty(format_image_seq), + format_image_seq = 'ras'; + else + if lower(format_image_seq(1)) == 'm', + format_image_seq = 'ppm'; + else + if lower(format_image_seq(1)) == 'b', + format_image_seq = 'bmp'; + else + if lower(format_image_seq(1)) == 't', + format_image_seq = 'tif'; + else + if lower(format_image_seq(1)) == 'p', + format_image_seq = 'pgm'; + else + if lower(format_image_seq(1)) == 'j', + format_image_seq = 'jpg'; + else + if lower(format_image_seq(1)) == 'r', + format_image_seq = 'ras'; + else + disp('Invalid image format'); + format_image_seq = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; +end; + + +ima_sequence = dir( [ seq_name '*.' format_image_seq]); + +if isempty(ima_sequence), + fprintf(1,'No image found\n'); + return; +end; + +ima_name = ima_sequence(1).name; +if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image_seq(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +[ny,nx,nc] = size(I); + + +% Pre-compute the necessary indices and blending coefficients to enable quick rectification: +[Irec_junk,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(zeros(ny,nx),eye(3),fc,cc,kc,alpha_c,KK); + + +n_seq = length(ima_sequence); + + +for kk = 1:n_seq, + + ima_name = ima_sequence(kk).name; + + fprintf(1,'Loading original image %s...',ima_name); + + %%% READ IN IMAGE: + + if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; + else + if format_image_seq(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; + end; + + [ny,nx,nc] = size(I); + + if graphout, + figure(2); + image(uint8(I)); + drawnow; + end; + + I2 = zeros(ny,nx,nc); + + for ii = 1:nc, + + Iii = I(:,:,ii); + I2ii = zeros(ny,nx); + + I2ii(ind_new) = uint8(a1 .* Iii(ind_1) + a2 .* Iii(ind_2) + a3 .* Iii(ind_3) + a4 .* Iii(ind_4)); + + I2(:,:,ii) = I2ii; + + end; + + I2 = uint8(I2); + + if graphout, + figure(3); + image(I2); + drawnow; + end; + + ima_name2 = ['undist_' ima_name]; + + fprintf(1,'Saving undistorted image under %s...\n',ima_name2); + + if format_image_seq(1) == 'p', + if format_image_seq(2) == 'p', + saveppm(ima_name2,I2); + else + savepgm(ima_name2,I2); + end; + else + if format_image_seq(1) == 'r', + writeras(ima_name2,I2,gray(256)); + else + imwrite(I2,ima_name2,format_image_seq); + end; + end; + + +end; diff --git a/mr/Ub5/TOOLBOX_calib/visualize_distortions.m b/mr/Ub5/TOOLBOX_calib/visualize_distortions.m new file mode 100755 index 0000000..ac515d0 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/visualize_distortions.m @@ -0,0 +1,217 @@ +% visualize_distortions +% +% +% A script to run in conjunction with calib_gui in TOOLBOX_calib to plot +% the distortion models. +% +% This is a slightly modified version of the script plot_CCT_distortion.m written by Mr. Oshel +% Thank you Mr. Oshel for your contribution! + + +[mx,my] = meshgrid(0:nx/20:(nx-1),0:ny/20:(ny-1)); +[nnx,nny]=size(mx); +px=reshape(mx',nnx*nny,1); +py=reshape(my',nnx*nny,1); +kk_new=[fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2);0 0 1]; +rays=inv(kk_new)*[px';py';ones(1,length(px))]; +x=[rays(1,:)./rays(3,:);rays(2,:)./rays(3,:)]; + + +title2=strcat('Complete Distortion Model'); + +fh1 = 2; + +%if ishandle(fh1), +% close(fh1); +%end; +figure(fh1); clf; +xd=apply_distortion(x,kc); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh1,'color',[1,1,1]); + +hold off; + + + + + +title2=strcat('Tangential Component of the Distortion Model'); + +fh2 = 3; + +%if ishandle(fh2), +% close(fh2); +%end; +figure(fh2); clf; +xd=apply_distortion(x,[0 0 kc(3) kc(4) 0]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh2,'color',[1,1,1]); + +hold off; + + + + + + + + +title2=strcat('Radial Component of the Distortion Model'); + +fh3 = 4; + +%if ishandle(fh3), +% close(fh3); +%end; +figure(fh3); clf; +xd=apply_distortion(x,[kc(1) kc(2) 0 0 kc(5)]); +px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1); +py2=fc(2)*xd(2,:)+cc(2); +dx=px2'-px; +dy=py2'-py; +Q=quiver(px+1,py+1,dx,dy); +hold on; +plot(cc(1)+1,cc(2)+1,'o'); +plot((nx-1)/2+1,(ny-1)/2+1,'x'); +dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)'; +[C,h]=contour(mx,my,dr,'k'); +clabel(C,h); +Mean=mean(mean(dr)); +Max=max(max(dr)); +title(title2); + +axis ij; +axis([1 nx 1 ny]) +axis equal; +axis tight; + +position=get(gca,'Position'); +shr = 0.9; +position(1)=position(1)+position(3)*((1-shr)/2); +position(2)=position(2)+position(4)*(1-shr)+0.03; +position(3:4)=position(3:4)*shr; +set(gca,'position',position); +set(gca,'fontsize',8,'fontname','clean') + +gh = gca; + +line1=sprintf('Principal Point = (%0.6g, %0.6g)',cc(1),cc(2)); +line2=sprintf('Focal Length = (%0.6g, %0.6g)',fc(1),fc(2)); +line3=sprintf('Radial coefficients = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5)); +line4=sprintf('Tangential coefficients = (%0.4g, %0.4g)',kc(3),kc(4)); +line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2)); +line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2)); +line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5)); +line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4)); +line9=sprintf('Pixel error = [%0.4g, %0.4g]',err_std(1),err_std(2)); +line10=sprintf('Skew = %0.4g',alpha_c); +line11=sprintf('+/- %0.4g',alpha_c_error); + + +axes('position',[0 0 1 1],'visible','off'); +th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean'); +th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean'); +%set(th,'FontName','fixed'); +axes(gh); + +set(fh3,'color',[1,1,1]); + +hold off; + +figure(fh1); diff --git a/mr/Ub5/TOOLBOX_calib/willson_convert.m b/mr/Ub5/TOOLBOX_calib/willson_convert.m new file mode 100755 index 0000000..9527181 --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/willson_convert.m @@ -0,0 +1,95 @@ +function [fc,cc,kc,alpha_c,Rc,Tc,omc,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + +%Conversion from Reg Willson's calibration format to my format + +% Conversion: + +% Focal length: +fc = [sx/dpx ; 1/dpy]*f; + +% Principal point; +cc = [Cx;Cy]; + +% Skew: +alpha_c = 0; + +% Extrinsic parameters: +Rx = rodrigues([Rx;0;0]); +Ry = rodrigues([0;Ry;0]); +Rz = rodrigues([0;0;Rz]); + +Rc = Rz * Ry * Rx; + +omc = rodrigues(Rc); + +Tc = [Tx;Ty;Tz]; + + +% More tricky: Take care of the distorsion: + +Nfy = round(Nfx * 3/4); + +nx = Nfx; +ny = Nfy; + +% Select a set of DISTORTED coordinates uniformely distributed across the image: + +[xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy); + +xp_dist = xp_dist(:)'; +yp_dist = yp_dist(:)'; + + +% Apply UNDISTORTION according to Willson: + +xp_sensor_dist = dpx*(xp_dist - Cx)/sx; +yp_sensor_dist = dpy*(yp_dist - Cy); + +dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2); + +xp_sensor = xp_sensor_dist .* dist_fact; +yp_sensor = yp_sensor_dist .* dist_fact; + +xp = xp_sensor * sx / dpx + Cx; +yp = yp_sensor / dpy + Cy; + +ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1)); + +xp = xp(ind); +yp = yp(ind); +xp_dist = xp_dist(ind); +yp_dist = yp_dist(ind); + + +% Now, find my own set of parameters: + +x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)]; +x_dist(1,:) = x_dist(1,:) - alpha_c * x_dist(2,:); + +x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)]; +x(1,:) = x(1,:) - alpha_c * x(2,:); + +k = [0;0;0;0;0]; + +for kk = 1:5, + + [xd,dxddk] = apply_distortion(x,k); + + err = x_dist - xd; + + %norm(err) + + k_step = inv(dxddk'*dxddk)*(dxddk')*err(:); + + k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:); + + %norm(k_step)/norm(k) + + if norm(k_step)/norm(k) < 10e-10, + break; + end; + +end; + + +kc = k; diff --git a/mr/Ub5/TOOLBOX_calib/willson_read.m b/mr/Ub5/TOOLBOX_calib/willson_read.m new file mode 100755 index 0000000..60e57ff --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/willson_read.m @@ -0,0 +1,92 @@ +% Read in Reg Willson's data file, and convert it into my data format: + +if exist('calib_file'), + if exist(calib_file)~=2, + ask = 1; + else + ask = 0; + end; +else + ask = 1; +end; + + +while ask, + + calib_file = input('Reg Willson calibration file name to convert: ','s'); + + if ~isempty(calib_file), + if exist(calib_file)~=2, + ask = 1; + else + ask = 0; + end; + else + ask = 1; + end; + + if ask, + fprintf(1,'File not found. Try again.\n'); + end; + +end; + +if exist(calib_file), + + fprintf(1,['Loading Willson calibration parameters from file ' calib_file '...\n']); + + load(calib_file); + + inddot = find(calib_file == '.'); + + if isempty(inddot), + varname = calib_file; + else + varname = calib_file(1:inddot(1)-1); + end; + + eval(['calib_params = ' varname ';']) + + Ncx = calib_params(1); + Nfx = calib_params(2); + dx = calib_params(3); + dy = calib_params(4); + dpx = calib_params(5); + dpy = calib_params(6); + Cx = calib_params(7); + Cy = calib_params(8); + sx = calib_params(9); + f = calib_params(10); + kappa1 = calib_params(11); + Tx = calib_params(12); + Ty = calib_params(13); + Tz = calib_params(14); + Rx = calib_params(15); + Ry = calib_params(16); + Rz = calib_params(17); + p1 = calib_params(18); + p2 = calib_params(19); + + fprintf(1,'Converting the calibration parameters... (may take a while due to the convertion of the distortion model)...\n'); + + % Conversion: + [fc,cc,kc,alpha_c,Rc_1,Tc_1,omc_1,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + + KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + + n_ima = 1; + + X_1 = [NaN;NaN;NaN]; + x_1 = [NaN;NaN]; + + map = gray(256); + + fprintf(1,'done\n'); + fprintf(1,'You are now ready to save the calibration parameters by clicking on Save.\n'); + +else + + disp(['WARNING: Calibration file ' calib_file ' not found']); + +end; + diff --git a/mr/Ub5/TOOLBOX_calib/write_image.m b/mr/Ub5/TOOLBOX_calib/write_image.m new file mode 100755 index 0000000..840428e --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/write_image.m @@ -0,0 +1,30 @@ +function [] = write_image(I, kk , calib_name , format_image , type_numbering , image_numbers , N_slots), + +if format_image(1) == 'j', + format_image = 'bmp'; +end; + + +if ~type_numbering, + number_ext = num2str(image_numbers(kk)); +else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); +end; + +ima_name2 = [calib_name number_ext '.' format_image]; + +fprintf(1,['Saving image under ' ima_name2 '...\n']); + +if format_image(1) == 'p', + if format_image(2) == 'p', + saveppm(ima_name2,uint8(round(I))); + else + savepgm(ima_name2,uint8(round(I))); + end; +else + if format_image(1) == 'r', + writeras(ima_name2,round(I),gray(256)); + else + imwrite(uint8(round(I)),gray(256),ima_name2,format_image); + end; +end; diff --git a/mr/Ub5/TOOLBOX_calib/writeras.m b/mr/Ub5/TOOLBOX_calib/writeras.m new file mode 100755 index 0000000..c7eb7bc --- /dev/null +++ b/mr/Ub5/TOOLBOX_calib/writeras.m @@ -0,0 +1,105 @@ +function writeras(filename, image, map); +%WRITERAS Write an image file in sun raster format. +% WRITERAS('imagefile.ras', image_matrix, map) writes a +% "sun.raster" image file. + +% Written by Thomas K. Leung 3/30/93. +% @ California Institute of Technology. + + +% PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if nargin < 3, + map = []; +end; + +if(strcmp(suffix, 'ras')) + %Write header + + fp = fopen(filename, 'wb'); + if(fp < 0) error(['Cannot open ' filename '.']), end + + [height, width] = size(image); + image = image - 1; + mapsize = size(map, 1)*size(map,2); + %fwrite(fp, ... + % [1504078485, width, height, 8, height*width, 1, 1, mapsize], ... + % 'long'); + + + zero_str = '00000000'; + + % MAGIC NUMBER: + + + fwrite(fp,89,'uchar'); + fwrite(fp,166,'uchar'); + fwrite(fp,106,'uchar'); + fwrite(fp,149,'uchar'); + + width_str = dec2hex(width); + width_str = [zero_str(1:8-length(width_str)) width_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar'); + end; + + + height_str = dec2hex(height); + height_str = [zero_str(1:8-length(height_str)) height_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,8,'uchar'); + + ll = height*width; + ll_str = dec2hex(ll); + ll_str = [zero_str(1:8-length(ll_str)) ll_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,1,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,~~mapsize,'uchar'); + + mapsize_str = dec2hex(mapsize); + mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str]; + + %keyboard; + + for ii = 1:2:7, + fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar'); + end; + + + if mapsize ~= 0 + map = min(255, fix(255*map)); + fwrite(fp, map, 'uchar'); + end + if rem(width,2) == 1 + image = [image'; zeros(1, height)]'; + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + else + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + end + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end diff --git a/mr/Ub5/calib_images.zip b/mr/Ub5/calib_images.zip new file mode 100644 index 0000000..0f5d870 --- /dev/null +++ b/mr/Ub5/calib_images.zip Binary files differ diff --git a/mr/Ub5/calib_images/Calib_Results.m b/mr/Ub5/calib_images/Calib_Results.m new file mode 100644 index 0000000..4767103 --- /dev/null +++ b/mr/Ub5/calib_images/Calib_Results.m @@ -0,0 +1,141 @@ +% Intrinsic and Extrinsic Camera Parameters +% +% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters. +% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points. +% All those complementary variables are saved in the complete matlab data file Calib_Results.mat. +% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/ + + +%-- Focal length: +fc = [ 854.978537098203219 ; 827.066128052557588 ]; + +%-- Principal point: +cc = [ 577.057178058216778 ; 144.980534130229529 ]; + +%-- Skew coefficient: +alpha_c = 0.000000000000000; + +%-- Distortion coefficients: +kc = [ 0.006945698565263 ; 0.083618463804694 ; -0.021769465128195 ; 0.072457896598761 ; 0.000000000000000 ]; + +%-- Focal length uncertainty: +fc_error = [ 57.040050910917287 ; 44.542135114088978 ]; + +%-- Principal point uncertainty: +cc_error = [ 65.877741250539444 ; 31.910194995190658 ]; + +%-- Skew coefficient uncertainty: +alpha_c_error = 0.000000000000000; + +%-- Distortion coefficients uncertainty: +kc_error = [ 0.092104139086721 ; 0.064532023758369 ; 0.015070247892645 ; 0.026234928011987 ; 0.000000000000000 ]; + +%-- Image size: +nx = 640; +ny = 480; + + +%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox): +%-- Those variables are used to control which intrinsic parameters should be optimized + +n_ima = 15; % Number of calibration images +est_fc = [ 1 ; 1 ]; % Estimation indicator of the two focal variables +est_aspect_ratio = 1; % Estimation indicator of the aspect ratio fc(2)/fc(1) +center_optim = 1; % Estimation indicator of the principal point +est_alpha = 0; % Estimation indicator of the skew coefficient +est_dist = [ 1 ; 1 ; 1 ; 1 ; 0 ]; % Estimation indicator of the distortion coefficients + + +%-- Extrinsic parameters: +%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties + +%-- Image #1: +omc_1 = [ 1.995757e+00 ; 2.053173e+00 ; 4.040009e-01 ]; +Tc_1 = [ -4.575606e+02 ; 1.783393e+02 ; 1.530365e+03 ]; +omc_error_1 = [ 9.134376e-02 ; 7.292460e-02 ; 1.228391e-01 ]; +Tc_error_1 = [ 1.181642e+02 ; 6.108623e+01 ; 9.010338e+01 ]; + +%-- Image #2: +omc_2 = [ 2.170851e+00 ; 2.095896e+00 ; 7.597938e-03 ]; +Tc_2 = [ -8.825159e+02 ; 4.132850e+01 ; 1.136566e+03 ]; +omc_error_2 = [ 6.264919e-02 ; 7.363788e-02 ; 1.256151e-01 ]; +Tc_error_2 = [ 8.959521e+01 ; 5.493221e+01 ; 9.443056e+01 ]; + +%-- Image #3: +omc_3 = [ 1.530043e+00 ; 1.240330e+00 ; -7.655396e-01 ]; +Tc_3 = [ -6.343018e+02 ; 1.672382e+02 ; 1.132173e+03 ]; +omc_error_3 = [ 3.056284e-02 ; 5.159205e-02 ; 8.365717e-02 ]; +Tc_error_3 = [ 8.776683e+01 ; 5.088329e+01 ; 7.076839e+01 ]; + +%-- Image #4: +omc_4 = [ 1.451303e+00 ; 1.321938e+00 ; -1.111874e+00 ]; +Tc_4 = [ -4.653485e+02 ; 1.948217e+02 ; 1.229732e+03 ]; +omc_error_4 = [ 3.844956e-02 ; 5.066002e-02 ; 8.395729e-02 ]; +Tc_error_4 = [ 9.568394e+01 ; 5.139910e+01 ; 6.146523e+01 ]; + +%-- Image #5: +omc_5 = [ -1.740680e+00 ; -1.905404e+00 ; -7.348696e-01 ]; +Tc_5 = [ -7.310709e+02 ; -1.646422e+02 ; 1.026920e+03 ]; +omc_error_5 = [ 4.543777e-02 ; 5.266932e-02 ; 9.174843e-02 ]; +Tc_error_5 = [ 8.035783e+01 ; 4.749430e+01 ; 8.802916e+01 ]; + +%-- Image #6: +omc_6 = [ 2.106712e+00 ; 2.136431e+00 ; 4.109344e-01 ]; +Tc_6 = [ -2.823792e+02 ; -3.466001e+02 ; 2.500420e+03 ]; +omc_error_6 = [ 1.196789e-01 ; 1.307821e-01 ; 2.281843e-01 ]; +Tc_error_6 = [ 1.892276e+02 ; 9.982670e+01 ; 1.734826e+02 ]; + +%-- Image #7: +omc_7 = [ 1.365947e+00 ; 1.451353e+00 ; 9.535067e-01 ]; +Tc_7 = [ -1.415682e+03 ; -3.013315e+02 ; 1.910662e+03 ]; +omc_error_7 = [ 5.867475e-02 ; 7.701908e-02 ; 8.475881e-02 ]; +Tc_error_7 = [ 1.556012e+02 ; 8.986739e+01 ; 1.663202e+02 ]; + +%-- Image #8: +omc_8 = [ 1.484042e+00 ; 9.849878e-01 ; -6.244043e-02 ]; +Tc_8 = [ -1.691445e+03 ; 4.763898e+02 ; 2.178031e+03 ]; +omc_error_8 = [ 4.062499e-02 ; 5.971006e-02 ; 8.542558e-02 ]; +Tc_error_8 = [ 1.795399e+02 ; 1.087514e+02 ; 1.883822e+02 ]; + +%-- Image #9: +omc_9 = [ 2.046247e+00 ; 2.033812e+00 ; -6.767102e-02 ]; +Tc_9 = [ -1.172746e+03 ; -1.960213e+02 ; 1.590760e+03 ]; +omc_error_9 = [ 4.766080e-02 ; 8.183373e-02 ; 1.439476e-01 ]; +Tc_error_9 = [ 1.251337e+02 ; 7.526812e+01 ; 1.306311e+02 ]; + +%-- Image #10: +omc_10 = [ -2.159407e+00 ; -2.072502e+00 ; -1.027183e+00 ]; +Tc_10 = [ -1.075519e+03 ; -2.610008e+02 ; 1.581199e+03 ]; +omc_error_10 = [ 8.136627e-02 ; 5.024202e-02 ; 1.517310e-01 ]; +Tc_error_10 = [ 1.280588e+02 ; 7.465618e+01 ; 1.315453e+02 ]; + +%-- Image #11: +omc_11 = [ -2.020336e+00 ; -1.429650e-02 ; -4.112296e-01 ]; +Tc_11 = [ -8.526627e+02 ; -2.045490e+00 ; 1.431795e+03 ]; +omc_error_11 = [ 4.739778e-02 ; 4.550232e-02 ; 8.341282e-02 ]; +Tc_error_11 = [ 1.119119e+02 ; 6.418486e+01 ; 9.524227e+01 ]; + +%-- Image #12: +omc_12 = [ 1.488865e+00 ; 1.461210e+00 ; 1.028688e+00 ]; +Tc_12 = [ -7.918597e+02 ; -1.870834e+02 ; 1.151936e+03 ]; +omc_error_12 = [ 5.934013e-02 ; 6.454959e-02 ; 7.664009e-02 ]; +Tc_error_12 = [ 9.135684e+01 ; 5.336108e+01 ; 9.703671e+01 ]; + +%-- Image #13: +omc_13 = [ -2.025432e+00 ; -1.889665e+00 ; 5.006002e-01 ]; +Tc_13 = [ -2.934532e+02 ; -1.289886e+02 ; 1.330181e+03 ]; +omc_error_13 = [ 5.024733e-02 ; 4.452978e-02 ; 9.125183e-02 ]; +Tc_error_13 = [ 9.929323e+01 ; 5.277202e+01 ; 7.672216e+01 ]; + +%-- Image #14: +omc_14 = [ -2.078049e+00 ; -2.032300e+00 ; 7.816034e-01 ]; +Tc_14 = [ -2.712394e+02 ; 5.364337e+01 ; 1.385082e+03 ]; +omc_error_14 = [ 5.302676e-02 ; 4.707744e-02 ; 1.011974e-01 ]; +Tc_error_14 = [ 1.038970e+02 ; 5.547939e+01 ; 7.767706e+01 ]; + +%-- Image #15: +omc_15 = [ -2.544995e+00 ; -1.176678e+00 ; -6.633479e-01 ]; +Tc_15 = [ -1.221342e+03 ; -1.068034e+02 ; 1.633177e+03 ]; +omc_error_15 = [ 8.672414e-02 ; 2.699572e-02 ; 1.407373e-01 ]; +Tc_error_15 = [ 1.290093e+02 ; 7.699008e+01 ; 1.406396e+02 ]; + diff --git a/mr/Ub5/calib_images/Calib_Results.mat b/mr/Ub5/calib_images/Calib_Results.mat new file mode 100644 index 0000000..556d52f --- /dev/null +++ b/mr/Ub5/calib_images/Calib_Results.mat Binary files differ diff --git a/mr/Ub5/calib_images/calib_data.mat b/mr/Ub5/calib_images/calib_data.mat new file mode 100644 index 0000000..cbc0a8e --- /dev/null +++ b/mr/Ub5/calib_images/calib_data.mat Binary files differ diff --git a/mr/Ub5/calib_images/img0.jpg b/mr/Ub5/calib_images/img0.jpg new file mode 100644 index 0000000..002bda4 --- /dev/null +++ b/mr/Ub5/calib_images/img0.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img1.jpg b/mr/Ub5/calib_images/img1.jpg new file mode 100644 index 0000000..dba9e17 --- /dev/null +++ b/mr/Ub5/calib_images/img1.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img10.jpg b/mr/Ub5/calib_images/img10.jpg new file mode 100644 index 0000000..6e0e8ab --- /dev/null +++ b/mr/Ub5/calib_images/img10.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img11.jpg b/mr/Ub5/calib_images/img11.jpg new file mode 100644 index 0000000..0605f68 --- /dev/null +++ b/mr/Ub5/calib_images/img11.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img12.jpg b/mr/Ub5/calib_images/img12.jpg new file mode 100644 index 0000000..7dff501 --- /dev/null +++ b/mr/Ub5/calib_images/img12.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img13.jpg b/mr/Ub5/calib_images/img13.jpg new file mode 100644 index 0000000..1c8060d --- /dev/null +++ b/mr/Ub5/calib_images/img13.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img14.jpg b/mr/Ub5/calib_images/img14.jpg new file mode 100644 index 0000000..6f67d8c --- /dev/null +++ b/mr/Ub5/calib_images/img14.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img2.jpg b/mr/Ub5/calib_images/img2.jpg new file mode 100644 index 0000000..6ed4ae0 --- /dev/null +++ b/mr/Ub5/calib_images/img2.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img3.jpg b/mr/Ub5/calib_images/img3.jpg new file mode 100644 index 0000000..093eedc --- /dev/null +++ b/mr/Ub5/calib_images/img3.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img4.jpg b/mr/Ub5/calib_images/img4.jpg new file mode 100644 index 0000000..ba7c17f --- /dev/null +++ b/mr/Ub5/calib_images/img4.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img5.jpg b/mr/Ub5/calib_images/img5.jpg new file mode 100644 index 0000000..d1455d8 --- /dev/null +++ b/mr/Ub5/calib_images/img5.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img6.jpg b/mr/Ub5/calib_images/img6.jpg new file mode 100644 index 0000000..bd55c7b --- /dev/null +++ b/mr/Ub5/calib_images/img6.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img7.jpg b/mr/Ub5/calib_images/img7.jpg new file mode 100644 index 0000000..ebb3d39 --- /dev/null +++ b/mr/Ub5/calib_images/img7.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img8.jpg b/mr/Ub5/calib_images/img8.jpg new file mode 100644 index 0000000..b541f80 --- /dev/null +++ b/mr/Ub5/calib_images/img8.jpg Binary files differ diff --git a/mr/Ub5/calib_images/img9.jpg b/mr/Ub5/calib_images/img9.jpg new file mode 100644 index 0000000..7770227 --- /dev/null +++ b/mr/Ub5/calib_images/img9.jpg Binary files differ diff --git a/mr/Ub5/calib_images/results.m b/mr/Ub5/calib_images/results.m new file mode 100644 index 0000000..b4e5878 --- /dev/null +++ b/mr/Ub5/calib_images/results.m @@ -0,0 +1,29 @@ + +Focal Length: fc = [ 854.97854 827.06613 ] ? [ 57.04005 44.54214 ] +Principal point: cc = [ 577.05718 144.98053 ] ? [ 65.87774 31.91019 ] +Skew: alpha_c = [ 0.00000 ] ? [ 0.00000 ] => angle of pixel axes = 90.00000 ? 0.00000 degrees +Distortion: schwer kc = [ 0.00695 0.08362 -0.02177 0.07246 0.00000 ] ? [ 0.09210 0.06453 0.01507 0.02623 0.00000 ] +Pixel error: err = [ 2.96180 2.16437 ] + +? = +- + +initialisation without uncertancies +Focal Length: fc = [ 333.06491 333.06491 ] +Principal point: cc = [ 319.50000 239.50000 ] +Skew: alpha_c = [ 0.00000 ] => angle of pixel = 90.00000 degrees +Distortion: kc = [ 0.00000 0.00000 0.00000 0.00000 0.00000 ] + +The pixel errors are mean re-projection error for extracted grid points, i.e. the actual pixel location and the one by using calculated K matrix. +Initialization of the principal point at the center of the image. + +removing picture 11,2,4,3 + +Calibration results after optimization (with uncertainties): + +Focal Length: fc = [ 802.16252 784.80772 ] ? [ 35.88106 32.30823 ] +Principal point: cc = [ 409.60542 137.13751 ] ? [ 36.82844 42.37479 ] +Skew: alpha_c = [ 0.00000 ] ? [ 0.00000 ] => angle of pixel axes = 90.00000 ? 0.00000 degrees +Distortion: kc = [ -0.14710 0.14740 -0.01636 0.00052 0.00000 ] ? [ 0.06378 0.16835 0.01201 0.01046 0.00000 ] +Pixel error: err = [ 1.33896 1.25581 ] + +Note: The numerical errors are approximately three times the standard deviations (for reference). diff --git a/mr/Ub5/calib_images/test.jpg b/mr/Ub5/calib_images/test.jpg new file mode 100644 index 0000000..d1455d8 --- /dev/null +++ b/mr/Ub5/calib_images/test.jpg Binary files differ diff --git a/mr/Ub5/toolbox_calib.zip b/mr/Ub5/toolbox_calib.zip new file mode 100644 index 0000000..53e6eb8 --- /dev/null +++ b/mr/Ub5/toolbox_calib.zip Binary files differ