├── startup.m ├── pattern.eps ├── calib_stereo.m ├── go_calib_stereo.m ├── show_calib_results.m ├── clear_windows.m ├── go_calib_optim_iter_weak.m ├── load_stereo_calib_files.m ├── show_stereo_calib_results.m ├── go_calib_optim_iter_fisheye.m ├── show_calib_results_fisheye.m ├── calibration_pattern └── pattern.eps ├── mean_std_robust.m ├── loading_calib.m ├── is3D.m ├── loading_stereo_calib.m ├── calib.m ├── rotation.m ├── eliminate_boundary.m ├── fixallvariables.m ├── clearwin.m ├── skew3.m ├── Rectangle2Square.m ├── fov.m ├── savepgm.m ├── README.txt ├── projectedGrid.m ├── fixvariable.m ├── pgmread.m ├── align_structures.m ├── dAB.m ├── README.md ├── check_convergence.m ├── extract_parameters3D.m ├── write_image.m ├── saveppm.m ├── error_depth.m ├── affine.m ├── point_distribution.m ├── check_extracted_images.m ├── load_image.m ├── check_active_images.m ├── UnWarpPlane.m ├── inverse_motion.m ├── saveinr.m ├── script_fit_distortion.m ├── combine_calib.m ├── click_stereo.m ├── extract_distortion_data.m ├── normalize_pixel_fisheye.m ├── loadinr.m ├── project2_oulu.m ├── saving_stereo_calib.m ├── run_error_analysis.m ├── comp_error_calib.m ├── normalize.m ├── comp_distortion.m ├── comp_error_calib_fisheye.m ├── error_depth_list.m ├── normalize_pixel.m ├── comp_fisheye_distortion.m ├── comp_distortion_oulu.m ├── compose_motion.m ├── compute_collineation.m ├── calib_gui.m ├── apply_fisheye_distortion.m ├── calib_gui_no_read.m ├── calib_gui_normal.m ├── count_squares.m ├── calib_gui_fisheye.m ├── anisdiff.m ├── script_fit_distortion_fisheye.m ├── projector_ima_corners.m ├── extract_parameters.m ├── cam_proj_extract_param.m ├── extract_parameters_fisheye.m ├── compute_epipole.m ├── comp_distortion2.m ├── projector_calib.m ├── count_squares_distorted.m ├── count_squares_fisheye_distorted.m ├── mosaic.m ├── downsample.m ├── small_test_script.m ├── rigid_motion.m ├── apply_distortion.m ├── merge_two_datasets.m ├── recomp_corner_calib_saddle_points.m ├── willson_convert.m ├── loadpgm.m ├── normalize2.m ├── show_window.m ├── willson_read.m ├── comp_ext_calib.m ├── comp_ext_calib_fisheye.m ├── smooth_images.m ├── Compute3D.m ├── apply_distortion2.m ├── error_cam_proj.m ├── error_cam_proj3.m ├── merge_calibration_sets.m ├── stereo_triangulation.m ├── error_cam_proj2.m ├── go_calib_optim.m ├── go_calib_optim_no_read.m ├── go_calib_optim_fisheye_no_read.m ├── init_intrinsic_param_fisheye.m ├── scanning_script.m ├── writeras.m ├── loadppm.m ├── rect.m ├── rect_index.m ├── data_calib_no_read.m ├── mosaic_no_read.m ├── readras.m ├── data_calib.m ├── cam_proj_calib_optim.m ├── compute_extrinsic_refine.m ├── go_calib_optim_fisheye_no_read.asv ├── compute_extrinsic_refine_fisheye.m ├── ima_read_calib.m ├── compute_extrinsic_refine2.m ├── export_calib_data.m ├── compute_extrinsic.m ├── ima_read_calib_no_read.m ├── recomp_corner_calib.m ├── saving_calib_no_results.m ├── TestFunction.m ├── saving_calib_ascii_fisheye.m └── saving_calib_ascii.m /startup.m: -------------------------------------------------------------------------------- 1 | % Main camera calibration toolbox: 2 | format compact 3 | -------------------------------------------------------------------------------- /pattern.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/pattern.eps -------------------------------------------------------------------------------- /calib_stereo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/calib_stereo.m -------------------------------------------------------------------------------- /go_calib_stereo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/go_calib_stereo.m -------------------------------------------------------------------------------- /show_calib_results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/show_calib_results.m -------------------------------------------------------------------------------- /clear_windows.m: -------------------------------------------------------------------------------- 1 | for kk = 1:n_ima, 2 | eval(['clear wintx_' num2str(kk)]); 3 | eval(['clear winty_' num2str(kk)]); 4 | end; 5 | -------------------------------------------------------------------------------- /go_calib_optim_iter_weak.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/go_calib_optim_iter_weak.m -------------------------------------------------------------------------------- /load_stereo_calib_files.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/load_stereo_calib_files.m -------------------------------------------------------------------------------- /show_stereo_calib_results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/show_stereo_calib_results.m -------------------------------------------------------------------------------- /go_calib_optim_iter_fisheye.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/go_calib_optim_iter_fisheye.m -------------------------------------------------------------------------------- /show_calib_results_fisheye.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/show_calib_results_fisheye.m -------------------------------------------------------------------------------- /calibration_pattern/pattern.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nghiaho12/camera_calibration_toolbox_octave/HEAD/calibration_pattern/pattern.eps -------------------------------------------------------------------------------- /mean_std_robust.m: -------------------------------------------------------------------------------- 1 | function [m,s] = mean_std_robust(x); 2 | 3 | x = x(:); 4 | 5 | m = median(x); 6 | 7 | s = median(abs(x - m))*1.4836; 8 | -------------------------------------------------------------------------------- /loading_calib.m: -------------------------------------------------------------------------------- 1 | if ~exist('Calib_Results.mat'), 2 | fprintf(1,'\nCalibration file Calib_Results.mat not found!\n'); 3 | return; 4 | end; 5 | 6 | fprintf(1,'\nLoading calibration results from Calib_Results.mat\n'); 7 | 8 | load Calib_Results 9 | 10 | fprintf(1,'done\n'); 11 | -------------------------------------------------------------------------------- /is3D.m: -------------------------------------------------------------------------------- 1 | function test = is3D(X), 2 | 3 | 4 | Np = size(X,2); 5 | 6 | %% Check for planarity of the structure: 7 | 8 | X_mean = mean(X')'; 9 | 10 | Y = X - (X_mean*ones(1,Np)); 11 | 12 | YY = Y*Y'; 13 | 14 | [U,S,V] = svd(YY); 15 | 16 | r = S(3,3)/S(2,2); 17 | 18 | test = (r > 1e-3); 19 | 20 | -------------------------------------------------------------------------------- /loading_stereo_calib.m: -------------------------------------------------------------------------------- 1 | if exist('Calib_Results_stereo.mat')~=2, 2 | fprintf(1,'\nStereo calibration file Calib_Results_stereo.mat not found!\n'); 3 | return; 4 | end; 5 | 6 | fprintf(1,'Loading stereo calibration results from Calib_Results_stereo.mat...\n'); 7 | load('Calib_Results_stereo.mat'); 8 | -------------------------------------------------------------------------------- /calib.m: -------------------------------------------------------------------------------- 1 | function calib(mode), 2 | 3 | % calib(mode) 4 | % 5 | % Runs the Camera Calibration Toolbox. 6 | % Set mode to 1 to run the memory efficient version. 7 | % Any other value for mode will run the normal version (see documentation) 8 | 9 | if nargin < 1, 10 | calib_gui; 11 | else 12 | calib_gui(mode); 13 | end; -------------------------------------------------------------------------------- /rotation.m: -------------------------------------------------------------------------------- 1 | function [] = rotation(st); 2 | 3 | if nargin < 1, 4 | st= 1; 5 | end; 6 | 7 | 8 | fig = gcf; 9 | 10 | ax = gca; 11 | 12 | vv = get(ax,'view'); 13 | 14 | az = vv(1); 15 | el = vv(2); 16 | 17 | for azi = az:-abs(st):az-360, 18 | 19 | set(ax,'view',[azi el]); 20 | figure(fig); 21 | drawnow; 22 | 23 | end; 24 | -------------------------------------------------------------------------------- /eliminate_boundary.m: -------------------------------------------------------------------------------- 1 | function Im2 = eliminate_boundary(Im); 2 | 3 | 4 | [nny,nnx] = size(Im); 5 | 6 | Im2 = zeros(nny,nnx); 7 | 8 | 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) & ... 9 | 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)); 10 | -------------------------------------------------------------------------------- /fixallvariables.m: -------------------------------------------------------------------------------- 1 | % Code that clears all empty or NaN variables 2 | 3 | varlist = whos; 4 | 5 | if ~isempty(varlist), 6 | 7 | Nvar = size(varlist,1); 8 | 9 | for c_var = 1:Nvar, 10 | 11 | var2fix = varlist(c_var).name; 12 | 13 | fixvariable; 14 | 15 | end; 16 | 17 | end; 18 | 19 | clear varlist var2fix Nvar c_var -------------------------------------------------------------------------------- /clearwin.m: -------------------------------------------------------------------------------- 1 | % Function that clears all the wintx_i and winty_i 2 | % In normal operation of the toolbox, this function should not be 3 | % useful. 4 | % 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. 5 | 6 | if exist('n_ima','var')~=1 7 | return; 8 | end; 9 | 10 | for kk = 1:n_ima, 11 | 12 | eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]); 13 | 14 | end; 15 | -------------------------------------------------------------------------------- /skew3.m: -------------------------------------------------------------------------------- 1 | function [S,dSdT] = skew3(T); 2 | 3 | S = [ 0 -T(3) T(2) 4 | T(3) 0 -T(1) 5 | -T(2) T(1) 0 ]; 6 | 7 | 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]; 8 | 9 | return; 10 | 11 | 12 | % Test of Jacobian: 13 | 14 | T1 = randn(3,1); 15 | 16 | dT = 0.001*randn(3,1); 17 | 18 | T2 = T1 + dT; 19 | 20 | [S1,dSdT] = skew3(T1); 21 | [S2] = skew3(T2); 22 | 23 | S2app = S1; 24 | S2app(:) = S2app(:) + dSdT*dT; 25 | 26 | 27 | norm(S1 - S2) / norm(S2app - S2) 28 | -------------------------------------------------------------------------------- /Rectangle2Square.m: -------------------------------------------------------------------------------- 1 | function [square] = Rectangle2Square(rectangle,L,W); 2 | 3 | % Generate the square from a rectangle of known segment lengths 4 | % from pt1 to pt2 : L 5 | % from pt2 to pt3 : W 6 | 7 | [u_hori,u_vert] = UnWarpPlane(rectangle); 8 | 9 | coeff_x = sqrt(W/L); 10 | coeff_y = 1/coeff_x; 11 | 12 | x_coord = [ 0 coeff_x coeff_x 0]; 13 | y_coord = [ 0 0 coeff_y coeff_y]; 14 | 15 | 16 | square = rectangle(:,1) * ones(1,4) + u_hori*x_coord + u_vert*y_coord; 17 | square = square ./ (ones(3,1)*square(3,:)); 18 | 19 | 20 | -------------------------------------------------------------------------------- /fov.m: -------------------------------------------------------------------------------- 1 | % small program that computes the field of view of a camera (in degrees) 2 | 3 | if ~exist('fc')|~exist('cc')|~exist('nx')|~exist('ny'), 4 | error('Need calibration results to compute FOV (fc,cc,Wcal,Hcal)'); 5 | end; 6 | 7 | FOV_HOR = 180 * ( atan((nx - (cc(1)+.5))/fc(1)) + atan((cc(1)+.5)/fc(1)) )/pi; 8 | 9 | FOV_VER = 180 * ( atan((ny - (cc(2)+.5))/fc(2)) + atan((cc(2)+.5)/fc(2)) )/pi; 10 | 11 | fprintf(1,'Horizontal field of view = %.2f degrees\n',FOV_HOR); 12 | fprintf(1,'Vertical field of view = %.2f degrees\n',FOV_VER); -------------------------------------------------------------------------------- /savepgm.m: -------------------------------------------------------------------------------- 1 | %SAVEPGM Write a PGM format file 2 | % 3 | % SAVEPGM(filename, im) 4 | % 5 | % Saves the specified image array in a binary (P5) format PGM image file. 6 | % 7 | % SEE ALSO: loadpgm 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | 12 | % Peter Corke 1994 13 | 14 | function savepgm(fname, im) 15 | 16 | fid = fopen(fname, 'w'); 17 | [r,c] = size(im'); 18 | fprintf(fid, 'P5\n'); 19 | fprintf(fid, '%d %d\n', r, c); 20 | fprintf(fid, '255\n'); 21 | fwrite(fid, im', 'uchar'); 22 | fclose(fid); 23 | -------------------------------------------------------------------------------- /README.txt: -------------------------------------------------------------------------------- 1 | Camera Calibration Toolbox for Matlab 2 | ------------------------------------- 3 | 4 | The complete documentation of this toolbox is available online at 5 | http://www.vision.caltech.edu/bouguetj/calib_doc/ 6 | 7 | The lastest version of the toolbox can also be downloaded from this web site. 8 | 9 | If you have any question/suggestion/bug report please send me email at 10 | jean-yves.bouguet@intel.com 11 | 12 | 13 | Enjoy the toolbox! 14 | 15 | 16 | Jean-Yves Bouguet 17 | Intel Corporation 18 | 19 | 20 | October 15th, 2004 -------------------------------------------------------------------------------- /projectedGrid.m: -------------------------------------------------------------------------------- 1 | function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny); 2 | 3 | % new formalism using homographies 4 | 5 | a00 = [P1;1]; 6 | a10 = [P2;1]; 7 | a11 = [P3;1]; 8 | a01 = [P4;1]; 9 | 10 | % Compute the planart collineation: 11 | 12 | [H] = compute_collineation (a00, a10, a11, a01); 13 | 14 | 15 | % Build the grid using the planar collineation: 16 | 17 | x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1); 18 | y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1); 19 | 20 | pts = [x_l(:) y_l(:) ones(nx*ny,1)]'; 21 | 22 | XX = H*pts; 23 | 24 | XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); 25 | -------------------------------------------------------------------------------- /fixvariable.m: -------------------------------------------------------------------------------- 1 | % Code that clears an empty variable, or a NaN vsriable. 2 | % Does not clear structures, or cells. 3 | 4 | if exist('var2fix')==1, 5 | if eval(['exist(''' var2fix ''') == 1']), 6 | if eval(['isempty(' var2fix ')']), 7 | eval(['clear ' var2fix ]); 8 | else 9 | if eval(['~isstruct(' var2fix ')']), 10 | if eval(['~iscell(' var2fix ')']), 11 | if eval(['isnan(' var2fix '(1))']), 12 | eval(['clear ' var2fix ]); 13 | end; 14 | end; 15 | end; 16 | end; 17 | end; 18 | end; 19 | -------------------------------------------------------------------------------- /pgmread.m: -------------------------------------------------------------------------------- 1 | function img = pgmread(filename) 2 | % function img = pgmread(filename) 3 | % this is my version of pgmread for the pgm file created by XV. 4 | % 5 | % this program also corrects for the shifts in the image from pm file. 6 | 7 | fid = fopen(filename,'r'); 8 | fscanf(fid, 'P5\n'); 9 | cmt = '#'; 10 | while findstr(cmt, '#'), 11 | cmt = fgets(fid); 12 | if length(findstr(cmt, '#')) ~= 1, 13 | YX = sscanf(cmt, '%d %d'); 14 | y = YX(1); x = YX(2); 15 | end 16 | end 17 | 18 | %fgets(fid); 19 | 20 | %img = fscanf(fid,'%d',size); 21 | %img = img'; 22 | 23 | img = fread(fid,[y,x],'uint8'); 24 | img = img'; 25 | fclose(fid); 26 | 27 | -------------------------------------------------------------------------------- /align_structures.m: -------------------------------------------------------------------------------- 1 | function [om,T,Y] = align_structures(X1,X2), 2 | 3 | % Find om (R) and T, such that Y = R*X1 + T is as close as 4 | % possible to X2. 5 | 6 | [m,n] = size(X1); 7 | 8 | % initialization: 9 | 10 | 11 | 12 | 13 | % initialize param to no motion: 14 | param = zeros(6,1); 15 | change = 1; 16 | 17 | while change > 1e-6, 18 | 19 | om = param(1:3); 20 | T = param(4:6); 21 | 22 | [Y,dYdom,dYdT] = rigid_motion(X1,om,T); 23 | 24 | J = [dYdom dYdT]; 25 | 26 | err = X2(:) - Y(:); 27 | 28 | param_up = inv(J'*J)*J'*err; 29 | 30 | param = param + param_up; 31 | 32 | change = norm(param_up)/norm(param); 33 | 34 | end; 35 | 36 | om = param(1:3); 37 | 38 | T = param(4:6); 39 | 40 | [Y,dYdom,dYdT] = rigid_motion(X1,om,T); 41 | -------------------------------------------------------------------------------- /dAB.m: -------------------------------------------------------------------------------- 1 | function [dABdA,dABdB] = dAB(A,B); 2 | 3 | % [dABdA,dABdB] = dAB(A,B); 4 | % 5 | % returns : dABdA and dABdB 6 | 7 | [p,n] = size(A); [n2,q] = size(B); 8 | 9 | if n2 ~= n, 10 | error(' A and B must have equal inner dimensions'); 11 | end; 12 | 13 | if issparse(A) | issparse(B) | p*q*p*n>625 , 14 | dABdA=spalloc(p*q,p*n,p*q*n); 15 | else 16 | dABdA=zeros(p*q,p*n); 17 | end; 18 | 19 | 20 | for i=1:q, 21 | for j=1:p, 22 | ij = j + (i-1)*p; 23 | for k=1:n, 24 | kj = j + (k-1)*p; 25 | dABdA(ij,kj) = B(k,i); 26 | end; 27 | end; 28 | end; 29 | 30 | 31 | if issparse(A) | issparse(B) | p*q*n*q>625 , 32 | dABdB=spalloc(p*q,n*q,p*q*n); 33 | else 34 | dABdB=zeros(p*q,q*n); 35 | end; 36 | 37 | for i=1:q 38 | dABdB([i*p-p+1:i*p]',[i*n-n+1:i*n]) = A; 39 | end; 40 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # camera_calibration_toolbox_octave 2 | An Octave port of Camera Calibration Toolbox for Matlab (1st June 2017) (http://www.vision.caltech.edu/bouguetj/calib_doc/) 3 | 4 | There is surprisingly very little code modifications required to make the original Matlab code to work in Octave (tested with 4.4.0). I only had to comment out usage of BackingStore 5 | 6 | ``` 7 | set(fig_number,'Units','points', ... 8 | %'BackingStore','off', ... 9 | ``` 10 | 11 | I've tested with single camera and stereo calibration. The GUI elements in Octave are usable, though rendering isn't always 100% perfect. I found Octave GUI can be a bit quirky at times, might be related to my choice of using i3 tiling windows manager. 12 | 13 | ![alt text](https://user-images.githubusercontent.com/1471705/42861822-abf6b66c-8a11-11e8-94c0-faaff3412701.jpg) 14 | -------------------------------------------------------------------------------- /check_convergence.m: -------------------------------------------------------------------------------- 1 | %%% Replay the set of solution vectors: 2 | 3 | 4 | if ~exist('param_list'), 5 | if ~exist('solution'); 6 | fprintf(1,'Error: Need to calibrate first\n'); 7 | return; 8 | else 9 | param_list = solution; 10 | end; 11 | end; 12 | 13 | N_iter = size(param_list,2); 14 | 15 | if N_iter == 1, 16 | fprintf(1,'Warning: There is a unique state in the list of parameters.\n'); 17 | end; 18 | 19 | 20 | 21 | %M = moviein(N_iter); 22 | 23 | for nn = 1:N_iter, 24 | 25 | solution = param_list(:,nn); 26 | 27 | extract_parameters; 28 | comp_error_calib; 29 | 30 | ext_calib; 31 | 32 | drawnow; 33 | 34 | % Mnn = getframe(gcf); 35 | 36 | % M(:,nn) = Mnn; 37 | 38 | end; 39 | 40 | %fig = gcf; 41 | 42 | 43 | %figure(fig+1); 44 | %close; 45 | %figure(fig+1); 46 | 47 | %clf; 48 | %movie(M,20); 49 | -------------------------------------------------------------------------------- /extract_parameters3D.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | 5 | fc = solution(1:2); 6 | kc = solution(3:6); 7 | cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3); 8 | 9 | % Calibration matrix: 10 | 11 | KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1]; 12 | inv_KK = inv(KK); 13 | 14 | % Extract the extrinsic paramters, and recomputer the collineations 15 | 16 | for kk = 1:n_ima, 17 | 18 | omckk = solution(4+6*(kk-1) + 3:6*kk + 3); 19 | 20 | Tckk = solution(6*kk+1 + 3:6*kk+3 + 3); 21 | 22 | Rckk = rodrigues(omckk); 23 | 24 | Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 25 | 26 | Hlkk = Hlkk / Hlkk(3,3); 27 | 28 | eval(['omc_' num2str(kk) ' = omckk;']); 29 | eval(['Rc_' num2str(kk) ' = Rckk;']); 30 | eval(['Tc_' num2str(kk) ' = Tckk;']); 31 | 32 | eval(['Hl_' num2str(kk) '=Hlkk;']); 33 | 34 | end; 35 | 36 | 37 | -------------------------------------------------------------------------------- /write_image.m: -------------------------------------------------------------------------------- 1 | function [] = write_image(I, kk , calib_name , format_image , type_numbering , image_numbers , N_slots), 2 | 3 | if format_image(1) == 'j', 4 | format_image = 'bmp'; 5 | end; 6 | 7 | 8 | if ~type_numbering, 9 | number_ext = num2str(image_numbers(kk)); 10 | else 11 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 12 | end; 13 | 14 | ima_name2 = [calib_name number_ext '.' format_image]; 15 | 16 | fprintf(1,['Saving image under ' ima_name2 '...\n']); 17 | 18 | if format_image(1) == 'p', 19 | if format_image(2) == 'p', 20 | saveppm(ima_name2,uint8(round(I))); 21 | else 22 | savepgm(ima_name2,uint8(round(I))); 23 | end; 24 | else 25 | if format_image(1) == 'r', 26 | writeras(ima_name2,round(I),gray(256)); 27 | else 28 | imwrite(uint8(round(I)),gray(256),ima_name2,format_image); 29 | end; 30 | end; 31 | -------------------------------------------------------------------------------- /saveppm.m: -------------------------------------------------------------------------------- 1 | %SAVEPPM Write a PPM format file 2 | % 3 | % SAVEPPM(filename, I) 4 | % 5 | % Saves the specified red, green and blue planes in a binary (P6) 6 | % format PPM image file. 7 | % 8 | % SEE ALSO: loadppm 9 | % 10 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 11 | 12 | 13 | % Peter Corke 1994 14 | 15 | function saveppm(fname, I) 16 | 17 | I = double(I); 18 | 19 | if size(I,3) == 1, 20 | R = I; 21 | G = I; 22 | B = I; 23 | else 24 | R = I(:,:,1); 25 | G = I(:,:,2); 26 | B = I(:,:,3); 27 | end; 28 | 29 | %keyboard; 30 | 31 | fid = fopen(fname, 'w'); 32 | [r,c] = size(R'); 33 | fprintf(fid, 'P6\n'); 34 | fprintf(fid, '%d %d\n', r, c); 35 | fprintf(fid, '255\n'); 36 | R = R'; 37 | G = G'; 38 | B = B'; 39 | im = [R(:) G(:) B(:)]; 40 | %im = reshape(im,r,c*3); 41 | im = im'; 42 | %im = im(:); 43 | fwrite(fid, im, 'uchar'); 44 | fclose(fid); 45 | 46 | -------------------------------------------------------------------------------- /error_depth.m: -------------------------------------------------------------------------------- 1 | function err_shape = error_depth(param_dist,xcn,xpn,R,T,X_shape,ind); 2 | 3 | 4 | 5 | X_new = depth_compute(xcn,xpn,[param_dist],R,T); 6 | 7 | 8 | N_pt_calib = size(xcn,2); 9 | 10 | % UnNormalized shape extraction: 11 | 12 | X_shape2 = X_new; 13 | X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); 14 | 15 | % map the second vector at [1;0;0]: 16 | 17 | omu = -cross([1;0;0],X_shape2(:,2)); 18 | omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); 19 | Ru = rodrigues(omu); 20 | 21 | X_shape2 = Ru* X_shape2; 22 | 23 | omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); 24 | omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); 25 | Ru2 = rodrigues(omu2); 26 | 27 | X_shape2 = Ru2* X_shape2; 28 | 29 | 30 | % Error: 31 | 32 | err_shape = X_shape2(:,2:end) - X_shape(:,2:end); 33 | 34 | err_shape = err_shape(:); 35 | 36 | 37 | 38 | %err_depth = Z_new - Z_ref; 39 | -------------------------------------------------------------------------------- /affine.m: -------------------------------------------------------------------------------- 1 | function A = affine(X,n,om,T,fc,cc,alpha_c) 2 | 3 | if nargin < 7, 4 | alpha_c = 0; 5 | if nargin < 6, 6 | cc = [0;0]; 7 | if nargin < 5, 8 | fc = [1;1]; 9 | if nargin < 4, 10 | T = zeros(3,1); 11 | if nargin < 3, 12 | om = zeros(3,1); 13 | if nargin < 2, 14 | n = [0;0;-1]; 15 | if nargin < 1, 16 | error('Error: affine needs some arguments: A = affine(X,n,om,T,fc,cc,alpha_c);'); 17 | end; 18 | end; 19 | end; 20 | end; 21 | end; 22 | end; 23 | end; 24 | 25 | 26 | KK = [fc(1) alpha_c*fc(1) cc(1); 0 fc(2) cc(2);0 0 1]; 27 | R = rodrigues(om); 28 | omega = n / dot(n,X); 29 | x = X/X(3); 30 | 31 | H = KK * [R + T*omega'] * inv(KK); 32 | 33 | 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; 34 | -------------------------------------------------------------------------------- /point_distribution.m: -------------------------------------------------------------------------------- 1 | % Point Distribution 2 | 3 | colors = 'brgkcm'; 4 | 5 | if ~exist('n_ima')|~exist('fc'), 6 | fprintf(1,'No calibration data available.\n'); 7 | return; 8 | end; 9 | 10 | check_active_images; 11 | 12 | if ~exist(['ex_' num2str(ind_active(1)) ]), 13 | fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); 14 | return; 15 | end; 16 | 17 | figure(6); 18 | 19 | for kk=1:n_ima 20 | 21 | if exist(['x_' num2str(kk)]), 22 | 23 | if active_images(kk) & eval(['~isnan(x_' num2str(kk) '(1,1))']), 24 | 25 | eval(['plot(x_' num2str(kk) '(1,:),x_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); 26 | 27 | hold on; 28 | end; 29 | 30 | end; 31 | 32 | end; 33 | 34 | axis('equal'); 35 | 36 | axis([0 nx 0 ny]); 37 | 38 | title1=pwd; 39 | title1=strrep(title1,'_','\_'); 40 | 41 | title({'Point Distribution in Images',title1}); 42 | 43 | xlabel('x'); 44 | 45 | ylabel('y'); 46 | -------------------------------------------------------------------------------- /check_extracted_images.m: -------------------------------------------------------------------------------- 1 | check_active_images; 2 | 3 | for kk = ind_active, 4 | 5 | if ~exist(['x_' num2str(kk)]), 6 | 7 | fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk); 8 | 9 | active_images(kk) = 0; 10 | 11 | eval(['dX_' num2str(kk) ' = NaN;']); 12 | eval(['dY_' num2str(kk) ' = NaN;']); 13 | 14 | eval(['wintx_' num2str(kk) ' = NaN;']); 15 | eval(['winty_' num2str(kk) ' = NaN;']); 16 | 17 | eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); 18 | eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); 19 | 20 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 21 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 22 | 23 | else 24 | 25 | eval(['xkk = x_' num2str(kk) ';']); 26 | 27 | if isnan(xkk(1)), 28 | 29 | fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk); 30 | 31 | active_images(kk) = 0; 32 | 33 | end; 34 | 35 | end; 36 | 37 | end; 38 | -------------------------------------------------------------------------------- /load_image.m: -------------------------------------------------------------------------------- 1 | function I = load_image(kk , calib_name , format_image , type_numbering , image_numbers , N_slots), 2 | 3 | 4 | if ~type_numbering, 5 | number_ext = num2str(image_numbers(kk)); 6 | else 7 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 8 | end; 9 | 10 | 11 | ima_name = [calib_name number_ext '.' format_image]; 12 | 13 | if ~exist(ima_name), 14 | 15 | fprintf(1,'Image %s not found!!!\n',ima_name); 16 | I = NaN; 17 | 18 | else 19 | 20 | fprintf(1,'Loading image %s...\n',ima_name); 21 | 22 | if format_image(1) == 'p', 23 | if format_image(2) == 'p', 24 | I = double(loadppm(ima_name)); 25 | else 26 | I = double(loadpgm(ima_name)); 27 | end; 28 | else 29 | if format_image(1) == 'r', 30 | I = readras(ima_name); 31 | else 32 | I = double(imread(ima_name)); 33 | end; 34 | end; 35 | 36 | 37 | if size(I,3)>1, 38 | I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); 39 | end; 40 | 41 | end; 42 | -------------------------------------------------------------------------------- /check_active_images.m: -------------------------------------------------------------------------------- 1 | if n_ima ~= 0, 2 | 3 | if ~exist('active_images'), 4 | active_images = ones(1,n_ima); 5 | end; 6 | n_act = length(active_images); 7 | if n_act < n_ima, 8 | active_images = [active_images ones(1,n_ima-n_act)]; 9 | else 10 | if n_act > n_ima, 11 | active_images = active_images(1:n_ima); 12 | end; 13 | end; 14 | 15 | ind_active = find(active_images); 16 | 17 | if prod(double(active_images == 0)), 18 | disp('Error: There is no active image. Run Add/Suppress images to add images'); 19 | return; 20 | end; 21 | 22 | if exist('center_optim'), 23 | center_optim = double(center_optim); 24 | end; 25 | if exist('est_alpha'), 26 | est_alpha = double(est_alpha); 27 | end; 28 | if exist('est_dist'), 29 | est_dist = double(est_dist); 30 | end; 31 | if exist('est_fc'), 32 | est_fc = double(est_fc); 33 | end; 34 | if exist('est_aspect_ratio'), 35 | est_aspect_ratio = double(est_aspect_ratio); 36 | end; 37 | 38 | end; 39 | -------------------------------------------------------------------------------- /UnWarpPlane.m: -------------------------------------------------------------------------------- 1 | function [u_hori,u_vert] = UnWarpPlane(x1,x2,x3,x4); 2 | 3 | % Recovers the two 3D directions of the rectangular patch x1x2x3x4 4 | % x1 is the origin point, ie any point of planar coordinate (x,y) on the 5 | % rectangular patch will be projected on the image plane at: 6 | % x1 + x * u_hori + y * u_vert 7 | % 8 | % Note: u_hori and u_vert are also the two vanishing points. 9 | 10 | 11 | if nargin < 4, 12 | 13 | x4 = x1(:,4); 14 | x3 = x1(:,3); 15 | x2 = x1(:,2); 16 | x1 = x1(:,1); 17 | 18 | end; 19 | 20 | 21 | % Image Projection: 22 | L1 = cross(x1,x2); 23 | L2 = cross(x4,x3); 24 | L3 = cross(x2,x3); 25 | L4 = cross(x1,x4); 26 | 27 | % Vanishing point: 28 | V1 = cross(L1,L2); 29 | V2 = cross(L3,L4); 30 | 31 | % Horizon line: 32 | H = cross(V1,V2); 33 | 34 | if H(3) < 0, H = -H; end; 35 | 36 | 37 | H = H / norm(H); 38 | 39 | 40 | X1 = x1 / dot(H,x1); 41 | X2 = x2 / dot(H,x2); 42 | X3 = x3 / dot(H,x3); 43 | X4 = x4 / dot(H,x4); 44 | 45 | scale = X1(3); 46 | 47 | X1 = X1/scale; 48 | X2 = X2/scale; 49 | X3 = X3/scale; 50 | X4 = X4/scale; 51 | 52 | 53 | u_hori = X2 - X1; 54 | u_vert = X4 - X1; 55 | -------------------------------------------------------------------------------- /inverse_motion.m: -------------------------------------------------------------------------------- 1 | function [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); 2 | 3 | % This function computes the inverse motion corresponding to (om,T) 4 | 5 | 6 | om2 = -om; 7 | dom2dom = -eye(3); 8 | dom2dT = zeros(3,3); 9 | 10 | 11 | [R,dRdom] = rodrigues(om); 12 | Rinv = R'; 13 | dRinvdR = zeros(9,9); 14 | dRinvdR([1 4 7],[1 2 3]) = eye(3); 15 | dRinvdR([2 5 8],[4 5 6]) = eye(3); 16 | dRinvdR([3 6 9],[7 8 9]) = eye(3); 17 | dRinvdom = dRinvdR * dRdom; 18 | 19 | Tt = Rinv * T; 20 | [dTtdRinv,dTtdT] = dAB(Rinv,T); 21 | 22 | T2 = -Tt; 23 | 24 | dT2dom = - dTtdRinv * dRinvdom; 25 | dT2dT = - dTtdT; 26 | 27 | 28 | return; 29 | 30 | % Test of the Jacobians: 31 | 32 | om = randn(3,1); 33 | T = 10*randn(3,1); 34 | [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); 35 | 36 | dom = randn(3,1) / 100000; 37 | dT = randn(3,1) / 100000; 38 | 39 | [om3r,T3r] = inverse_motion(om+dom,T+dT); 40 | 41 | om3p = om2 + dom2dom*dom + dom2dT*dT; 42 | T3p = T2 + dT2dom*dom + dT2dT*dT; 43 | 44 | %norm(om3r - om2) / norm(om3r - om3p) %-> Leads to infinity, since the opreation is linear! 45 | norm(T3r - T2) / norm(T3r - T3p) 46 | -------------------------------------------------------------------------------- /saveinr.m: -------------------------------------------------------------------------------- 1 | %SAVEINR Write an INRIMAGE format file 2 | % 3 | % SAVEINR(filename, im) 4 | % 5 | % Saves the specified image array in a INRIA image format file. 6 | % 7 | % SEE ALSO: loadinr 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | % Peter Corke 1996 12 | 13 | function saveinr(fname, im) 14 | 15 | fid = fopen(fname, 'w'); 16 | [r,c] = size(im'); 17 | 18 | % build the header 19 | hdr = []; 20 | s = sprintf('#INRIMAGE-4#{\n'); 21 | hdr = [hdr s]; 22 | s = sprintf('XDIM=%d\n',c); 23 | hdr = [hdr s]; 24 | s = sprintf('YDIM=%d\n',r); 25 | hdr = [hdr s]; 26 | s = sprintf('ZDIM=1\n'); 27 | hdr = [hdr s]; 28 | s = sprintf('VDIM=1\n'); 29 | hdr = [hdr s]; 30 | s = sprintf('TYPE=float\n'); 31 | hdr = [hdr s]; 32 | s = sprintf('PIXSIZE=32\n'); 33 | hdr = [hdr s]; 34 | s = sprintf('SCALE=2**0\n'); 35 | hdr = [hdr s]; 36 | s = sprintf('CPU=sun\n#'); 37 | hdr = [hdr s]; 38 | 39 | % make it 256 bytes long and write it 40 | hdr256 = zeros(1,256); 41 | hdr256(1:length(hdr)) = hdr; 42 | fwrite(fid, hdr256, 'uchar'); 43 | 44 | % now the binary data 45 | fwrite(fid, im', 'float32'); 46 | fclose(fid) 47 | -------------------------------------------------------------------------------- /script_fit_distortion.m: -------------------------------------------------------------------------------- 1 | 2 | satis_distort = 0; 3 | 4 | disp(['Estimated focal: ' num2str(f_g) ' pixels']); 5 | 6 | while ~satis_distort, 7 | 8 | k_g = input('Guess for distortion factor kc ([]=0): '); 9 | 10 | if isempty(k_g), k_g = 0; end; 11 | 12 | xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g); 13 | 14 | xu = xy_corners_undist(1,:)'; 15 | yu = xy_corners_undist(2,:)'; 16 | 17 | [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 18 | 19 | XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu; 20 | XX(1,:) = f_g*XX(1,:)+c_g(1); 21 | XX(2,:) = f_g*XX(2,:)+c_g(2); 22 | 23 | figure(2); 24 | image(I); 25 | colormap(map); 26 | zoom on; 27 | hold on; 28 | %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro'); 29 | plot(XX(1,:),XX(2,:),'r+'); 30 | title('The red crosses should be on the grid corners...'); 31 | hold off; 32 | 33 | satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); 34 | 35 | satis_distort = ~isempty(satis_distort); 36 | 37 | 38 | end; 39 | -------------------------------------------------------------------------------- /combine_calib.m: -------------------------------------------------------------------------------- 1 | %%% Script that combines two calibration sets together. 2 | 3 | 4 | dir; 5 | 6 | name1 = input('Calibration file name #1: ','s'); 7 | name2 = input('Calibration file name #2: ','s'); 8 | 9 | 10 | load(name1); 11 | 12 | n_ima_1 = n_ima; 13 | 14 | 15 | load(name2); 16 | 17 | n_ima_2= n_ima; 18 | active_images_2 = active_images; 19 | 20 | for kk=n_ima:-1:1, 21 | 22 | eval(['X_' num2str(kk+n_ima_1) '=X_' num2str(kk) ';' ]); 23 | eval(['x_' num2str(kk+n_ima_1) '=x_' num2str(kk) ';' ]); 24 | eval(['dX_' num2str(kk+n_ima_1) '=dX_' num2str(kk) ';' ]); 25 | eval(['dY_' num2str(kk+n_ima_1) '=dY_' num2str(kk) ';' ]); 26 | eval(['n_sq_x_' num2str(kk+n_ima_1) '=n_sq_x_' num2str(kk) ';' ]); 27 | eval(['n_sq_y_' num2str(kk+n_ima_1) '=n_sq_y_' num2str(kk) ';' ]); 28 | eval(['wintx_' num2str(kk+n_ima_1) '=wintx_' num2str(kk) ';' ]); 29 | eval(['winty_' num2str(kk+n_ima_1) '=winty_' num2str(kk) ';' ]); 30 | 31 | end; 32 | 33 | load(name1); 34 | 35 | n_ima = n_ima + n_ima_2; 36 | active_images = [ active_images active_images_2]; 37 | 38 | %no_image = 1; 39 | 40 | clear calib_name format_image type_numbering image_numbers N_slots -------------------------------------------------------------------------------- /click_stereo.m: -------------------------------------------------------------------------------- 1 | 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); 2 | 3 | 4 | figure(1); 5 | image(IL); 6 | 7 | figure(2); 8 | image(IR); 9 | 10 | [ny,nx] = size(IL); 11 | 12 | xL = []; 13 | xR = []; 14 | 15 | for kk = 1:NUMBER_OF_POINTS, 16 | 17 | figure(1); 18 | hold on; 19 | x = ginput(1); 20 | plot(x(1),x(2),'g.'); 21 | hold off; 22 | x = x'-1; 23 | 24 | xL = [xL x]; 25 | 26 | [epipole] = compute_epipole(x,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); 27 | 28 | figure(2); 29 | hold on; 30 | h = plot(epipole(1,:)+1,epipole(2,:)+1,'r.','markersize',1); 31 | hold off; 32 | 33 | x2 = ginput(1); 34 | x2 = x2' - 1; 35 | 36 | NN = size(epipole,2); 37 | d = sum((epipole - repmat(x2,1,NN)).^2); 38 | [junk,indmin] = min(d); 39 | 40 | x2 = epipole(:,indmin); 41 | 42 | xR = [xR x2]; 43 | 44 | delete(h); 45 | 46 | figure(2); 47 | hold on; 48 | plot(x2(1)+1,x2(2)+1,'g.'); 49 | drawnow; 50 | hold off; 51 | 52 | end; 53 | 54 | -------------------------------------------------------------------------------- /extract_distortion_data.m: -------------------------------------------------------------------------------- 1 | %%% Small script file that etst 2 | 3 | %load Calib_Results; 4 | 5 | % Collect all the points distorted (xd) and undistorted (xn) from the 6 | % images: 7 | 8 | xn = []; 9 | xd = []; 10 | for kk = ind_active, 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | xd_kk = normalize_pixel(x_kk,fc,cc,zeros(5,1),alpha_c); 13 | eval(['X_kk = X_' num2str(kk) ';']); 14 | eval(['omckk = omc_' num2str(kk) ';']); 15 | eval(['Tckk = Tc_' num2str(kk) ';']); 16 | xn_kk = project_points2(X_kk,omckk,Tckk); 17 | xd = [xd xd_kk]; 18 | xn = [xn xn_kk]; 19 | end; 20 | 21 | 22 | % Data points: 23 | r = sqrt(sum(xn.^2)); % The undistorted radii 24 | rp = sqrt(sum(xd.^2)); % The distorted radii 25 | 26 | %--- Try different analytical models to fit r_prime = D(r) 27 | 28 | ri = 0.005:.005:max(r); 29 | 30 | % Calibration toolbox model: 31 | rt = ri .* (1 + kc(1)*ri.^2 + kc(2)*ri.^4 + kc(5)*ri.^6); 32 | 33 | 34 | 35 | return; 36 | 37 | 38 | figure(10); 39 | clf; 40 | h1 = plot(r,rp,'r.','markersize',.1); hold on; 41 | h2 = plot(ri,rt,'r-','linewidth',.1); 42 | title('Radial distortion function (with unit focal) - r prime = D(r)'); 43 | xlabel('r'); 44 | ylabel('r prime'); 45 | zoom on; 46 | 47 | -------------------------------------------------------------------------------- /normalize_pixel_fisheye.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Fisheye distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | 18 | if nargin < 5, 19 | alpha_c = 0; 20 | if nargin < 4; 21 | kc = [0;0;0;0;0]; 22 | if nargin < 3; 23 | cc = [0;0]; 24 | if nargin < 2, 25 | fc = [1;1]; 26 | end; 27 | end; 28 | end; 29 | end; 30 | 31 | 32 | % First: Subtract principal point, and divide by the focal length: 33 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 34 | 35 | % Second: undo skew 36 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 37 | 38 | % Third: Compensate for lens distortion: 39 | xn = comp_fisheye_distortion(x_distort,kc); 40 | 41 | -------------------------------------------------------------------------------- /loadinr.m: -------------------------------------------------------------------------------- 1 | %LOADINR Load an INRIMAGE format file 2 | % 3 | % LOADINR(filename, im) 4 | % 5 | % Load an INRIA image format file and return it as a matrix 6 | % 7 | % SEE ALSO: saveinr 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | 12 | % Peter Corke 1996 13 | 14 | function im = loadinr(fname, im) 15 | 16 | fid = fopen(fname, 'r'); 17 | 18 | s = fgets(fid); 19 | if strcmp(s(1:12), '#INRIMAGE-4#') == 0, 20 | error('not INRIMAGE format'); 21 | end 22 | 23 | % not very complete, only looks for the X/YDIM keys 24 | while 1, 25 | s = fgets(fid); 26 | n = length(s) - 1; 27 | if s(1) == '#', 28 | break 29 | end 30 | if strcmp(s(1:5), 'XDIM='), 31 | cols = str2num(s(6:n)); 32 | end 33 | if strcmp(s(1:5), 'YDIM='), 34 | rows = str2num(s(6:n)); 35 | end 36 | if strcmp(s(1:4), 'CPU='), 37 | if strcmp(s(5:n), 'sun') == 0, 38 | error('not sun data ordering'); 39 | end 40 | end 41 | 42 | end 43 | disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)]) 44 | 45 | % now the binary data 46 | fseek(fid, 256, 'bof'); 47 | [im count] = fread(fid, [cols rows], 'float32'); 48 | im = im'; 49 | if count ~= (rows*cols), 50 | error('file too short'); 51 | end 52 | fclose(fid); 53 | -------------------------------------------------------------------------------- /project2_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = project2_oulu(X,R,T,f,t,k) 2 | %PROJECT Subsidiary to calib 3 | 4 | % (c) Pietro Perona -- March 24, 1994 5 | % California Institute of Technology 6 | % Pasadena, CA 7 | % 8 | % Renamed because project exists in matlab 5.2!!! 9 | % Now uses the more elaborate intrinsic model from Oulu 10 | 11 | 12 | 13 | [m,n] = size(X); 14 | 15 | Y = R*X + T*ones(1,n); 16 | Z = Y(3,:); 17 | 18 | f = f(:); %% make a column vector 19 | if length(f)==1, 20 | f = [f f]'; 21 | end; 22 | 23 | x = (Y(1:2,:) ./ (ones(2,1) * Z)) ; 24 | 25 | 26 | radius_2 = x(1,:).^2 + x(2,:).^2; 27 | 28 | if length(k) > 1, 29 | 30 | radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2)); 31 | 32 | if length(k) < 4, 33 | 34 | delta_x = zeros(2,n); 35 | 36 | else 37 | 38 | delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ; 39 | k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)]; 40 | 41 | end; 42 | 43 | 44 | else 45 | 46 | radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2)); 47 | 48 | delta_x = zeros(2,n); 49 | 50 | end; 51 | 52 | 53 | x = (x .* radial_distortion + delta_x).* (f * ones(1,n)) + t*ones(1,n); 54 | -------------------------------------------------------------------------------- /saving_stereo_calib.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat...\n'); 2 | 3 | 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'; 4 | 5 | for kk = 1:n_ima, 6 | if active_images(kk), 7 | string_save = [string_save ' x_right_' num2str(kk) ' X_right_' num2str(kk) ' x_left_' num2str(kk) ' 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)]; 8 | end; 9 | end; 10 | eval(string_save); 11 | -------------------------------------------------------------------------------- /run_error_analysis.m: -------------------------------------------------------------------------------- 1 | %%% Program that launchs the complete 2 | 3 | for N_ima_active = 1:30, 4 | 5 | error_analysis; 6 | 7 | end; 8 | 9 | 10 | 11 | return; 12 | 13 | 14 | f = []; 15 | f_std = []; 16 | 17 | c = []; 18 | c_std = []; 19 | 20 | k = []; 21 | k_std = []; 22 | 23 | NN = 30; 24 | 25 | for rr = 1:NN, 26 | 27 | load(['Calib_Accuracies_' num2str(rr)]); 28 | 29 | [m1,s1] = mean_std_robust(fc_list(1,:)); 30 | [m2,s2] = mean_std_robust(fc_list(2,:)); 31 | 32 | f = [f [m1;m2]]; 33 | f_std = [f_std [s1;s2]]; 34 | 35 | [m1,s1] = mean_std_robust(cc_list(1,:)); 36 | [m2,s2] = mean_std_robust(cc_list(2,:)); 37 | 38 | c = [c [m1;m2]]; 39 | c_std = [c_std [s1;s2]]; 40 | 41 | [m1,s1] = mean_std_robust(kc_list(1,:)); 42 | [m2,s2] = mean_std_robust(kc_list(2,:)); 43 | [m3,s3] = mean_std_robust(kc_list(3,:)); 44 | [m4,s4] = mean_std_robust(kc_list(4,:)); 45 | 46 | k = [k [m1;m2;m3;m4]]; 47 | k_std = [k_std [s1;s2;s3;s4]]; 48 | 49 | end; 50 | 51 | figure(1); 52 | errorbar([1:NN;1:NN]',f',f_std'); 53 | figure(2); 54 | errorbar([1:NN;1:NN]',c',c_std'); 55 | figure(3); 56 | errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std'); 57 | 58 | figure(4); 59 | semilogy(f_std'); 60 | 61 | figure(5); 62 | semilogy(c_std'); 63 | 64 | figure(6); 65 | semilogy(k_std'); 66 | -------------------------------------------------------------------------------- /comp_error_calib.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% 2 | 3 | check_active_images; 4 | 5 | % Reproject the patterns on the images, and compute the pixel errors: 6 | 7 | ex = []; % Global error vector 8 | x = []; % Detected corners on the image plane 9 | y = []; % Reprojected points 10 | 11 | if ~exist('alpha_c'), 12 | alpha_c = 0; 13 | end; 14 | 15 | for kk = 1:n_ima, 16 | 17 | eval(['omckk = omc_' num2str(kk) ';']); 18 | eval(['Tckk = Tc_' num2str(kk) ';']); 19 | 20 | if active_images(kk) & (~isnan(omckk(1,1))), 21 | 22 | %Rkk = rodrigues(omckk); 23 | 24 | eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); 25 | 26 | eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); 27 | 28 | eval(['x_kk = x_' num2str(kk) ';']); 29 | 30 | eval(['ex = [ex ex_' num2str(kk) '];']); 31 | eval(['x = [x x_' num2str(kk) '];']); 32 | eval(['y = [y y_' num2str(kk) '];']); 33 | 34 | else 35 | 36 | % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); 37 | 38 | 39 | % If inactivated image, the error does not make sense: 40 | eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); 41 | 42 | end; 43 | 44 | end; 45 | 46 | err_std = std(ex')'; 47 | -------------------------------------------------------------------------------- /normalize.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | % 20 | %comp_distortion_oulu: undistort pixel coordinates. 21 | 22 | if nargin < 5, 23 | alpha_c = 0; 24 | if nargin < 4; 25 | kc = [0;0;0;0;0]; 26 | if nargin < 3; 27 | cc = [0;0]; 28 | if nargin < 2, 29 | fc = [1;1]; 30 | end; 31 | end; 32 | end; 33 | end; 34 | 35 | 36 | % First: Subtract principal point, and divide by the focal length: 37 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 38 | 39 | % Second: undo skew 40 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 41 | 42 | if norm(kc) ~= 0, 43 | % Third: Compensate for lens distortion: 44 | xn = comp_distortion_oulu(x_distort,kc); 45 | else 46 | xn = x_distort; 47 | end; 48 | -------------------------------------------------------------------------------- /comp_distortion.m: -------------------------------------------------------------------------------- 1 | function [x_comp] = comp_distortion(x_dist,k2); 2 | 3 | % [x_comp] = comp_distortion(x_dist,k2); 4 | % 5 | % compensates the radial distortion of the camera 6 | % on the image plane. 7 | % 8 | % x_dist : the image points got without considering the 9 | % radial distortion. 10 | % x : The image plane points after correction for the distortion 11 | % 12 | % x and x_dist are 2xN arrays 13 | % 14 | % NOTE : This compensation has to be done after the substraction 15 | % of the center of projection, and division by the focal 16 | % length. 17 | % 18 | % (do it up to a second order approximation) 19 | 20 | 21 | [two,N] = size(x_dist); 22 | 23 | 24 | if (two ~= 2 ), 25 | error('ERROR : The dimension of the points should be 2xN'); 26 | end; 27 | 28 | 29 | if length(k2) > 1, 30 | 31 | [x_comp] = comp_distortion_oulu(x_dist,k2); 32 | 33 | else 34 | 35 | radius_2= x_dist(1,:).^2 + x_dist(2,:).^2; 36 | radial_distortion = 1 + ones(2,1)*(k2 * radius_2); 37 | radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:); 38 | radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp); 39 | x_comp = x_dist ./ radial_distortion; 40 | 41 | end; 42 | 43 | %% Function completely checked : It works fine !!! -------------------------------------------------------------------------------- /comp_error_calib_fisheye.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% 2 | 3 | check_active_images; 4 | 5 | % Reproject the patterns on the images, and compute the pixel errors: 6 | 7 | ex = []; % Global error vector 8 | x = []; % Detected corners on the image plane 9 | y = []; % Reprojected points 10 | 11 | if ~exist('alpha_c'), 12 | alpha_c = 0; 13 | end; 14 | 15 | for kk = 1:n_ima, 16 | 17 | eval(['omckk = omc_' num2str(kk) ';']); 18 | eval(['Tckk = Tc_' num2str(kk) ';']); 19 | 20 | if active_images(kk) & (~isnan(omckk(1,1))), 21 | 22 | %Rkk = rodrigues(omckk); 23 | 24 | eval(['y_' num2str(kk) ' = project_points_fisheye(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); 25 | 26 | eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); 27 | 28 | eval(['x_kk = x_' num2str(kk) ';']); 29 | 30 | eval(['ex = [ex ex_' num2str(kk) '];']); 31 | eval(['x = [x x_' num2str(kk) '];']); 32 | eval(['y = [y y_' num2str(kk) '];']); 33 | 34 | else 35 | 36 | % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); 37 | 38 | 39 | % If inactivated image, the error does not make sense: 40 | eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); 41 | 42 | end; 43 | 44 | end; 45 | 46 | err_std = std(ex')'; 47 | -------------------------------------------------------------------------------- /error_depth_list.m: -------------------------------------------------------------------------------- 1 | function err_total = error_depth_list(param_dist,xcn_list,xpn_list,R,T,X_shape_list,ind_list); 2 | 3 | 4 | N_view = length(ind_list); 5 | 6 | err_total = []; 7 | 8 | N_pts = zeros(1,N_view); 9 | 10 | for kk = 1:N_view, 11 | 12 | xcn = xcn_list{kk}; 13 | xpn = xpn_list{kk}; 14 | ind = ind_list{kk}; 15 | 16 | xpn = xpn([1 3],:); 17 | 18 | X_shape = X_shape_list{kk}; 19 | 20 | 21 | X_new = depth_compute(xcn,xpn,[param_dist],R,T); 22 | 23 | 24 | N_pt_calib = size(xcn,2); 25 | 26 | % UnNormalized shape extraction: 27 | 28 | X_shape2 = X_new; 29 | X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); 30 | 31 | % map the second vector at [1;0;0]: 32 | 33 | omu = -cross([1;0;0],X_shape2(:,2)); 34 | omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); 35 | Ru = rodrigues(omu); 36 | 37 | X_shape2 = Ru* X_shape2; 38 | 39 | omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); 40 | omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); 41 | Ru2 = rodrigues(omu2); 42 | 43 | X_shape2 = Ru2* X_shape2; 44 | 45 | 46 | % Error: 47 | 48 | err_shape = X_shape2(:,2:end) - X_shape(:,2:end); 49 | 50 | err_shape = err_shape(:); 51 | 52 | N_pts(kk) = N_pt_calib; 53 | 54 | err_total = [ err_total ; err_shape ]; 55 | 56 | end; 57 | 58 | 59 | %err_depth = Z_new - Z_ref; 60 | -------------------------------------------------------------------------------- /normalize_pixel.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | % 20 | %comp_distortion_oulu: undistort pixel coordinates. 21 | 22 | if nargin < 5, 23 | alpha_c = 0; 24 | if nargin < 4; 25 | kc = [0;0;0;0;0]; 26 | if nargin < 3; 27 | cc = [0;0]; 28 | if nargin < 2, 29 | fc = [1;1]; 30 | end; 31 | end; 32 | end; 33 | end; 34 | 35 | 36 | % First: Subtract principal point, and divide by the focal length: 37 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 38 | 39 | % Second: undo skew 40 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 41 | 42 | if norm(kc) ~= 0, 43 | % Third: Compensate for lens distortion: 44 | xn = comp_distortion_oulu(x_distort,kc); 45 | else 46 | xn = x_distort; 47 | end; 48 | -------------------------------------------------------------------------------- /comp_fisheye_distortion.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_fisheye_distortion(xd,k) 2 | 3 | %comp_fisheye_distortion.m 4 | % 5 | %[x] = comp_fisheye_distortion(xd,k) 6 | % 7 | %Compensates for fisheye distortions 8 | % 9 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 10 | % k: Fisheye distortion coefficients (5x1 vector) 11 | % 12 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 13 | % 14 | %Method: Iterative method for compensation. 15 | % 16 | %NOTE: This compensation has to be done after the subtraction 17 | % of the principal point, and division by the focal length. 18 | 19 | theta_d = sqrt(xd(1,:).^2 + xd(2,:).^2); 20 | theta = theta_d; % initial guess 21 | for kk=1:20, 22 | theta = theta_d ./ (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); 23 | end; 24 | scaling = tan(theta) ./ theta_d; 25 | 26 | x = xd .* (ones(2,1)*scaling); 27 | 28 | return; 29 | 30 | % Test 31 | 32 | n = 4; 33 | xxx = rand(2,n); 34 | 35 | xxx = [[1.0840 0.3152 0.2666 0.9347 ];[ 0.7353 0.6101 -0.6415 -0.8006]]; 36 | 37 | k = 0.00 * randn(4,1); 38 | 39 | [xd] = comp_fisheye_distortion(xxx,k); 40 | x2 = apply_fisheye_distortion(xd,k); 41 | norm(x2-xd)/norm(x2-xxx) 42 | 43 | 44 | %[xd] = apply_fisheye_distortion(xxx,k); 45 | %x2 = comp_fisheye_distortion(xd,k); 46 | %norm(x2-xd)/norm(x2-xxx) 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /comp_distortion_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_distortion_oulu(xd,k); 2 | 3 | %comp_distortion_oulu.m 4 | % 5 | %[x] = comp_distortion_oulu(xd,k) 6 | % 7 | %Compensates for radial and tangential distortion. Model From Oulu university. 8 | %For more informatino about the distortion model, check the forward projection mapping function: 9 | %project_points.m 10 | % 11 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 12 | % k: Distortion coefficients (radial and tangential) (4x1 vector) 13 | % 14 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 15 | % 16 | %Method: Iterative method for compensation. 17 | % 18 | %NOTE: This compensation has to be done after the subtraction 19 | % of the principal point, and division by the focal length. 20 | 21 | 22 | if length(k) == 1, 23 | 24 | [x] = comp_distortion(xd,k); 25 | 26 | else 27 | 28 | k1 = k(1); 29 | k2 = k(2); 30 | k3 = k(5); 31 | p1 = k(3); 32 | p2 = k(4); 33 | 34 | x = xd; % initial guess 35 | 36 | for kk=1:20, 37 | 38 | r_2 = sum(x.^2); 39 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 40 | delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); 41 | p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; 42 | x = (xd - delta_x)./(ones(2,1)*k_radial); 43 | 44 | end; 45 | 46 | end; 47 | 48 | -------------------------------------------------------------------------------- /compose_motion.m: -------------------------------------------------------------------------------- 1 | function [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); 2 | 3 | % Rotations: 4 | 5 | [R1,dR1dom1] = rodrigues(om1); 6 | [R2,dR2dom2] = rodrigues(om2); 7 | 8 | R3 = R2 * R1; 9 | 10 | [dR3dR2,dR3dR1] = dAB(R2,R1); 11 | 12 | [om3,dom3dR3] = rodrigues(R3); 13 | 14 | dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; 15 | dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; 16 | 17 | dom3dT1 = zeros(3,3); 18 | dom3dT2 = zeros(3,3); 19 | 20 | 21 | % Translations: 22 | 23 | T3t = R2 * T1; 24 | 25 | [dT3tdR2,dT3tdT1] = dAB(R2,T1); 26 | 27 | dT3tdom2 = dT3tdR2 * dR2dom2; 28 | 29 | 30 | T3 = T3t + T2; 31 | 32 | dT3dT1 = dT3tdT1; 33 | dT3dT2 = eye(3); 34 | 35 | dT3dom2 = dT3tdom2; 36 | dT3dom1 = zeros(3,3); 37 | 38 | 39 | return; 40 | 41 | % Test of the Jacobians: 42 | 43 | om1 = randn(3,1); 44 | om2 = randn(3,1); 45 | T1 = 10*randn(3,1); 46 | T2 = 10*randn(3,1); 47 | 48 | [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); 49 | 50 | 51 | dom1 = randn(3,1) / 1000; 52 | dom2 = randn(3,1) / 1000; 53 | dT1 = randn(3,1) / 10000; 54 | dT2 = randn(3,1) / 10000; 55 | 56 | [om3r,T3r] = compose_motion(om1+dom1,T1+dT1,om2+dom2,T2+dT2); 57 | 58 | om3p = om3 + dom3dom1*dom1 + dom3dT1*dT1 + dom3dom2*dom2 + dom3dT2*dT2; 59 | T3p = T3 + dT3dom1*dom1 + dT3dT1*dT1 + dT3dom2*dom2 + dT3dT2*dT2; 60 | 61 | norm(om3r - om3) / norm(om3r - om3p) 62 | norm(T3r - T3) / norm(T3r - T3p) 63 | -------------------------------------------------------------------------------- /compute_collineation.m: -------------------------------------------------------------------------------- 1 | function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); 2 | 3 | % new formalism using homographies 4 | 5 | a00 = a00 / a00(3); 6 | a10 = a10 / a10(3); 7 | a11 = a11 / a11(3); 8 | a01 = a01 / a01(3); 9 | 10 | 11 | % Prenormalization of point coordinates (very important): 12 | % (Affine normalization) 13 | 14 | ax = [a00(1);a10(1);a11(1);a01(1)]; 15 | ay = [a00(2);a10(2);a11(2);a01(2)]; 16 | 17 | mxx = mean(ax); 18 | myy = mean(ay); 19 | ax = ax - mxx; 20 | ay = ay - myy; 21 | 22 | scxx = mean(abs(ax)); 23 | scyy = mean(abs(ay)); 24 | 25 | 26 | Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; 27 | inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; 28 | 29 | 30 | a00n = Hnorm*a00; 31 | a10n = Hnorm*a10; 32 | a11n = Hnorm*a11; 33 | a01n = Hnorm*a01; 34 | 35 | 36 | % Computation of the vanishing points: 37 | 38 | V1n = cross(cross(a00n,a10n),cross(a01n,a11n)); 39 | V2n = cross(cross(a00n,a01n),cross(a10n,a11n)); 40 | 41 | V1 = inv_Hnorm*V1n; 42 | V2 = inv_Hnorm*V2n; 43 | 44 | 45 | % Normalizaion of the vanishing points: 46 | 47 | V1n = V1n/norm(V1n); 48 | V2n = V2n/norm(V2n); 49 | 50 | 51 | % Closed-form solution of the coefficients: 52 | 53 | alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2)); 54 | 55 | alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2)); 56 | 57 | 58 | % Remaining Homography 59 | 60 | Hrem = [alpha_x*V1n alpha_y*V2n a00n]; 61 | 62 | 63 | % Final homography: 64 | 65 | H = inv_Hnorm*Hrem; 66 | 67 | -------------------------------------------------------------------------------- /calib_gui.m: -------------------------------------------------------------------------------- 1 | %function calib_gui(mode) 2 | 3 | % calib_gui(mode) 4 | % 5 | % Runs the Camera Calibration Toolbox. 6 | % Set mode to 1 to run the memory efficient version. 7 | % Any other value for mode will run the normal version (see documentation) 8 | % 9 | % INFORMATION ABOUT THE MEMORY EFFICIENT MODE FOR THE CAMERA CALIBRATION TOOLBOX: 10 | % 11 | % If your calibration images are large, or if you calibrate using a lot of images, you may have experienced memory problems 12 | % in Matlab when using the calibration toolbox (OUT OF MEMORY errors). If this is the case, you can now run the 13 | % new memory efficient version of the toolbox that loads every image one by one without storing them all in memory. 14 | % If you choose to run the standard version of the toolbox now, you can always switch to the other memory efficient mode 15 | % later in case the OUT OF MEMORY error message is encountered. The two modes of operation are totally compatible. 16 | 17 | more off; 18 | %do_braindead_shortcircuit_evaluation (0); 19 | 20 | cell_list = {}; 21 | 22 | fig_number = 1; 23 | 24 | title_figure = 'Camera Calibration Toolbox - Select mode of operation:'; 25 | 26 | cell_list{1,1} = {'Standard (all the images are stored in memory)','calib_gui_normal;'}; 27 | cell_list{2,1} = {'Memory efficient (the images are loaded one by one)','calib_gui_no_read;'}; 28 | cell_list{3,1} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; 29 | 30 | show_window(cell_list,fig_number,title_figure,290,18,0,'clean',12); 31 | -------------------------------------------------------------------------------- /apply_fisheye_distortion.m: -------------------------------------------------------------------------------- 1 | function [xd, dxd_dx] = apply_fisheye_distortion(x,k) 2 | 3 | %apply_fisheye_distortion.m 4 | % 5 | %[x] = apply_fisheye_distortion(xd,k) 6 | % 7 | %Apply the fisheye distortions 8 | % 9 | %INPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 10 | % k: Fisheye distortion coefficients (5x1 vector) 11 | % 12 | %OUTPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 13 | % dxd_dx : jacobian (2x2xN matrix) 14 | 15 | r = sqrt(x(1,:).^2 + x(2,:).^2); 16 | 17 | theta = atan(r); 18 | theta_d = theta .* (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); 19 | 20 | scaling = ones(1,length(r)); 21 | 22 | ind_good = find(r > 1e-8); 23 | 24 | scaling(ind_good) = theta_d(ind_good) ./ r(ind_good); 25 | 26 | xd = x .* (ones(2,1)*scaling); 27 | 28 | if (nargout > 1) 29 | n = size(x,2); 30 | dr_dx = x; % 2xn 31 | dr_dx(:,ind_good) = dr_dx(:,ind_good) ./ repmat(r(ind_good), [2,1]); 32 | 33 | theta_d_r2 = scaling; 34 | theta_d_r2(ind_good) = theta_d_r2(ind_good) ./ r(ind_good); 35 | 36 | dtheta_dr = 1 ./ (1 + r.^2); 37 | dtheta_d_dtheta = 1 + 3*k(1)*theta.^2 + 5*k(2)*theta.^4 + ... 38 | 7*k(3)*theta.^6 + 9*k(4)*theta.^8; 39 | dtheta_d_dr_r = dtheta_d_dtheta .* dtheta_dr; % 1xn 40 | dtheta_d_dr_r(ind_good) = dtheta_d_dr_r(ind_good) ./ r(ind_good); 41 | 42 | dxd_dx = zeros(2,2,n); 43 | for i = 1:n 44 | dxd_dx(:,:,i) = scaling(i)*eye(2,2) + x(:,i)*(dtheta_d_dr_r(i) - ... 45 | theta_d_r2(i))*dr_dx(:,i)'; 46 | end 47 | end 48 | -------------------------------------------------------------------------------- /calib_gui_no_read.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_no_read, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Camera Calibration Toolbox - Memory efficient version'; 14 | 15 | cell_list{1,1} = {'Image names','data_calib_no_read;'}; 16 | cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; 17 | cell_list{1,3} = {'Extract grid corners','click_calib_no_read;'}; 18 | cell_list{1,4} = {'Calibration','go_calib_optim_no_read;'}; 19 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 20 | cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; 21 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 22 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_no_read;'}; 23 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 24 | cell_list{3,2} = {'Save','saving_calib;'}; 25 | cell_list{3,3} = {'Load','loading_calib;'}; 26 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 27 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 28 | cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; 29 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 30 | cell_list{4,4} = {'Show calib results','show_calib_results;'}; 31 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 32 | 33 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 34 | 35 | 36 | %-------- End editable region -------------% 37 | %-------- End editable region -------------% 38 | -------------------------------------------------------------------------------- /calib_gui_normal.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_normal, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Camera Calibration Toolbox - Standard Version'; 14 | 15 | clear fc cc kc KK 16 | kc = zeros(5,1); 17 | clearwin; 18 | 19 | cell_list{1,1} = {'Image names','data_calib;'}; 20 | cell_list{1,2} = {'Read images','ima_read_calib;'}; 21 | cell_list{1,3} = {'Extract grid corners','click_calib;'}; 22 | cell_list{1,4} = {'Calibration','go_calib_optim;'}; 23 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 24 | cell_list{2,2} = {'Reproject on images','reproject_calib;'}; 25 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 26 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib;'}; 27 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 28 | cell_list{3,2} = {'Save','saving_calib;'}; 29 | cell_list{3,3} = {'Load','loading_calib;'}; 30 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 31 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 32 | cell_list{4,2} = {'Undistort image','undistort_image;'}; 33 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 34 | cell_list{4,4} = {'Show calib results','show_calib_results;'}; 35 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 36 | 37 | 38 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 39 | 40 | 41 | %-------- End editable region -------------% 42 | %-------- End editable region -------------% 43 | 44 | -------------------------------------------------------------------------------- /count_squares.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares(I,x1,y1,x2,y2,win); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | if ((x1 - x2)^2+(y1-y2)^2) < win, 12 | ns = -1; 13 | return; 14 | end; 15 | 16 | lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1]; 17 | lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda; 18 | l1 = lambda + [0;0;win]; 19 | l2 = lambda - [0;0;win]; 20 | dx = x2-x1; 21 | dy = y2 - y1; 22 | 23 | if abs(dx) > abs(dy), 24 | if x2 > x1, 25 | xs = x1:x2; 26 | else 27 | xs = x1:-1:x2; 28 | end; 29 | ys = -(lambda(3) + lambda(1)*xs)/lambda(2); 30 | else 31 | if y2 > y1, 32 | ys = y1:y2; 33 | else 34 | ys = y1:-1:y2; 35 | end; 36 | xs = -(lambda(3) + lambda(2)*ys)/lambda(1); 37 | end; 38 | 39 | Np = length(xs); 40 | xs_mat = ones(2*win + 1,1)*xs; 41 | ys_mat = ones(2*win + 1,1)*ys; 42 | win_mat = (-win:win)'*ones(1,Np); 43 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 44 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 45 | ind_mat = (xs_mat2 - 1) * ny + ys_mat2; 46 | ima_patch = zeros(2*win + 1,Np); 47 | ima_patch(:) = I(ind_mat(:)); 48 | 49 | %ima2 = ima_patch(:,win+1:end-win); 50 | 51 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 52 | out_f = sum(filtk.*ima_patch); 53 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 54 | out_f_f = out_f_f(win+1:end-win); 55 | 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; 56 | 57 | return; 58 | -------------------------------------------------------------------------------- /calib_gui_fisheye.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_no_read, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Fisheye Camera Calibration Toolbox'; 14 | 15 | cell_list{1,1} = {'Image names','data_calib_no_read;'}; 16 | cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; 17 | cell_list{1,3} = {'Extract grid corners','click_calib_fisheye_no_read;'}; 18 | cell_list{1,4} = {'Calibration','go_calib_optim_fisheye_no_read;'}; 19 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 20 | cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; 21 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 22 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_fisheye_no_read;'}; %recomp_corner_calib_no_read;'}; 23 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 24 | cell_list{3,2} = {'Save','saving_calib_fisheye;'}; 25 | cell_list{3,3} = {'Load','loading_calib;'}; 26 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 27 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 28 | cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; 29 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 30 | cell_list{4,4} = {'Show calib results','show_calib_results_fisheye;'}; 31 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 32 | 33 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 34 | 35 | 36 | %-------- End editable region -------------% 37 | %-------- End editable region -------------% 38 | 39 | 40 | -------------------------------------------------------------------------------- /anisdiff.m: -------------------------------------------------------------------------------- 1 | function Is = anisdiff(I,sigI,NIter); 2 | 3 | if nargin < 3, 4 | NIter = 4; 5 | if nargin < 2, 6 | sigI = 10; 7 | end; 8 | end; 9 | 10 | % Function that diffuse an image anisotropially. 11 | 12 | Is = I; 13 | 14 | [ny,nx] = size(I); 15 | 16 | c_n = zeros(ny-2,nx-2); 17 | c_s = zeros(ny-2,nx-2); 18 | c_w = zeros(ny-2,nx-2); 19 | c_e = zeros(ny-2,nx-2); 20 | c_nw = zeros(ny-2,nx-2); 21 | c_ne = zeros(ny-2,nx-2); 22 | c_sw = zeros(ny-2,nx-2); 23 | c_se = zeros(ny-2,nx-2); 24 | c_c = ones(ny-2,nx-2); 25 | 26 | 27 | for k=1:NIter, 28 | 29 | dI_n = Is(2:end-1,2:end-1) - Is(1:end-2,2:end-1); 30 | dI_s = Is(2:end-1,2:end-1) - Is(3:end,2:end-1); 31 | dI_w = Is(2:end-1,2:end-1) - Is(2:end-1,1:end-2); 32 | dI_e = Is(2:end-1,2:end-1) - Is(2:end-1,3:end); 33 | dI_nw = Is(2:end-1,2:end-1) - Is(1:end-2,1:end-2); 34 | dI_ne = Is(2:end-1,2:end-1) - Is(1:end-2,3:end); 35 | dI_sw = Is(2:end-1,2:end-1) - Is(3:end,1:end-2); 36 | dI_se = Is(2:end-1,2:end-1) - Is(3:end,3:end); 37 | 38 | 39 | c_n = exp(-.5*(dI_n/sigI).^2)/8; 40 | c_s = exp(-.5*(dI_s/sigI).^2)/8; 41 | c_w = exp(-.5*(dI_w/sigI).^2)/8; 42 | c_e = exp(-.5*(dI_e/sigI).^2)/8; 43 | c_nw = exp(-.5*(dI_nw/sigI).^2)/16; 44 | c_ne = exp(-.5*(dI_ne/sigI).^2)/16; 45 | c_sw = exp(-.5*(dI_sw/sigI).^2)/16; 46 | c_se = exp(-.5*(dI_se/sigI).^2)/16; 47 | 48 | c_c = 1 - (c_n + c_s + c_w + c_e + c_nw + c_ne + c_sw + c_se); 49 | 50 | 51 | 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) + ... 52 | 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) + ... 53 | c_sw .* Is(3:end,1:end-2) + c_se .* Is(3:end,3:end); 54 | 55 | end; 56 | -------------------------------------------------------------------------------- /script_fit_distortion_fisheye.m: -------------------------------------------------------------------------------- 1 | 2 | satis_distort = 0; 3 | 4 | while ~satis_distort, 5 | 6 | k_g_save = k_g; 7 | 8 | k_g = input(['Guess for distortion factor kc ([]=' num2str(k_g_save) '): ']); 9 | 10 | if isempty(k_g), k_g = k_g_save; end; 11 | 12 | 13 | x_n = (x - c_g(1))/f_g; 14 | y_n = (y - c_g(2))/f_g; 15 | 16 | [x_pn] = comp_fisheye_distortion([x_n' ; y_n'],[k_g;0;0;0]); 17 | 18 | 19 | % Compute the inside points through computation of the planar homography (collineation) 20 | 21 | a00 = [x_pn(1,1);x_pn(2,1);1]; 22 | a10 = [x_pn(1,2);x_pn(2,2);1]; 23 | a11 = [x_pn(1,3);x_pn(2,3);1]; 24 | a01 = [x_pn(1,4);x_pn(2,4);1]; 25 | 26 | % Compute the planar collineation: (return the normalization matrix as well) 27 | [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); 28 | 29 | 30 | % Build the grid using the planar collineation: 31 | 32 | x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; 33 | y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; 34 | pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; 35 | 36 | XXpn = Homo*pts; 37 | XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); 38 | 39 | XX = apply_fisheye_distortion(XXpn,[k_g;0;0;0]); 40 | 41 | XX(1,:) = f_g*XX(1,:) + c_g(1); 42 | XX(2,:) = f_g*XX(2,:) + c_g(2); 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | figure(2); 51 | image(I); 52 | colormap(map); 53 | zoom on; 54 | hold on; 55 | plot(XX(1,:),XX(2,:),'r+'); 56 | title('The red crosses should be on the grid corners...'); 57 | hold off; 58 | 59 | satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); 60 | 61 | satis_distort = ~isempty(satis_distort); 62 | 63 | 64 | end; 65 | -------------------------------------------------------------------------------- /projector_ima_corners.m: -------------------------------------------------------------------------------- 1 | 2 | eval(['Ip = Ip_' num2str(kk) ';']); 3 | eval(['In = In_' num2str(kk) ';']); 4 | 5 | xr = xr_list(kk); 6 | yr = yr_list(kk); 7 | 8 | fprintf(1,'Processing image %d...',kk); 9 | 10 | if ~recompute_corner, 11 | [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); 12 | xproj = x; 13 | else 14 | eval(['xproj = xproj_' num2str(kk) ';']); 15 | x = cornerfinder(xproj+1,Ip,winty,wintx); 16 | xproj = x - 1; 17 | end; 18 | 19 | Np_proj = size(x,2); 20 | 21 | figure(2); 22 | image(Ip); 23 | hold on; 24 | plot(xproj(1,:)+1,xproj(2,:)+1,'r+'); 25 | %title('Click on your reference point'); 26 | xlabel('Xc (in camera frame)'); 27 | ylabel('Yc (in camera frame)'); 28 | hold off; 29 | 30 | %disp('Click on your reference point...'); 31 | 32 | %[xr,yr] = ginput2(1); 33 | 34 | xr = xr_list(kk); 35 | yr = yr_list(kk); 36 | 37 | err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2)); 38 | ind_ref = find(err == min(err)); 39 | 40 | ref_pt = xproj(:,ind_ref); 41 | 42 | figure(2); 43 | hold on; 44 | plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off; 45 | title(['Image ' num2str(kk)]); 46 | drawnow; 47 | 48 | 49 | if ~recompute_corner, 50 | 51 | off_x = mod(ind_ref-1,n_sq_x+1); 52 | off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1)); 53 | 54 | x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj)); 55 | 56 | eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image 57 | 58 | end; 59 | 60 | eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image 61 | 62 | -------------------------------------------------------------------------------- /extract_parameters.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | check_active_images; 5 | 6 | if ~exist('solution_error') 7 | solution_error = zeros(6*n_ima + 15,1); 8 | end; 9 | 10 | fc = solution(1:2);%*** 11 | cc = solution(3:4);%*** 12 | alpha_c = solution(5);%*** 13 | kc = solution(6:10);%*** 14 | 15 | fc_error = solution_error(1:2); 16 | cc_error = solution_error(3:4); 17 | alpha_c_error = solution_error(5); 18 | kc_error = solution_error(6:10); 19 | 20 | % Calibration matrix: 21 | 22 | KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; 23 | inv_KK = inv(KK); 24 | 25 | % Extract the extrinsic paramters, and recomputer the collineations 26 | 27 | for kk = 1:n_ima, 28 | 29 | if active_images(kk), 30 | 31 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** 32 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 33 | 34 | omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 35 | Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 36 | 37 | Rckk = rodrigues(omckk); 38 | 39 | Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 40 | 41 | Hkk = Hkk / Hkk(3,3); 42 | 43 | else 44 | 45 | omckk = NaN*ones(3,1); 46 | Tckk = NaN*ones(3,1); 47 | Rckk = NaN*ones(3,3); 48 | Hkk = NaN*ones(3,3); 49 | omckk_error = NaN*ones(3,1); 50 | Tckk_error = NaN*ones(3,1); 51 | 52 | end; 53 | 54 | eval(['omc_' num2str(kk) ' = omckk;']); 55 | eval(['Rc_' num2str(kk) ' = Rckk;']); 56 | eval(['Tc_' num2str(kk) ' = Tckk;']); 57 | eval(['H_' num2str(kk) '= Hkk;']); 58 | eval(['omc_error_' num2str(kk) ' = omckk_error;']); 59 | eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /cam_proj_extract_param.m: -------------------------------------------------------------------------------- 1 | 2 | % Computation of the errors: 3 | 4 | fc = param(1:2); 5 | cc = param(3:4); 6 | alpha_c = param(5); 7 | kc = param(6:10); 8 | 9 | e_cam = []; 10 | 11 | for kk = 1:n_ima, 12 | omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); 13 | Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); 14 | 15 | eval(['Xkk = X_' num2str(kk) ';']); 16 | eval(['xkk = x_' num2str(kk) ';']); 17 | 18 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 19 | 20 | Rckk = rodrigues(omckk); 21 | eval(['omc_' num2str(kk) '= omckk;']); 22 | eval(['Tc_' num2str(kk) '= Tckk;']); 23 | eval(['Rc_' num2str(kk) '= Rckk;']); 24 | 25 | e_cam = [e_cam ekk]; 26 | 27 | end; 28 | 29 | X_proj = []; 30 | x_proj = []; 31 | 32 | for kk = 1:n_ima, 33 | eval(['xproj = xproj_' num2str(kk) ';']); 34 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 35 | eval(['Rc = Rc_' num2str(kk) ';']); 36 | eval(['Tc = Tc_' num2str(kk) ';']); 37 | Np_proj = size(xproj,2); 38 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 39 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 40 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 41 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 42 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 43 | end; 44 | 45 | 46 | fp = param((1:2)+n_ima * 6 + 10); 47 | cp = param((3:4)+n_ima * 6 + 10); 48 | alpha_p = param((5)+n_ima * 6 + 10); 49 | kp = param((6:10)+n_ima * 6 + 10); 50 | 51 | om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); 52 | T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); 53 | R = rodrigues(om); 54 | 55 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 56 | 57 | e_global = [e_cam e_proj]; 58 | 59 | -------------------------------------------------------------------------------- /extract_parameters_fisheye.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | check_active_images; 5 | 6 | if ~exist('solution_error') 7 | solution_error = zeros(6*n_ima + 15,1); 8 | end; 9 | 10 | fc = solution(1:2);%*** 11 | cc = solution(3:4);%*** 12 | alpha_c = solution(5);%*** 13 | kc = solution(6:9);%*** 14 | 15 | fc_error = solution_error(1:2); 16 | cc_error = solution_error(3:4); 17 | alpha_c_error = solution_error(5); 18 | kc_error = solution_error(6:9); 19 | 20 | % Calibration matrix: 21 | 22 | KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; 23 | inv_KK = inv(KK); 24 | 25 | % Extract the extrinsic paramters, and recomputer the collineations 26 | 27 | for kk = 1:n_ima, 28 | 29 | if active_images(kk), 30 | 31 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** 32 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 33 | 34 | omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 35 | Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 36 | 37 | Rckk = rodrigues(omckk); 38 | 39 | Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 40 | 41 | Hkk = Hkk / Hkk(3,3); 42 | 43 | else 44 | 45 | omckk = NaN*ones(3,1); 46 | Tckk = NaN*ones(3,1); 47 | Rckk = NaN*ones(3,3); 48 | Hkk = NaN*ones(3,3); 49 | omckk_error = NaN*ones(3,1); 50 | Tckk_error = NaN*ones(3,1); 51 | 52 | end; 53 | 54 | eval(['omc_' num2str(kk) ' = omckk;']); 55 | eval(['Rc_' num2str(kk) ' = Rckk;']); 56 | eval(['Tc_' num2str(kk) ' = Tckk;']); 57 | eval(['H_' num2str(kk) '= Hkk;']); 58 | eval(['omc_error_' num2str(kk) ' = omckk_error;']); 59 | eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /compute_epipole.m: -------------------------------------------------------------------------------- 1 | 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); 2 | 3 | if ~exist('D'), 4 | D = 400; 5 | end; 6 | 7 | uo = [ normalize_pixel(xLp,fc_left,cc_left,kc_left,alpha_c_left); 1 ]; 8 | 9 | S = [ 0 -T(3) T(2) 10 | T(3) 0 -T(1) 11 | -T(2) T(1) 0 ]; 12 | 13 | l_epipole = (S*R)*uo; 14 | 15 | KK_right = [fc_right(1) alpha_c_right * fc_right(1) cc_right(1) ; 0 fc_right(2) cc_right(2) ; 0 0 1]; 16 | 17 | N_line = 800; 18 | 19 | if norm(l_epipole(2)) > norm(l_epipole(1)), 20 | 21 | % Horizontal line: 22 | 23 | limit_x_pos = ((xLp(1) + D/2) - cc_right(1)) / fc_right(1); 24 | limit_x_neg = ((xLp(1) - D/2) - cc_right(1)) / fc_right(1); 25 | 26 | 27 | %limit_x_pos = ((nx-1) - cc_right(1)) / fc_right(1); 28 | %limit_x_neg = (0 - cc_right(1)) / fc_right(1); 29 | 30 | x_list = (limit_x_pos - limit_x_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_x_neg; 31 | 32 | 33 | pt = cross(repmat(l_epipole,1,N_line),[ones(1,N_line);zeros(1,N_line);-x_list]); 34 | 35 | 36 | else 37 | 38 | limit_y_pos = ((xLp(2) + D/2) - cc_right(2)) / fc_right(2); 39 | limit_y_neg = ((xLp(2) - D/2) - cc_right(2)) / fc_right(2); 40 | 41 | %limit_y_pos = ((ny-1) - cc_right(2)) / fc_right(2); 42 | %limit_y_neg = (0 - cc_right(2)) / fc_right(2); 43 | 44 | y_list = (limit_y_pos - limit_y_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_y_neg; 45 | 46 | 47 | pt = cross(repmat(l_epipole,1,N_line),[zeros(1,N_line);ones(1,N_line);-y_list]); 48 | 49 | 50 | end; 51 | 52 | 53 | pt = [pt(1,:) ./ pt(3,:) ; pt(2,:)./pt(3,:)]; 54 | ptd = apply_distortion(pt,kc_right); 55 | epipole = KK_right * [ ptd ; ones(1,N_line)]; 56 | 57 | epipole = epipole(1:2,:); -------------------------------------------------------------------------------- /comp_distortion2.m: -------------------------------------------------------------------------------- 1 | function [x_comp] = comp_distortion(x_dist,k2); 2 | 3 | % [x_comp] = comp_distortion(x_dist,k2); 4 | % 5 | % compensates the radial distortion of the camera 6 | % on the image plane. 7 | % 8 | % x_dist : the image points got without considering the 9 | % radial distortion. 10 | % k2: Radial distortion factor 11 | % 12 | % x : The image plane points after correction for the distortion 13 | % 14 | % x and x_dist are 2xN arrays 15 | % 16 | % NOTE : This compensation has to be done after the substraction 17 | % of the center of projection, and division by the focal 18 | % length. 19 | % 20 | % Solve for cubic roots using method from Numerical Recipes in C 2nd Ed. 21 | % pages 184-185. 22 | 23 | 24 | % California Institute of Technology 25 | % (c) Jean-Yves Bouguet - April 27th, 1998 26 | 27 | % fully checked! JYB, april 27th, 1998 - 2am 28 | 29 | if k2 ~= 0, 30 | 31 | [two,N] = size(x_dist); 32 | 33 | if (two ~= 2 ), 34 | error('ERROR : The dimension of the points should be 2xN'); 35 | end; 36 | 37 | 38 | ph = atan2(x_dist(2,:),x_dist(1,:)); 39 | 40 | Q = -1/(3*k2); 41 | R = -x_dist(1,:)./(2*k2*cos(ph)); 42 | 43 | R2 = R.^2; 44 | Q3 = Q^3; 45 | 46 | 47 | if k2 < 0, 48 | 49 | % this works in all practical situations (it starts failing for very large 50 | % values of k2) 51 | 52 | th = acos(R./sqrt(Q3)); 53 | r = -2*sqrt(Q)*cos((th-2*pi)/3); 54 | 55 | else 56 | 57 | % note: this always works, even for ridiculous values of k2 58 | 59 | A = (sqrt(R2-Q3)-R).^(1/3); 60 | B = Q*(1./A); 61 | r = (A+B); 62 | 63 | end; 64 | 65 | x_comp = [r.*cos(ph); r.*sin(ph)]; 66 | 67 | else 68 | 69 | x_comp = x_dist; 70 | 71 | end; 72 | 73 | x_comp = real(x_comp); 74 | -------------------------------------------------------------------------------- /projector_calib.m: -------------------------------------------------------------------------------- 1 | 2 | % load camera results (for setting active images, n_ima,...) 3 | load camera_results; 4 | 5 | 6 | % Projection settings: 7 | 8 | wintx = 12; %8; 9 | winty = 12; %8; 10 | nx = 1024; 11 | ny = 768; 12 | dX = 32; 13 | dY = 32; 14 | dXoff=511.5; 15 | dYoff=383.5; 16 | 17 | 18 | proj_name = 'proj'; %input('Basename projector calibration images (without number nor suffix): ','s'); 19 | camera_name = 'cam'; 20 | 21 | 22 | xr_list = NaN*ones(1,n_ima); 23 | yr_list = NaN*ones(1,n_ima); 24 | 25 | 26 | ind_proc = ind_active; 27 | 28 | 29 | 30 | DEBUG = 1; 31 | %ind_ima_proj = [18]; 32 | 33 | for i = ind_proc, 34 | 35 | projector_marker; 36 | 37 | end; 38 | 39 | 40 | fprintf(1,'\nExtraction of the grid corners on the image\n'); 41 | 42 | 43 | 44 | recompute_corner = 0; 45 | 46 | if recompute_corner & ~exist(['xproj_' num2str(ind_proc(1))]), 47 | if exist('projector_data'), 48 | load projector_data; 49 | else 50 | recompute_corner = 0; 51 | disp('WARNING: Cannot recompute corners. Data need to be extracted at least once'); 52 | end; 53 | end; 54 | 55 | if ~recompute_corner, 56 | disp('Manual extraction mode'); 57 | else 58 | disp('Automatic recomputation of the corners'); 59 | end; 60 | 61 | % extract the projector corners: 62 | 63 | 64 | for kk = ind_proc, 65 | 66 | projector_ima_corners 67 | 68 | end; 69 | 70 | 71 | string_save = 'save projector_data '; 72 | 73 | for kk = ind_active, 74 | string_save = [string_save ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; 75 | end; 76 | eval(string_save); 77 | 78 | 79 | 80 | 81 | return; 82 | 83 | 84 | i = 18; 85 | 86 | figure(4) 87 | image(I_29); 88 | colormap(gray(256)); 89 | hold on; 90 | plot(x_29(1,:)+1,x_29(2,:)+1,'r+','markersize',13,'linewidth',3) 91 | hold off; 92 | axis image 93 | axis off; 94 | -------------------------------------------------------------------------------- /count_squares_distorted.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | 12 | if ((x1 - x2)^2+(y1-y2)^2) < win, 13 | ns = -1; 14 | return; 15 | end; 16 | 17 | nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); 18 | alpha_x = (0:nX)/nX; 19 | pt1n = normalize_pixel([x1;y1]-1,fg,cg,kg,0); 20 | pt2n = normalize_pixel([x2;y2]-1,fg,cg,kg,0); 21 | 22 | ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; 23 | 24 | pts = apply_distortion2(ptsn,kg); 25 | pts(1,:) = fg(1)*pts(1,:) + cg(1); 26 | pts(2,:) = fg(2)*pts(2,:) + cg(2); 27 | 28 | % Check that the curve is within the image: 29 | good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... 30 | (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); 31 | 32 | if ~good_line, 33 | ns = -1; 34 | return; 35 | end; 36 | 37 | % Deviate the trajectory orthogonally: 38 | lambda = [y1 - y2 ; x2 - x1]; 39 | lambda = lambda / sqrt(sum(lambda.^2)); 40 | 41 | Np = size(pts,2); 42 | xs_mat = ones(2*win + 1,1)*pts(1,:); 43 | ys_mat = ones(2*win + 1,1)*pts(2,:); 44 | win_mat = (-win:win)'*ones(1,Np); 45 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 46 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 47 | 48 | 49 | ind_mat = (xs_mat2) * ny + ys_mat2 + 1; 50 | ima_patch = zeros(2*win + 1,Np); 51 | ima_patch(:) = I(ind_mat(:)); 52 | 53 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 54 | out_f = sum(filtk.*ima_patch); 55 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 56 | out_f_f = out_f_f(win+1:end-win); 57 | 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; 58 | 59 | 60 | -------------------------------------------------------------------------------- /count_squares_fisheye_distorted.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | 12 | if ((x1 - x2)^2+(y1-y2)^2) < win, 13 | ns = -1; 14 | return; 15 | end; 16 | 17 | nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); 18 | alpha_x = (0:nX)/nX; 19 | 20 | pt1n = normalize_pixel_fisheye([x1;y1]-1,fg,cg,kg,0); 21 | pt2n = normalize_pixel_fisheye([x2;y2]-1,fg,cg,kg,0); 22 | 23 | ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; 24 | 25 | pts = apply_fisheye_distortion(ptsn,kg); 26 | pts(1,:) = fg(1)*pts(1,:) + cg(1); 27 | pts(2,:) = fg(2)*pts(2,:) + cg(2); 28 | 29 | % Check that the curve is within the image: 30 | good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... 31 | (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); 32 | 33 | if ~good_line, 34 | ns = -1; 35 | return; 36 | end; 37 | 38 | % Deviate the trajectory orthogonally: 39 | lambda = [y1 - y2 ; x2 - x1]; 40 | lambda = lambda / sqrt(sum(lambda.^2)); 41 | 42 | Np = size(pts,2); 43 | xs_mat = ones(2*win + 1,1)*pts(1,:); 44 | ys_mat = ones(2*win + 1,1)*pts(2,:); 45 | win_mat = (-win:win)'*ones(1,Np); 46 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 47 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 48 | ind_mat = (xs_mat2) * ny + ys_mat2 + 1; 49 | ima_patch = zeros(2*win + 1,Np); 50 | ima_patch(:) = I(ind_mat(:)); 51 | 52 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 53 | out_f = sum(filtk.*ima_patch); 54 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 55 | out_f_f = out_f_f(win+1:end-win); 56 | 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; 57 | -------------------------------------------------------------------------------- /mosaic.m: -------------------------------------------------------------------------------- 1 | 2 | if ~exist('I_1'), 3 | active_images_save = active_images; 4 | ima_read_calib; 5 | active_images = active_images_save; 6 | check_active_images; 7 | end; 8 | 9 | check_active_images; 10 | 11 | if isempty(ind_read), 12 | return; 13 | end; 14 | 15 | 16 | n_col = floor(sqrt(n_ima*nx/ny)); 17 | 18 | n_row = ceil(n_ima / n_col); 19 | 20 | 21 | ker2 = 1; 22 | for ii = 1:n_col, 23 | ker2 = conv(ker2,[1/4 1/2 1/4]); 24 | end; 25 | 26 | 27 | II = I_1(1:n_col:end,1:n_col:end); 28 | 29 | [ny2,nx2] = size(II); 30 | 31 | 32 | 33 | kk_c = 1; 34 | 35 | II_mosaic = []; 36 | 37 | for jj = 1:n_row, 38 | 39 | 40 | II_row = []; 41 | 42 | for ii = 1:n_col, 43 | 44 | if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima), 45 | 46 | if active_images(kk_c), 47 | eval(['I = I_' num2str(kk_c) ';']); 48 | %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing 49 | I = I(1:n_col:end,1:n_col:end); 50 | else 51 | I = zeros(ny2,nx2); 52 | end; 53 | 54 | else 55 | 56 | I = zeros(ny2,nx2); 57 | 58 | end; 59 | 60 | 61 | 62 | II_row = [II_row I]; 63 | 64 | if ii ~= n_col, 65 | 66 | II_row = [II_row zeros(ny2,3)]; 67 | 68 | end; 69 | 70 | 71 | kk_c = kk_c + 1; 72 | 73 | end; 74 | 75 | nn2 = size(II_row,2); 76 | 77 | if jj ~= n_row, 78 | II_row = [II_row; zeros(3,nn2)]; 79 | end; 80 | 81 | II_mosaic = [II_mosaic ; II_row]; 82 | 83 | end; 84 | 85 | figure(2); 86 | image(II_mosaic); 87 | colormap(gray(256)); 88 | title('Calibration images'); 89 | set(gca,'Xtick',[]) 90 | set(gca,'Ytick',[]) 91 | axis('image'); 92 | 93 | -------------------------------------------------------------------------------- /downsample.m: -------------------------------------------------------------------------------- 1 | function V = downsample(U) 2 | 3 | % Try the 5x5 filter for building the pyramid: 4 | % Now works with color images! 5 | 6 | U = double(U); 7 | 8 | [r,c,k] = size(U); 9 | 10 | p = floor((r+1)/2); % row 11 | q = floor((c+1)/2); % col 12 | 13 | 14 | U2 = zeros(r+2,c+2,k); 15 | 16 | for i=1:k 17 | 18 | U2(:,:,i) = [U(:,:,i) U(:,end,i)*ones(1,2); ones(2,1)*U(end,:,i) U(end,end,i)*ones(2,2)]; 19 | 20 | end; 21 | 22 | cp = 2*(0:(p-1))+1; % row 23 | cq = 2*(0:(q-1))+1; % col 24 | 25 | 26 | r2 = length(cp); 27 | c2 = length(cq); 28 | 29 | e = cq+1; 30 | ee = cq+2; 31 | w = cq-1; w(1) = 1; 32 | ww = cq-2; ww(1) = 1; ww(2) = 1; 33 | n = cp-1; n(1) = 1; 34 | nn = cp-2; nn(1) = 1; nn(2) = 1; 35 | s = cp+1; 36 | ss = cp+2; 37 | 38 | V = zeros(r2,c2,k); 39 | 40 | for i = 1:k, 41 | 42 | V(:,:,i) = (36*U2(cp,cq,i) + 24*(U2(n,cq,i) + U2(s,cq,i) + U2(cp,e,i) + U2(cp,w,i)) + ... 43 | 16 * (U2(n,e,i) + U2(s,e,i) + U2(n,w,i) + U2(s,w,i)) + ... 44 | 6 * (U2(nn,cq,i) + U2(ss,cq,i) + U2(cp,ee,i) + U2(cp,ww,i)) + ... 45 | 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)) + ... 46 | (U2(nn,ee,i) + U2(ss,ee,i) + U2(nn,ww,i) + U2(ss,ww,i)))/256; 47 | 48 | end; 49 | 50 | return 51 | 52 | % DOWNSAMPLE2 9-point subsampling (see Burt,Adelson IEEE Tcomm 31, 532) 53 | % V = downsample2(U) 54 | 55 | [r,c] = size(U); 56 | 57 | p = floor((r+1)/2); 58 | q = floor((c+1)/2); 59 | 60 | 61 | U2 = [U U(:,end); U(end,:) U(end,end)]; 62 | 63 | 64 | cq = 2*(0:(q-1))+1; 65 | cp = 2*(0:(p-1))+1; 66 | 67 | %cp = 2*([1:p]'-1)+1; 68 | %cq = 2*([1:q]-1)+1; 69 | 70 | e = cq+1; %e(q) = e(q)-1; 71 | w = cq-1; w(1) = w(1)+1; 72 | n = cp-1; n(1) = n(1)+1; 73 | s = cp+1; %s(p) = s(p)-1; 74 | 75 | % Interior 76 | V = 0.25 * U2(cp,cq) + ... 77 | 0.125*(U2(n,cq) + U2(s,cq) + U2(cp,e) + U2(cp,w)) + ... 78 | 0.0625*(U2(n,e) + U2(s,e) + U2(n,w) + U2(s,w)); 79 | -------------------------------------------------------------------------------- /small_test_script.m: -------------------------------------------------------------------------------- 1 | % This is small script that demonstrate the computation of extrinsic 2 | % parameters using 3D structures. 3 | % This test was build from data provided by Daniel Small (thank you Daniel!) 4 | 5 | 6 | %-- Image points (in pixels): 7 | 8 | x = [479.5200 236.0800 9 | 608.4100 415.3700 10 | 461.0000 40.0000 11 | 451.4800 308.7000 12 | 373.9900 314.8900 13 | 299.3200 319.1300 14 | 231.5500 321.3700 15 | 443.7300 282.9200 16 | 378.3600 288.3000 17 | 314.6900 292.7400 18 | 255.4700 296.2300]'; 19 | 20 | 21 | % 3D world coordinates: 22 | 23 | X = [ 0 0 0 24 | 54.0000 0 0 25 | 0 0 40.5000 26 | 27.0000 -8.4685 -2.3750 27 | 27.0000 -18.4685 -2.3750 28 | 27.0000 -28.4685 -2.3750 29 | 27.0000 -38.4685 -2.3750 30 | 17.0000 -8.4685 -2.3750 31 | 17.0000 -18.4685 -2.3750 32 | 17.0000 -28.4685 -2.3750 33 | 17.0000 -38.4685 -2.3750]'; 34 | 35 | 36 | %------------ Intrinsic parameters: 37 | %--- focal: 38 | fc = [ 395.0669 357.1178 ]'; 39 | %--- Principal point: 40 | cc = [ 380.5387 230.5278 ]'; 41 | %--- Distortion coefficients: 42 | kc = [-0.2601 0.0702 -0.0019 -0.0003 0]'; 43 | %--- Skew coefficient: 44 | alpha_c = 0; 45 | 46 | %----- Computation of the pose of the object in space 47 | %----- (or the rigid motion between world reference frame and camera ref. frame) 48 | [om,T,R] = compute_extrinsic(x,X,fc,cc,kc,alpha_c); 49 | 50 | %--- Try to reproject the structure to see if the computed pose makes sense: 51 | x2 = project_points2(X_1,omckk,Tckk,fc,cc,kc,alpha_c); 52 | 53 | 54 | % Graphical output: 55 | figure(2); 56 | plot(x(1,:),x(2,:),'r+'); 57 | hold on; 58 | plot(x2(1,:),x2(2,:),'go'); 59 | hold off; 60 | axis('equal'); 61 | axis('image'); 62 | title('red crosses: data, green circles: reprojected structure -- IT WORKS!!!'); 63 | xlabel('x'); 64 | ylabel('y'); 65 | 66 | -------------------------------------------------------------------------------- /rigid_motion.m: -------------------------------------------------------------------------------- 1 | function [Y,dYdom,dYdT] = rigid_motion(X,om,T); 2 | 3 | %rigid_motion.m 4 | % 5 | %[Y,dYdom,dYdT] = rigid_motion(X,om,T) 6 | % 7 | %Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). 8 | % 9 | %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) 10 | % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame 11 | % om: rotation vector (3x1 vector); T: translation vector (3x1 vector) 12 | % 13 | %OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) 14 | % dYdom: Derivative of Y with respect to om ((3N)x3 matrix) 15 | % dYdT: Derivative of Y with respect to T ((3N)x3 matrix) 16 | % 17 | %Definitions: 18 | %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) 19 | %The coordinate vector of P in the camera reference frame is: Y = R*X + T 20 | %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); 21 | % 22 | %Important function called within that program: 23 | % 24 | %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector 25 | 26 | 27 | 28 | if nargin < 3, 29 | T = zeros(3,1); 30 | if nargin < 2, 31 | om = zeros(3,1); 32 | if nargin < 1, 33 | error('Need at least a 3D structure as input (in rigid_motion.m)'); 34 | return; 35 | end; 36 | end; 37 | end; 38 | 39 | 40 | [R,dRdom] = rodrigues(om); 41 | 42 | [m,n] = size(X); 43 | 44 | Y = R*X + repmat(T,[1 n]); 45 | 46 | if nargout > 1, 47 | 48 | 49 | dYdR = zeros(3*n,9); 50 | dYdT = zeros(3*n,3); 51 | 52 | dYdR(1:3:end,1:3:end) = X'; 53 | dYdR(2:3:end,2:3:end) = X'; 54 | dYdR(3:3:end,3:3:end) = X'; 55 | 56 | dYdT(1:3:end,1) = ones(n,1); 57 | dYdT(2:3:end,2) = ones(n,1); 58 | dYdT(3:3:end,3) = ones(n,1); 59 | 60 | dYdom = dYdR * dRdom; 61 | 62 | end; 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /apply_distortion.m: -------------------------------------------------------------------------------- 1 | function [xd,dxddk] = apply_distortion(x,k) 2 | 3 | 4 | % Complete the distortion vector if you are using the simple distortion model: 5 | length_k = length(k); 6 | if length_k <5 , 7 | k = [k ; zeros(5-length_k,1)]; 8 | end; 9 | 10 | 11 | [m,n] = size(x); 12 | 13 | % Add distortion: 14 | 15 | r2 = x(1,:).^2 + x(2,:).^2; 16 | 17 | r4 = r2.^2; 18 | 19 | r6 = r2.^3; 20 | 21 | 22 | % Radial distortion: 23 | 24 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; 25 | 26 | if nargout > 1, 27 | dcdistdk = [ r2' r4' zeros(n,2) r6']; 28 | end; 29 | 30 | 31 | xd1 = x .* (ones(2,1)*cdist); 32 | 33 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); 34 | 35 | if nargout > 1, 36 | dxd1dk = zeros(2*n,5); 37 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; 38 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; 39 | end; 40 | 41 | 42 | % tangential distortion: 43 | 44 | a1 = 2.*x(1,:).*x(2,:); 45 | a2 = r2 + 2*x(1,:).^2; 46 | a3 = r2 + 2*x(2,:).^2; 47 | 48 | delta_x = [k(3)*a1 + k(4)*a2 ; 49 | k(3) * a3 + k(4)*a1]; 50 | 51 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); 52 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); 53 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); 54 | 55 | if nargout > 1, 56 | ddelta_xdk = zeros(2*n,5); 57 | ddelta_xdk(1:2:end,3) = a1'; 58 | ddelta_xdk(1:2:end,4) = a2'; 59 | ddelta_xdk(2:2:end,3) = a3'; 60 | ddelta_xdk(2:2:end,4) = a1'; 61 | end; 62 | 63 | xd = xd1 + delta_x; 64 | 65 | if nargout > 1, 66 | dxddk = dxd1dk + ddelta_xdk ; 67 | if length_k < 5, 68 | dxddk = dxddk(:,1:length_k); 69 | end; 70 | end; 71 | 72 | 73 | return; 74 | 75 | % Test of the Jacobians: 76 | 77 | n = 10; 78 | 79 | lk = 1; 80 | 81 | x = 10*randn(2,n); 82 | k = 0.5*randn(lk,1); 83 | 84 | [xd,dxddk] = apply_distortion(x,k); 85 | 86 | 87 | % Test on k: OK!! 88 | 89 | dk = 0.001 * norm(k)*randn(lk,1); 90 | k2 = k + dk; 91 | 92 | [x2] = apply_distortion(x,k2); 93 | 94 | x_pred = xd + reshape(dxddk * dk,2,n); 95 | 96 | 97 | norm(x2-xd)/norm(x2 - x_pred) 98 | -------------------------------------------------------------------------------- /merge_two_datasets.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Script that merges two "Cabib_Results.mat" data sets of the same camera into a single dataset\n') 2 | 3 | dir; 4 | 5 | 6 | cont = 1; 7 | while cont 8 | data_set1 = input('Filename of the first dataset (with complete path if necessary): ','s'); 9 | cont = ((exist(data_set1)~=2)); 10 | if cont, 11 | fprintf(1,'File not found. Try again.\n'); 12 | end; 13 | end; 14 | cont = 1; 15 | while cont 16 | data_set2 = input('Filename of the second dataset (with complete path if necessary): ','s'); 17 | cont = ((exist(data_set2)~=2)); 18 | if cont, 19 | fprintf(1,'File not found. Try again.\n'); 20 | end; 21 | end; 22 | 23 | 24 | load(data_set1); % part1\Calib_Results; 25 | 26 | shift = n_ima; 27 | 28 | load(data_set2); % part2\Calib_Results; 29 | 30 | active_images2 = active_images; 31 | n_ima2 = n_ima; 32 | 33 | 34 | for kk = 1:n_ima 35 | 36 | eval(['X_' num2str(kk+shift) ' = X_' num2str(kk) ';']); 37 | 38 | 39 | eval(['dX_' num2str(kk+shift) ' = dX_' num2str(kk) ';']); 40 | eval(['dY_' num2str(kk+shift) ' = dY_' num2str(kk) ';']); 41 | 42 | eval(['wintx_' num2str(kk+shift) ' = wintx_' num2str(kk) ';']); 43 | eval(['winty_' num2str(kk+shift) ' = winty_' num2str(kk) ';']); 44 | 45 | eval(['x_' num2str(kk+shift) ' = x_' num2str(kk) ';']); 46 | eval(['y_' num2str(kk+shift) ' = y_' num2str(kk) ';']); 47 | 48 | eval(['n_sq_x_' num2str(kk+shift) ' = n_sq_x_' num2str(kk) ';']); 49 | eval(['n_sq_y_' num2str(kk+shift) ' = n_sq_y_' num2str(kk) ';']); 50 | 51 | 52 | eval(['omc_' num2str(kk+shift) ' = omc_' num2str(kk) ';']); 53 | eval(['Tc_' num2str(kk+shift) ' = Tc_' num2str(kk) ';']); 54 | 55 | end; 56 | 57 | load(data_set1); % part1\Calib_Results; 58 | 59 | n_ima = n_ima + n_ima2; 60 | active_images = [active_images active_images2]; 61 | 62 | no_image = 1; 63 | 64 | % Recompute the error (in the vector ex): 65 | comp_error_calib; 66 | 67 | fprintf('The two calibration datasets are now merged. You are now ready to run calibration. \n'); 68 | 69 | -------------------------------------------------------------------------------- /recomp_corner_calib_saddle_points.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Recomputation of the image corners using Lucchese''s saddle point detector\n'); 2 | 3 | q = input('This detector only works for X junctions (checker board corners). Is this the case here? ([]=yes, other=no)','s'); 4 | 5 | 6 | if isempty(q), 7 | 8 | ima_proc = 1:n_ima; 9 | 10 | fprintf(1,'Processing image '); 11 | 12 | for kk = ima_proc; 13 | 14 | if active_images(kk), 15 | 16 | fprintf(1,'%d...',kk); 17 | 18 | 19 | eval(['I = I_' num2str(kk) ';']); 20 | 21 | eval(['y = x_' num2str(kk) ';']); 22 | eval(['wintx = wintx_' num2str(kk) ';']); 23 | eval(['winty = winty_' num2str(kk) ';']); 24 | 25 | xc = cornerfinder_saddle_point(y+1,I,winty,wintx); % the four corners 26 | 27 | eval(['x_' num2str(kk) '= xc - 1;']); 28 | 29 | else 30 | 31 | if ~exist(['omc_' num2str(kk)]), 32 | 33 | eval(['dX_' num2str(kk) ' = NaN;']); 34 | eval(['dY_' num2str(kk) ' = NaN;']); 35 | 36 | eval(['wintx_' num2str(kk) ' = NaN;']); 37 | eval(['winty_' num2str(kk) ' = NaN;']); 38 | 39 | eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); 40 | eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); 41 | 42 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 43 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 44 | 45 | end; 46 | 47 | end; 48 | 49 | end; 50 | 51 | % Recompute the error: 52 | 53 | comp_error_calib; 54 | 55 | fprintf(1,'\ndone\n'); 56 | 57 | else 58 | 59 | fprintf(1,'No recomputation done (The points are not locally saddle points)\n'); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /willson_convert.m: -------------------------------------------------------------------------------- 1 | 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); 2 | 3 | %Conversion from Reg Willson's calibration format to my format 4 | 5 | % Conversion: 6 | 7 | % Focal length: 8 | fc = [sx/dpx ; 1/dpy]*f; 9 | 10 | % Principal point; 11 | cc = [Cx;Cy]; 12 | 13 | % Skew: 14 | alpha_c = 0; 15 | 16 | % Extrinsic parameters: 17 | Rx = rodrigues([Rx;0;0]); 18 | Ry = rodrigues([0;Ry;0]); 19 | Rz = rodrigues([0;0;Rz]); 20 | 21 | Rc = Rz * Ry * Rx; 22 | 23 | omc = rodrigues(Rc); 24 | 25 | Tc = [Tx;Ty;Tz]; 26 | 27 | 28 | % More tricky: Take care of the distorsion: 29 | 30 | Nfy = round(Nfx * 3/4); 31 | 32 | nx = Nfx; 33 | ny = Nfy; 34 | 35 | % Select a set of DISTORTED coordinates uniformely distributed across the image: 36 | 37 | [xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy); 38 | 39 | xp_dist = xp_dist(:)'; 40 | yp_dist = yp_dist(:)'; 41 | 42 | 43 | % Apply UNDISTORTION according to Willson: 44 | 45 | xp_sensor_dist = dpx*(xp_dist - Cx)/sx; 46 | yp_sensor_dist = dpy*(yp_dist - Cy); 47 | 48 | dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2); 49 | 50 | xp_sensor = xp_sensor_dist .* dist_fact; 51 | yp_sensor = yp_sensor_dist .* dist_fact; 52 | 53 | xp = xp_sensor * sx / dpx + Cx; 54 | yp = yp_sensor / dpy + Cy; 55 | 56 | ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1)); 57 | 58 | xp = xp(ind); 59 | yp = yp(ind); 60 | xp_dist = xp_dist(ind); 61 | yp_dist = yp_dist(ind); 62 | 63 | 64 | % Now, find my own set of parameters: 65 | 66 | x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)]; 67 | x_dist(1,:) = x_dist(1,:) - alpha_c * x_dist(2,:); 68 | 69 | x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)]; 70 | x(1,:) = x(1,:) - alpha_c * x(2,:); 71 | 72 | k = [0;0;0;0;0]; 73 | 74 | for kk = 1:5, 75 | 76 | [xd,dxddk] = apply_distortion(x,k); 77 | 78 | err = x_dist - xd; 79 | 80 | %norm(err) 81 | 82 | k_step = inv(dxddk'*dxddk)*(dxddk')*err(:); 83 | 84 | k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:); 85 | 86 | %norm(k_step)/norm(k) 87 | 88 | if norm(k_step)/norm(k) < 10e-10, 89 | break; 90 | end; 91 | 92 | end; 93 | 94 | 95 | kc = k; 96 | -------------------------------------------------------------------------------- /loadpgm.m: -------------------------------------------------------------------------------- 1 | %LOADPGM Load a PGM image 2 | % 3 | % I = loadpgm(filename) 4 | % 5 | % Returns a matrix containing the image loaded from the PGM format 6 | % file filename. Handles ASCII (P2) and binary (P5) PGM file formats. 7 | % 8 | % If the filename has no extension, and open fails, a '.pgm' will 9 | % be appended. 10 | % 11 | % 12 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 13 | 14 | 15 | % Peter Corke 1994 16 | 17 | function I = loadpgm(file) 18 | white = [' ' 9 10 13]; % space, tab, lf, cr 19 | white = setstr(white); 20 | 21 | fid = fopen(file, 'r'); 22 | if fid < 0, 23 | fid = fopen([file '.pgm'], 'r'); 24 | end 25 | if fid < 0, 26 | error('Couldn''t open file'); 27 | end 28 | 29 | magic = fread(fid, 2, 'char'); 30 | while 1 31 | c = fread(fid,1,'char'); 32 | if c == '#', 33 | fgetl(fid); 34 | elseif ~any(c == white) 35 | fseek(fid, -1, 'cof'); % unputc() 36 | break; 37 | end 38 | end 39 | cols = fscanf(fid, '%d', 1); 40 | while 1 41 | c = fread(fid,1,'char'); 42 | if c == '#', 43 | fgetl(fid); 44 | elseif ~any(c == white) 45 | fseek(fid, -1, 'cof'); % unputc() 46 | break; 47 | end 48 | end 49 | rows = fscanf(fid, '%d', 1); 50 | while 1 51 | c = fread(fid,1,'char'); 52 | if c == '#', 53 | fgetl(fid); 54 | elseif ~any(c == white) 55 | fseek(fid, -1, 'cof'); % unputc() 56 | break; 57 | end 58 | end 59 | maxval = fscanf(fid, '%d', 1); 60 | while 1 61 | c = fread(fid,1,'char'); 62 | if c == '#', 63 | fgetl(fid); 64 | elseif ~any(c == white) 65 | fseek(fid, -1, 'cof'); % unputc() 66 | break; 67 | end 68 | end 69 | if magic(1) == 'P', 70 | if magic(2) == '2', 71 | %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)]) 72 | I = fscanf(fid, '%d', [cols rows])'; 73 | elseif magic(2) == '5', 74 | %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)]) 75 | if maxval == 1, 76 | fmt = 'unint1'; 77 | elseif maxval == 15, 78 | fmt = 'uint4'; 79 | elseif maxval == 255, 80 | fmt = 'uint8'; 81 | elseif maxval == 2^32-1, 82 | fmt = 'uint32'; 83 | end 84 | I = fread(fid, [cols rows], fmt)'; 85 | else 86 | disp('Not a PGM file'); 87 | end 88 | end 89 | fclose(fid); 90 | -------------------------------------------------------------------------------- /normalize2.m: -------------------------------------------------------------------------------- 1 | function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c), 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | 20 | k1 = kc(1); 21 | k2 = kc(2); 22 | k3 = kc(5); 23 | p1 = kc(3); 24 | p2 = kc(4); 25 | 26 | N = size(x_kk,2); 27 | 28 | % First: Subtract principal point, and divide by the focal length: 29 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 30 | 31 | 32 | v1 = - x_distort(1,:) / fc(1); 33 | v2 = - x_distort(2,:) / fc(1); 34 | 35 | dx_distortdfc = zeros(2*N,2); 36 | dx_distortdfc(1:2:end,1) = v1'; 37 | dx_distortdfc(2:2:end,2) = v2'; 38 | 39 | v1 = - x_distort(1,:) / fc(1); 40 | v2 = - x_distort(2,:) / fc(1); 41 | 42 | dx_distortdcc = zeros(2*N,2); 43 | dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1); 44 | dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1); 45 | 46 | % Second: undo skew 47 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 48 | 49 | dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2) dx_distortdfc(:,2)]; 50 | dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2) dx_distortdcc(:,2)]; 51 | 52 | dx_distort2dalpha_c = zeros(2*N,1); 53 | dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)'; 54 | 55 | x = x_distort; % initial guess 56 | 57 | for kk=1:20, 58 | 59 | r_2 = sum(x.^2); 60 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 61 | 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,:)]; 62 | x = (x_distort - delta_x)./(ones(2,1)*k_radial); 63 | 64 | end; 65 | 66 | 67 | xn = x; 68 | 69 | 70 | dxdk = zeros(2*N,5); % Approximation (no time) 71 | dxdf = dx_distort2dfc; 72 | dxdc = dx_distort2dcc; 73 | dxdalpha = dx_distort2dalpha_c; 74 | -------------------------------------------------------------------------------- /show_window.m: -------------------------------------------------------------------------------- 1 | function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) 2 | 3 | 4 | if ~exist('cell_list'), 5 | error('No description of the functions'); 6 | end; 7 | 8 | if ~exist('fig_number'), 9 | fig_number = 1; 10 | end; 11 | if ~exist('title_figure'), 12 | title_figure = ''; 13 | end; 14 | if ~exist('x_size'), 15 | x_size = 85; 16 | end; 17 | if ~exist('y_size'), 18 | y_size = 14; 19 | end; 20 | if ~exist('gap_x'), 21 | gap_x = 0; 22 | end; 23 | if ~exist('font_name'), 24 | font_name = 'clean'; 25 | end; 26 | if ~exist('font_size'), 27 | font_size = 8; 28 | end; 29 | 30 | figure(fig_number); clf; 31 | pos = get(fig_number,'Position'); 32 | 33 | [n_row,n_col] = size(cell_list); 34 | 35 | fig_size_x = x_size*n_col+(n_col+1)*gap_x; 36 | fig_size_y = y_size*n_row+(n_row+1)*gap_x; 37 | 38 | set(fig_number,'Units','points', ... 39 | % 'BackingStore','off', ... 40 | 'Color',[0.8 0.8 0.8], ... 41 | 'MenuBar','none', ... 42 | 'Resize','off', ... 43 | 'Name',title_figure, ... 44 | 'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... 45 | 'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); 46 | 47 | h_mat = zeros(n_row,n_col); 48 | 49 | posx = zeros(n_row,n_col); 50 | posy = zeros(n_row,n_col); 51 | 52 | for i=n_row:-1:1, 53 | for j = n_col:-1:1, 54 | posx(i,j) = gap_x+(j-1)*(x_size+gap_x); 55 | posy(i,j) = fig_size_y - i*(gap_x+y_size); 56 | end; 57 | end; 58 | 59 | for i=n_row:-1:1, 60 | for j = n_col:-1:1, 61 | if ~isempty(cell_list{i,j}), 62 | if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), 63 | h_mat(i,j) = uicontrol('Parent',fig_number, ... 64 | 'Units','points', ... 65 | 'Callback',cell_list{i,j}{2}, ... 66 | 'ListboxTop',0, ... 67 | 'Position',[posx(i,j) posy(i,j) x_size y_size], ... 68 | 'String',cell_list{i,j}{1}, ... 69 | 'fontsize',font_size,... 70 | 'fontname',font_name,... 71 | 'Tag','Pushbutton1'); 72 | end; 73 | end; 74 | end; 75 | end; 76 | 77 | %------ END PROTECTED REGION ----------------% 78 | -------------------------------------------------------------------------------- /willson_read.m: -------------------------------------------------------------------------------- 1 | % Read in Reg Willson's data file, and convert it into my data format: 2 | 3 | if exist('calib_file'), 4 | if exist(calib_file)~=2, 5 | ask = 1; 6 | else 7 | ask = 0; 8 | end; 9 | else 10 | ask = 1; 11 | end; 12 | 13 | 14 | while ask, 15 | 16 | calib_file = input('Reg Willson calibration file name to convert: ','s'); 17 | 18 | if ~isempty(calib_file), 19 | if exist(calib_file)~=2, 20 | ask = 1; 21 | else 22 | ask = 0; 23 | end; 24 | else 25 | ask = 1; 26 | end; 27 | 28 | if ask, 29 | fprintf(1,'File not found. Try again.\n'); 30 | end; 31 | 32 | end; 33 | 34 | if exist(calib_file), 35 | 36 | fprintf(1,['Loading Willson calibration parameters from file ' calib_file '...\n']); 37 | 38 | load(calib_file); 39 | 40 | inddot = find(calib_file == '.'); 41 | 42 | if isempty(inddot), 43 | varname = calib_file; 44 | else 45 | varname = calib_file(1:inddot(1)-1); 46 | end; 47 | 48 | eval(['calib_params = ' varname ';']) 49 | 50 | Ncx = calib_params(1); 51 | Nfx = calib_params(2); 52 | dx = calib_params(3); 53 | dy = calib_params(4); 54 | dpx = calib_params(5); 55 | dpy = calib_params(6); 56 | Cx = calib_params(7); 57 | Cy = calib_params(8); 58 | sx = calib_params(9); 59 | f = calib_params(10); 60 | kappa1 = calib_params(11); 61 | Tx = calib_params(12); 62 | Ty = calib_params(13); 63 | Tz = calib_params(14); 64 | Rx = calib_params(15); 65 | Ry = calib_params(16); 66 | Rz = calib_params(17); 67 | p1 = calib_params(18); 68 | p2 = calib_params(19); 69 | 70 | fprintf(1,'Converting the calibration parameters... (may take a while due to the convertion of the distortion model)...\n'); 71 | 72 | % Conversion: 73 | [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); 74 | 75 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; 76 | 77 | n_ima = 1; 78 | 79 | X_1 = [NaN;NaN;NaN]; 80 | x_1 = [NaN;NaN]; 81 | 82 | map = gray(256); 83 | 84 | fprintf(1,'done\n'); 85 | fprintf(1,'You are now ready to save the calibration parameters by clicking on Save.\n'); 86 | 87 | else 88 | 89 | disp(['WARNING: Calibration file ' calib_file ' not found']); 90 | 91 | end; 92 | 93 | -------------------------------------------------------------------------------- /comp_ext_calib.m: -------------------------------------------------------------------------------- 1 | %%% Computes the extrinsic parameters for all the active calibration images 2 | 3 | check_active_images; 4 | 5 | N_points_views = zeros(1,n_ima); 6 | 7 | for kk = 1:n_ima, 8 | 9 | if exist(['x_' num2str(kk)]), 10 | 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | eval(['X_kk = X_' num2str(kk) ';']); 13 | 14 | if (isnan(x_kk(1,1))), 15 | if active_images(kk), 16 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 17 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 18 | end; 19 | end; 20 | if active_images(kk), 21 | N_points_views(kk) = size(x_kk,2); 22 | [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); 23 | [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); 24 | if check_cond, 25 | if (cond(JJ_kk)> thresh_cond), 26 | active_images(kk) = 0; 27 | omckk = NaN*ones(3,1); 28 | Tckk = NaN*ones(3,1); 29 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) 30 | desactivated_images = [desactivated_images kk]; 31 | end; 32 | end; 33 | if isnan(omckk(1,1)), 34 | %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) 35 | active_images(kk) = 0; 36 | end; 37 | else 38 | omckk = NaN*ones(3,1); 39 | Tckk = NaN*ones(3,1); 40 | end; 41 | 42 | else 43 | 44 | omckk = NaN*ones(3,1); 45 | Tckk = NaN*ones(3,1); 46 | 47 | if active_images(kk), 48 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 49 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 50 | end; 51 | 52 | active_images(kk) = 0; 53 | 54 | end; 55 | 56 | eval(['omc_' num2str(kk) ' = omckk;']); 57 | eval(['Tc_' num2str(kk) ' = Tckk;']); 58 | 59 | end; 60 | 61 | 62 | check_active_images; 63 | -------------------------------------------------------------------------------- /comp_ext_calib_fisheye.m: -------------------------------------------------------------------------------- 1 | %%% Computes the extrinsic parameters for all the active calibration images 2 | 3 | check_active_images; 4 | 5 | N_points_views = zeros(1,n_ima); 6 | 7 | for kk = 1:n_ima, 8 | 9 | if exist(['x_' num2str(kk)]), 10 | 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | eval(['X_kk = X_' num2str(kk) ';']); 13 | 14 | if (isnan(x_kk(1,1))), 15 | if active_images(kk), 16 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 17 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 18 | end; 19 | end; 20 | if active_images(kk), 21 | N_points_views(kk) = size(x_kk,2); 22 | [omckk,Tckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c); 23 | [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); 24 | if check_cond, 25 | if (cond(JJ_kk)> thresh_cond), 26 | active_images(kk) = 0; 27 | omckk = NaN*ones(3,1); 28 | Tckk = NaN*ones(3,1); 29 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) 30 | desactivated_images = [desactivated_images kk]; 31 | end; 32 | end; 33 | if isnan(omckk(1,1)), 34 | %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) 35 | active_images(kk) = 0; 36 | end; 37 | else 38 | omckk = NaN*ones(3,1); 39 | Tckk = NaN*ones(3,1); 40 | end; 41 | 42 | else 43 | 44 | omckk = NaN*ones(3,1); 45 | Tckk = NaN*ones(3,1); 46 | 47 | if active_images(kk), 48 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 49 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 50 | end; 51 | 52 | active_images(kk) = 0; 53 | 54 | end; 55 | 56 | eval(['omc_' num2str(kk) ' = omckk;']); 57 | eval(['Tc_' num2str(kk) ' = Tckk;']); 58 | 59 | end; 60 | 61 | 62 | check_active_images; 63 | -------------------------------------------------------------------------------- /smooth_images.m: -------------------------------------------------------------------------------- 1 | % Anisotropically diffuse the calibration image 2 | % to enhance the corner detection 3 | 4 | 5 | fprintf(1,'Anisotropic diffusion of the images for corner enhancement (the images have to be loaded in memory)\n'); 6 | 7 | 8 | ker = [1/4 1/2 1/4]; 9 | ker2 = conv2(ker,ker); 10 | ker2 = conv2(ker2,ker); 11 | ker2 = conv2(ker2,ker); 12 | 13 | 14 | 15 | if ~exist(['I_' num2str(ind_active(1))]), 16 | ima_read_calib; 17 | end; 18 | 19 | check_active_images; 20 | 21 | format_image2 = format_image; 22 | if format_image2(1) == 'j', 23 | format_image2 = 'bmp'; 24 | end; 25 | 26 | for kk = 1:n_ima, 27 | 28 | if exist(['I_' num2str(kk)]), 29 | 30 | %fprintf(1,'%d...',kk); 31 | 32 | eval(['I = I_' num2str(kk) ';']); 33 | 34 | 35 | % Compute the sigI automatically: 36 | [nn,xx] = hist(I(:),50); 37 | nn = conv2(nn,ker2,'same'); 38 | 39 | max_nn = max(nn); 40 | 41 | 42 | localmax = [0 (nn(2:end-1)>=nn(3:end)) & (nn(2:end-1) > nn(1:end-2)) 0] .* (nn >= max_nn/5); 43 | 44 | %plot(xx,nn); 45 | %hold on; 46 | %plot(xx,nn .* localmax,'r' ); 47 | %hold off; 48 | 49 | localmax_ind = find(localmax); 50 | nn_local_max = nn(localmax_ind); 51 | 52 | % order the picks: 53 | [a,b] = sort(-nn_local_max); 54 | 55 | localmax_ind = localmax_ind(b); 56 | nn_local_max = nn_local_max(b); 57 | 58 | sig_I = abs(xx(localmax_ind(1)) - xx(localmax_ind(2)))/4.25; 59 | 60 | 61 | 62 | 63 | I2 = anisdiff(I,sig_I,30); 64 | 65 | 66 | if ~type_numbering, 67 | number_ext = num2str(image_numbers(kk)); 68 | else 69 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 70 | end; 71 | 72 | ima_name2 = [calib_name '_smth' number_ext '.' format_image2]; 73 | 74 | fprintf(1,['Saving smoothed image under ' ima_name2 '...\n']); 75 | 76 | if format_image2(1) == 'p', 77 | if format_images2(2) == 'p', 78 | saveppm(ima_name2,uint8(round(I2))); 79 | else 80 | savepgm(ima_name2,uint8(round(I2))); 81 | end; 82 | else 83 | if format_image2(1) == 'r', 84 | writeras(ima_name2,round(I2),gray(256)); 85 | else 86 | imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); 87 | end; 88 | end; 89 | 90 | end; 91 | 92 | end; 93 | 94 | fprintf(1,'\ndone\n'); -------------------------------------------------------------------------------- /Compute3D.m: -------------------------------------------------------------------------------- 1 | function [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 2 | 3 | % [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 4 | % 5 | % Reconstruction of the 3D structure of the striped object. 6 | % 7 | % Xc : The 3D coordinates of the points in the camera reference frame 8 | % Xp : The 3D coordinates of the points in the projector reference frame 9 | % 10 | % xc, xp: Camera coordinates and projector coordinates from ComputeStripes 11 | % R,T : rigid motion from camera to projector: Xp = R*Xc + T 12 | % fc,fp : Camera and Projector focal lengths 13 | % cc,cp : Camera and Projector center of projection 14 | % kc,kp : Camera and Projector distortion factors 15 | % alpha_c, alpha_p: skew coefficients for camera and projector 16 | % 17 | % The set R,T,fc,fp,cc,cp and kc comes from the calibration. 18 | 19 | % Intel Corporation - Dec. 2003 20 | % (c) Jean-Yves Bouguet 21 | 22 | 23 | if nargin < 12, 24 | alpha_p = 0; 25 | if nargin < 11, 26 | alpha_c = 0; 27 | end; 28 | end; 29 | 30 | 31 | Np = size(xc,2); 32 | 33 | 34 | xc = normalize_pixel(xc,fc,cc,kc,alpha_c); 35 | 36 | xp = (xp - cp(1))/fp(1); 37 | 38 | xp_save = xp; % save the real distorted x - coordinates + alpha'ed 39 | 40 | 41 | if (norm(kp) == 0)&(alpha_p == 0), 42 | N_rep = 1; 43 | else 44 | N_rep = 5; 45 | end; 46 | 47 | 48 | % xp is the first entry of the undistorted projector coordinates (iteratively refined) 49 | % xc is the complete undistorted camera coordinates 50 | for kk = 1:N_rep, 51 | 52 | R2 = R([1 3],:); 53 | if length(T) > 2, 54 | Tp = T([1 3]); % The old technique for calibration 55 | else 56 | Tp = T; % The new technique for calibration (using stripes only) 57 | end; 58 | 59 | % Triangulation: 60 | 61 | D1 = [-xc(1,:);xc(1,:).*xp(1,:);-xc(2,:);xc(2,:).*xp(1,:);-ones(1,Np);xp(1,:)]; 62 | D2 = R2(:)*ones(1,Np); 63 | 64 | D = sum(D1.*D2); 65 | N1 = [-ones(1,Np);xp(1,:)]; 66 | N2 = -sum(N1.*(Tp*ones(1,Np))); 67 | Z = N2./D; 68 | Xc = (ones(3,1)*Z).*[xc;ones(1,Np)]; 69 | 70 | % reproject on the projetor view, and apply distortion... 71 | 72 | Xp = R*Xc + T*ones(1,Np); 73 | 74 | xp_v = [Xp(1,:)./Xp(3,:); Xp(2,:)./Xp(3,:)]; 75 | 76 | xp_v(1,:) = xp_v(1,:) + alpha_p * xp_v(2,:); 77 | 78 | xp_dist = apply_distortion(xp_v,kp); 79 | 80 | %norm(xp_dist(1,:) - xp_save) 81 | 82 | xp_dist(1,:) = xp_save; 83 | 84 | xp_v = comp_distortion(xp_dist,kp); 85 | 86 | xp_v(1,:) = xp_v(1,:) - alpha_p * xp_v(2,:); 87 | 88 | xp = xp_v(1,:); 89 | 90 | end; 91 | -------------------------------------------------------------------------------- /apply_distortion2.m: -------------------------------------------------------------------------------- 1 | function [xd,dxddk,dxddx] = apply_distortion2(x,k) 2 | 3 | 4 | [m,n] = size(x); 5 | 6 | % Add distortion: 7 | 8 | r2 = x(1,:).^2 + x(2,:).^2; 9 | 10 | r4 = r2.^2; 11 | 12 | r6 = r2.^3; 13 | 14 | 15 | % Radial distortion: 16 | 17 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; 18 | 19 | if nargout > 1, 20 | dcdistdk = [ r2' r4' zeros(n,2) r6']; 21 | end; 22 | 23 | 24 | xd1 = x .* (ones(2,1)*cdist); 25 | 26 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); 27 | 28 | if nargout > 1, 29 | dxd1dk = zeros(2*n,5); 30 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; 31 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; 32 | end; 33 | 34 | 35 | % tangential distortion: 36 | 37 | a1 = 2.*x(1,:).*x(2,:); 38 | a2 = r2 + 2*x(1,:).^2; 39 | a3 = r2 + 2*x(2,:).^2; 40 | 41 | delta_x = [k(3)*a1 + k(4)*a2 ; 42 | k(3) * a3 + k(4)*a1]; 43 | 44 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); 45 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); 46 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); 47 | 48 | if nargout > 1, 49 | ddelta_xdk = zeros(2*n,5); 50 | ddelta_xdk(1:2:end,3) = a1'; 51 | ddelta_xdk(1:2:end,4) = a2'; 52 | ddelta_xdk(2:2:end,3) = a3'; 53 | ddelta_xdk(2:2:end,4) = a1'; 54 | end; 55 | 56 | xd = xd1 + delta_x; 57 | 58 | if nargout > 1, 59 | dxddk = dxd1dk + ddelta_xdk ; 60 | end; 61 | 62 | if nargout > 2, 63 | 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,:)'; 64 | 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,:)'; 65 | d3 = (k(1) + 2*k(2).*r2' + 3*k(5).*r4').*a1' + 2*k(3).*x(1,:)' + 2*k(4).*x(2,:)'; 66 | 67 | i = [1:2:2*n 1:2:2*n 2:2:2*n 2:2:2*n]; 68 | j = [1:2:2*n 2:2:2*n 1:2:2*n 2:2:2*n]; 69 | s = [d1' d3' d3' d2']; 70 | 71 | dxddx = sparse(i, j, s, 2*n, 2*n); 72 | end 73 | 74 | return; 75 | 76 | % Test of the Jacobians: 77 | 78 | n = 10; 79 | 80 | x = 10*randn(2,n); 81 | k = 0.5*randn(5,1); 82 | 83 | [xd,dxddk,dxddx] = apply_distortion2(x,k); 84 | 85 | 86 | % Test on k: OK!! 87 | 88 | dk = 0.001 * norm(k)*randn(5,1); 89 | k2 = k + dk; 90 | 91 | [x2] = apply_distortion2(x,k2); 92 | 93 | x_pred = xd + reshape(dxddk * dk,2,n); 94 | 95 | 96 | norm(x2-xd)/norm(x2 - x_pred) 97 | 98 | % Test on x: 99 | dx = 0.000001 *randn(2,n); 100 | x2 = x + dx; 101 | 102 | xd2 = apply_distortion2(x2,k); 103 | 104 | x_pred = xd + reshape(dxddx*dx(:),2,n); 105 | 106 | norm(xd2-xd)/norm(xd2-x_pred) -------------------------------------------------------------------------------- /error_cam_proj.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | 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 4 | 5 | % Computation of the errors: 6 | 7 | fc = param(1:2); 8 | cc = param(3:4); 9 | alpha_c = param(5); 10 | kc = param(6:10); 11 | 12 | e_cam = []; 13 | 14 | for kk = 1:n_ima, 15 | omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); 16 | Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); 17 | 18 | eval(['Xkk = X_' num2str(kk) ';']); 19 | eval(['xkk = x_' num2str(kk) ';']); 20 | 21 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 22 | 23 | Rckk = rodrigues(omckk); 24 | eval(['omc_' num2str(kk) '= omckk;']); 25 | eval(['Tc_' num2str(kk) '= Tckk;']); 26 | eval(['Rc_' num2str(kk) '= Rckk;']); 27 | 28 | e_cam = [e_cam ekk]; 29 | 30 | end; 31 | 32 | X_proj = []; 33 | x_proj = []; 34 | 35 | for kk = 1:n_ima, 36 | eval(['xproj = xproj_' num2str(kk) ';']); 37 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 38 | eval(['Rc = Rc_' num2str(kk) ';']); 39 | eval(['Tc = Tc_' num2str(kk) ';']); 40 | Np_proj = size(xproj,2); 41 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 42 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 43 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 44 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 45 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 46 | end; 47 | 48 | fp = param((1:2)+n_ima * 6 + 10); 49 | cp = param((3:4)+n_ima * 6 + 10); 50 | alpha_p = param((5)+n_ima * 6 + 10); 51 | kp = param((6:10)+n_ima * 6 + 10); 52 | 53 | om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); 54 | T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); 55 | 56 | 57 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 58 | 59 | 60 | e_global = [e_cam e_proj]; 61 | 62 | -------------------------------------------------------------------------------- /error_cam_proj3.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | 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 4 | 5 | % This is the same model, but with a simpler distortion model (no 6th order) 6 | 7 | % Computation of the errors: 8 | 9 | fc = param(1:2); 10 | cc = param(3:4); 11 | alpha_c = 0; 12 | kc = [param(5);zeros(4,1)]; 13 | 14 | e_cam = []; 15 | 16 | for kk = 1:n_ima, 17 | omckk = param(kk*6:kk*6+2); 18 | Tckk = param(kk*6+3:kk*6+5); 19 | 20 | eval(['Xkk = X_' num2str(kk) ';']); 21 | eval(['xkk = x_' num2str(kk) ';']); 22 | 23 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 24 | 25 | Rckk = rodrigues(omckk); 26 | eval(['omc_' num2str(kk) '= omckk;']); 27 | eval(['Tc_' num2str(kk) '= Tckk;']); 28 | eval(['Rc_' num2str(kk) '= Rckk;']); 29 | 30 | e_cam = [e_cam ekk]; 31 | 32 | end; 33 | 34 | X_proj = []; 35 | x_proj = []; 36 | 37 | for kk = 1:n_ima, 38 | eval(['xproj = xproj_' num2str(kk) ';']); 39 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 40 | eval(['Rc = Rc_' num2str(kk) ';']); 41 | eval(['Tc = Tc_' num2str(kk) ';']); 42 | Np_proj = size(xproj,2); 43 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 44 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 45 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 46 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 47 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 48 | end; 49 | 50 | fp = param((1:2)+n_ima * 6 + 5); 51 | cp = param((3:4)+n_ima * 6 + 5); 52 | alpha_p = 0; 53 | kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; 54 | 55 | om = param((6:8)+n_ima * 6 + 5); 56 | T = param((9:11)+n_ima * 6 + 5); 57 | 58 | 59 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 60 | 61 | 62 | e_global = [e_cam e_proj]; 63 | 64 | -------------------------------------------------------------------------------- /merge_calibration_sets.m: -------------------------------------------------------------------------------- 1 | 2 | set1 = load(data_set1); % part1\Calib_Results; 3 | set2 = load(data_set2); % part2\Calib_Results; 4 | 5 | shift = set1.n_ima; 6 | 7 | for kk = 1:set1.n_ima 8 | 9 | eval(['X_' num2str(kk) ' = set1.X_' num2str(kk) ';']); 10 | 11 | eval(['dX_' num2str(kk) ' = set1.dX_' num2str(kk) ';']); 12 | eval(['dY_' num2str(kk) ' = set1.dY_' num2str(kk) ';']); 13 | 14 | eval(['wintx_' num2str(kk) ' = set1.wintx_' num2str(kk) ';']); 15 | eval(['winty_' num2str(kk) ' = set1.winty_' num2str(kk) ';']); 16 | 17 | eval(['x_' num2str(kk) ' = set1.x_' num2str(kk) ';']); 18 | 19 | if isfield(set1,'y') 20 | eval(['y_' num2str(kk) ' = set1.y_' num2str(kk) ';']); 21 | else 22 | eval(['y_' num2str(kk) ' = [NaN];']); 23 | end; 24 | 25 | eval(['n_sq_x_' num2str(kk) ' = set1.n_sq_x_' num2str(kk) ';']); 26 | eval(['n_sq_y_' num2str(kk) ' = set1.n_sq_y_' num2str(kk) ';']); 27 | 28 | 29 | if isfield(set1,['omc_' num2str(kk+shift)]) 30 | eval(['omc_' num2str(kk+shift) ' = set1.omc_' num2str(kk) ';']); 31 | eval(['Tc_' num2str(kk+shift) ' = set1.Tc_' num2str(kk) ';']); 32 | else 33 | eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 34 | eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 35 | end; 36 | 37 | end; 38 | 39 | for kk = 1:set2.n_ima 40 | 41 | eval(['X_' num2str(kk+shift) ' = set2.X_' num2str(kk) ';']); 42 | 43 | 44 | eval(['dX_' num2str(kk+shift) ' = set2.dX_' num2str(kk) ';']); 45 | eval(['dY_' num2str(kk+shift) ' = set2.dY_' num2str(kk) ';']); 46 | 47 | eval(['wintx_' num2str(kk+shift) ' = set2.wintx_' num2str(kk) ';']); 48 | eval(['winty_' num2str(kk+shift) ' = set2.winty_' num2str(kk) ';']); 49 | 50 | eval(['x_' num2str(kk+shift) ' = set2.x_' num2str(kk) ';']); 51 | 52 | if isfield(set2,'y') 53 | eval(['y_' num2str(kk) ' = set2.y_' num2str(kk) ';']); 54 | else 55 | eval(['y_' num2str(kk) ' = [NaN];']); 56 | end; 57 | 58 | eval(['n_sq_x_' num2str(kk+shift) ' = set2.n_sq_x_' num2str(kk) ';']); 59 | eval(['n_sq_y_' num2str(kk+shift) ' = set2.n_sq_y_' num2str(kk) ';']); 60 | 61 | 62 | 63 | if isfield(set2,['omc_' num2str(kk+shift)]) 64 | eval(['omc_' num2str(kk+shift) ' = set2.omc_' num2str(kk) ';']); 65 | eval(['Tc_' num2str(kk+shift) ' = set2.Tc_' num2str(kk) ';']); 66 | else 67 | eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 68 | eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 69 | end; 70 | 71 | end; 72 | 73 | 74 | fc = set2.fc; 75 | kc = set2.kc; 76 | cc = set2.cc; 77 | alpha_c = set2.alpha_c; 78 | KK = set2.KK; 79 | inv_KK = set2.inv_KK; 80 | 81 | 82 | n_ima = set1.n_ima + set2.n_ima; 83 | active_images = [set1.active_images set2.active_images]; 84 | 85 | no_image = 1; 86 | -------------------------------------------------------------------------------- /stereo_triangulation.m: -------------------------------------------------------------------------------- 1 | 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), 2 | 3 | % [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), 4 | % 5 | % Function that computes the position of a set on N points given the left and right image projections. 6 | % The cameras are assumed to be calibrated, intrinsically, and extrinsically. 7 | % 8 | % Input: 9 | % xL: 2xN matrix of pixel coordinates in the left image 10 | % xR: 2xN matrix of pixel coordinates in the right image 11 | % om,T: rotation vector and translation vector between right and left cameras (output of stereo calibration) 12 | % fc_left,cc_left,...: intrinsic parameters of the left camera (output of stereo calibration) 13 | % fc_right,cc_right,...: intrinsic parameters of the right camera (output of stereo calibration) 14 | % 15 | % Output: 16 | % 17 | % XL: 3xN matrix of coordinates of the points in the left camera reference frame 18 | % XR: 3xN matrix of coordinates of the points in the right camera reference frame 19 | % 20 | % Note: XR and XL are related to each other through the rigid motion equation: XR = R * XL + T, where R = rodrigues(om) 21 | % For more information, visit http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html 22 | % 23 | % 24 | % (c) Jean-Yves Bouguet - Intel Corporation - April 9th, 2003 25 | 26 | 27 | 28 | %--- Normalize the image projection according to the intrinsic parameters of the left and right cameras 29 | xt = normalize_pixel(xL,fc_left,cc_left,kc_left,alpha_c_left); 30 | xtt = normalize_pixel(xR,fc_right,cc_right,kc_right,alpha_c_right); 31 | 32 | %--- Extend the normalized projections in homogeneous coordinates 33 | xt = [xt;ones(1,size(xt,2))]; 34 | xtt = [xtt;ones(1,size(xtt,2))]; 35 | 36 | %--- Number of points: 37 | N = size(xt,2); 38 | 39 | %--- Rotation matrix corresponding to the rigid motion between left and right cameras: 40 | R = rodrigues(om); 41 | 42 | 43 | %--- Triangulation of the rays in 3D space: 44 | 45 | u = R * xt; 46 | 47 | n_xt2 = dot(xt,xt); 48 | n_xtt2 = dot(xtt,xtt); 49 | 50 | T_vect = repmat(T, [1 N]); 51 | 52 | DD = n_xt2 .* n_xtt2 - dot(u,xtt).^2; 53 | 54 | dot_uT = dot(u,T_vect); 55 | dot_xttT = dot(xtt,T_vect); 56 | dot_xttu = dot(u,xtt); 57 | 58 | NN1 = dot_xttu.*dot_xttT - n_xtt2 .* dot_uT; 59 | NN2 = n_xt2.*dot_xttT - dot_uT.*dot_xttu; 60 | 61 | Zt = NN1./DD; 62 | Ztt = NN2./DD; 63 | 64 | X1 = xt .* repmat(Zt,[3 1]); 65 | X2 = R'*(xtt.*repmat(Ztt,[3,1]) - T_vect); 66 | 67 | 68 | %--- Left coordinates: 69 | XL = 1/2 * (X1 + X2); 70 | 71 | %--- Right coordinates: 72 | XR = R*XL + T_vect; 73 | 74 | -------------------------------------------------------------------------------- /error_cam_proj2.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | 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 4 | 5 | % This is the same model, but with a simpler distortion model (no 6th order) 6 | 7 | % Computation of the errors: 8 | 9 | fc = param(1:2); 10 | cc = param(3:4); 11 | alpha_c = param(5); 12 | kc = [param(6:9);0]; 13 | 14 | e_cam = []; 15 | 16 | for kk = 1:n_ima, 17 | omckk = param(11+(kk-1)*6-1:11+(kk-1)*6+2-1); 18 | Tckk = param(11+(kk-1)*6+3-1:11+(kk-1)*6+3+2-1); 19 | 20 | eval(['Xkk = X_' num2str(kk) ';']); 21 | eval(['xkk = x_' num2str(kk) ';']); 22 | 23 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 24 | 25 | Rckk = rodrigues(omckk); 26 | eval(['omc_' num2str(kk) '= omckk;']); 27 | eval(['Tc_' num2str(kk) '= Tckk;']); 28 | eval(['Rc_' num2str(kk) '= Rckk;']); 29 | 30 | e_cam = [e_cam ekk]; 31 | 32 | end; 33 | 34 | X_proj = []; 35 | x_proj = []; 36 | 37 | for kk = 1:n_ima, 38 | eval(['xproj = xproj_' num2str(kk) ';']); 39 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 40 | eval(['Rc = Rc_' num2str(kk) ';']); 41 | eval(['Tc = Tc_' num2str(kk) ';']); 42 | Np_proj = size(xproj,2); 43 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 44 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 45 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 46 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 47 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 48 | end; 49 | 50 | fp = param((1:2)+n_ima * 6 + 10-1); 51 | cp = param((3:4)+n_ima * 6 + 10-1); 52 | alpha_p = param((5)+n_ima * 6 + 10-1); 53 | kp = [param((6-1:10-2)+n_ima * 6 + 10);0]; 54 | 55 | om = param(10+n_ima*6+10+1-2:10+n_ima*6+10+1+2-2); 56 | T = param(10+n_ima*6+10+1+2+1-2:10+n_ima*6+10+1+2+1+2-2); 57 | 58 | 59 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 60 | 61 | 62 | e_global = [e_cam e_proj]; 63 | 64 | -------------------------------------------------------------------------------- /go_calib_optim.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib; % Load the images 35 | click_calib; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | -------------------------------------------------------------------------------- /go_calib_optim_no_read.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib_no_read; % Load the images 35 | click_calib_no_read; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | 61 | -------------------------------------------------------------------------------- /go_calib_optim_fisheye_no_read.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib_no_read; % Load the images 35 | click_calib_fisheye_no_read; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter_fisheye; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter_fisheye; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | -------------------------------------------------------------------------------- /init_intrinsic_param_fisheye.m: -------------------------------------------------------------------------------- 1 | %init_intrinsic_param_fisheye 2 | % 3 | %Initialization of the intrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % kc: Fisheye distortion coefficients 12 | % alpha_c: skew coefficient 13 | % KK: The camera matrix (containing fc, cc and alpha_c) 14 | % 15 | %Method: Computes the planar homographies H_1, H_2, H_3, ... and computes 16 | % the focal length fc from orthogonal vanishing points constraint. 17 | % The principal point cc is assumed at the center of the image. 18 | % Assumes no image distortion (kc = [0;0;0;0]) 19 | % 20 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 21 | % corresponding entry in the active_images vector to zero. 22 | % 23 | % 24 | %Important function called within that program: 25 | % 26 | %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. 27 | % 28 | % 29 | %VERY IMPORTANT: This function works only with 2D rigs. 30 | %In the future, a more general function will be there (working with 3D rigs as well). 31 | 32 | 33 | if ~exist('two_focals_init'), 34 | two_focals_init = 0; 35 | end; 36 | 37 | if ~exist('est_aspect_ratio'), 38 | est_aspect_ratio = 1; 39 | end; 40 | 41 | check_active_images; 42 | 43 | if ~exist(['x_' num2str(ind_active(1)) ]), 44 | click_calib; 45 | end; 46 | 47 | 48 | fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); 49 | 50 | check_active_images; 51 | 52 | % initial guess for principal point and distortion: 53 | 54 | if ~exist('nx'), [ny,nx] = size(I); end; 55 | 56 | f_init = (max(nx,ny) / pi) * ones(2,1); 57 | c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image 58 | k_init = [0;0;0;0]; % initialize to zero (no distortion) 59 | 60 | if ~est_aspect_ratio, 61 | f_init(1) = (f_init(1)+f_init(2))/2; 62 | f_init(2) = f_init(1); 63 | end; 64 | 65 | alpha_init = 0; 66 | 67 | % Global calibration matrix (initial guess): 68 | 69 | KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; 70 | inv_KK = inv(KK); 71 | 72 | cc = c_init; 73 | fc = f_init; 74 | kc = k_init; 75 | alpha_c = alpha_init; 76 | 77 | 78 | fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); 79 | fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); 80 | fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); 81 | fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); 82 | fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); 83 | -------------------------------------------------------------------------------- /scanning_script.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %-- Main 3D Scanning Script 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | 6 | if exist('calib_cam_proj_optim.mat')~=2, 7 | 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)'); 8 | end; 9 | 10 | % Loading the scanner calibration parameters: 11 | fprintf(1,'Loading the scanner calibration data...\n'); 12 | load calib_cam_proj_optim; 13 | 14 | 15 | % Choose a dataset (scan #20 for example) 16 | ind_view = 20; 17 | stripe_image = ['strip' sprintf('%.4d',ind_view) ]; 18 | 19 | if exist([stripe_image '_pat00p.bmp'])~=2, 20 | error('The scanning images cannot be found. Make sure to go to the directory where the scanning images are located (folder scanning_example)'); 21 | end; 22 | 23 | 24 | % Compute the projector coordinates at every pixel in the camera image: 25 | fprintf(1,'Computing the subpixel projector coordinates at every pixel in the camera image...\n'); 26 | [xc,xp,nx,ny] = ComputeStripes(stripe_image,1); 27 | 28 | 29 | % Triangulate the 3D geometry: 30 | fprintf(1,'Triangulating the 3D geometry...\n'); 31 | [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 32 | 33 | 34 | % Meshing the points: 35 | Thresh_connect = 15; 36 | N_smoothing = 1; 37 | fprintf(1,'Computing the 3D surface mesh...\n'); 38 | [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); 39 | 40 | 41 | % Display the 3D mesh: 42 | figure(7); 43 | h = trimesh(tri2,Xc2(1,:),Xc2(3,:),-Xc2(2,:)); 44 | set(h,'EdgeColor', 'b'); 45 | xlabel('X'); 46 | ylabel('Y'); 47 | zlabel('Z'); 48 | axis('equal'); 49 | rotate3d on; 50 | view(0.5,12); 51 | axis equal 52 | 53 | 54 | 55 | % Save the mesh in a VRML file: 56 | TT = [0;0;0]; 57 | wwn = [1;0;0]; 58 | nw = pi; 59 | fieldOfView = 3*atan((ny/2)/fc(2)); 60 | 61 | filename_VRML = ['mesh' sprintf('%.4d',ind_view) '.wrl']; 62 | 63 | fprintf(1,'Saving Geometry in the VRML file %s...(use Cosmo Player to visualize the mesh)\n',filename_VRML); 64 | 65 | file = fopen(filename_VRML,'wt'); 66 | fprintf(file ,'#VRML V2.0 utf8\n'); 67 | fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; nw; fieldOfView]); 68 | %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))]); 69 | fprintf(file ,'Transform { children [ Shape { appearance Appearance { material Material { }} geometry IndexedFaceSet { coord Coordinate { point [\n'); 70 | fprintf(file,'%.3f %.3f %.3f,\n',Xc2); 71 | fprintf(file ,']} coordIndex [\n'); 72 | fprintf(file,'%d,%d,%d,-1,\n',tri2'-1); 73 | fprintf(file ,']}}]}\n'); 74 | fprintf(file,']} '); 75 | fclose(file) ; 76 | -------------------------------------------------------------------------------- /writeras.m: -------------------------------------------------------------------------------- 1 | function writeras(filename, image, map); 2 | %WRITERAS Write an image file in sun raster format. 3 | % WRITERAS('imagefile.ras', image_matrix, map) writes a 4 | % "sun.raster" image file. 5 | 6 | % Written by Thomas K. Leung 3/30/93. 7 | % @ California Institute of Technology. 8 | 9 | 10 | % PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998 11 | 12 | dot = max(find(filename == '.')); 13 | suffix = filename(dot+1:dot+3); 14 | 15 | if nargin < 3, 16 | map = []; 17 | end; 18 | 19 | if(strcmp(suffix, 'ras')) 20 | %Write header 21 | 22 | fp = fopen(filename, 'wb'); 23 | if(fp < 0) error(['Cannot open ' filename '.']), end 24 | 25 | [height, width] = size(image); 26 | image = image - 1; 27 | mapsize = size(map, 1)*size(map,2); 28 | %fwrite(fp, ... 29 | % [1504078485, width, height, 8, height*width, 1, 1, mapsize], ... 30 | % 'long'); 31 | 32 | 33 | zero_str = '00000000'; 34 | 35 | % MAGIC NUMBER: 36 | 37 | 38 | fwrite(fp,89,'uchar'); 39 | fwrite(fp,166,'uchar'); 40 | fwrite(fp,106,'uchar'); 41 | fwrite(fp,149,'uchar'); 42 | 43 | width_str = dec2hex(width); 44 | width_str = [zero_str(1:8-length(width_str)) width_str]; 45 | 46 | for ii = 1:2:7, 47 | fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar'); 48 | end; 49 | 50 | 51 | height_str = dec2hex(height); 52 | height_str = [zero_str(1:8-length(height_str)) height_str]; 53 | 54 | for ii = 1:2:7, 55 | fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar'); 56 | end; 57 | 58 | fwrite(fp,0,'uchar'); 59 | fwrite(fp,0,'uchar'); 60 | fwrite(fp,0,'uchar'); 61 | fwrite(fp,8,'uchar'); 62 | 63 | ll = height*width; 64 | ll_str = dec2hex(ll); 65 | ll_str = [zero_str(1:8-length(ll_str)) ll_str]; 66 | 67 | for ii = 1:2:7, 68 | fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar'); 69 | end; 70 | 71 | fwrite(fp,0,'uchar'); 72 | fwrite(fp,0,'uchar'); 73 | fwrite(fp,0,'uchar'); 74 | fwrite(fp,1,'uchar'); 75 | fwrite(fp,0,'uchar'); 76 | fwrite(fp,0,'uchar'); 77 | fwrite(fp,0,'uchar'); 78 | fwrite(fp,~~mapsize,'uchar'); 79 | 80 | mapsize_str = dec2hex(mapsize); 81 | mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str]; 82 | 83 | %keyboard; 84 | 85 | for ii = 1:2:7, 86 | fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar'); 87 | end; 88 | 89 | 90 | if mapsize ~= 0 91 | map = min(255, fix(255*map)); 92 | fwrite(fp, map, 'uchar'); 93 | end 94 | if rem(width,2) == 1 95 | image = [image'; zeros(1, height)]'; 96 | top = 255 * ones(size(image)); 97 | fwrite(fp, min(top,image)', 'uchar'); 98 | else 99 | top = 255 * ones(size(image)); 100 | fwrite(fp, min(top,image)', 'uchar'); 101 | end 102 | fclose(fp); 103 | else 104 | error('Image file name must end in either ''ras'' or ''rast''.'); 105 | end 106 | -------------------------------------------------------------------------------- /loadppm.m: -------------------------------------------------------------------------------- 1 | %LOADPPM Load a PPM image 2 | % 3 | % I = loadppm(filename) 4 | % 5 | % Returns a matrix containing the image loaded from the PPM format 6 | % file filename. Handles ASCII (P3) and binary (P6) PPM file formats. 7 | % 8 | % If the filename has no extension, and open fails, a '.ppm' and 9 | % '.pnm' extension will be tried. 10 | % 11 | % SEE ALSO: saveppm loadpgm 12 | % 13 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 14 | 15 | 16 | % Peter Corke 1994 17 | 18 | function I = loadppm(file) 19 | white = [' ' 9 10 13]; % space, tab, lf, cr 20 | white = setstr(white); 21 | 22 | fid = fopen(file, 'r'); 23 | if fid < 0, 24 | fid = fopen([file '.ppm'], 'r'); 25 | end 26 | if fid < 0, 27 | fid = fopen([file '.pnm'], 'r'); 28 | end 29 | if fid < 0, 30 | error('Couldn''t open file'); 31 | end 32 | 33 | magic = fread(fid, 2, 'char'); 34 | while 1 35 | c = fread(fid,1,'char'); 36 | if c == '#', 37 | fgetl(fid); 38 | elseif ~any(c == white) 39 | fseek(fid, -1, 'cof'); % unputc() 40 | break; 41 | end 42 | end 43 | cols = fscanf(fid, '%d', 1); 44 | while 1 45 | c = fread(fid,1,'char'); 46 | if c == '#', 47 | fgetl(fid); 48 | elseif ~any(c == white) 49 | fseek(fid, -1, 'cof'); % unputc() 50 | return; 51 | end 52 | end 53 | rows = fscanf(fid, '%d', 1); 54 | while 1 55 | c = fread(fid,1,'char'); 56 | if c == '#', 57 | fgetl(fid); 58 | elseif ~any(c == white) 59 | fseek(fid, -1, 'cof'); % unputc() 60 | break; 61 | end 62 | end 63 | maxval = fscanf(fid, '%d', 1); 64 | 65 | % assume a carriage return only: 66 | 67 | c = fread(fid,1,'char'); 68 | 69 | % bug: because the image might be starting with special characters! 70 | %while 1 71 | % c = fread(fid,1,'char'); 72 | % if c == '#', 73 | % fgetl(fid); 74 | % elseif ~any(c == white) 75 | % fseek(fid, -1, 'cof'); % unputc() 76 | % break; 77 | % end 78 | %end 79 | if magic(1) == 'P', 80 | if magic(2) == '3', 81 | %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)]) 82 | I = fscanf(fid, '%d', [cols*3 rows]); 83 | elseif magic(2) == '6', 84 | %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)]) 85 | if maxval == 1, 86 | fmt = 'unint1'; 87 | elseif maxval == 15, 88 | fmt = 'uint4'; 89 | elseif maxval == 255, 90 | fmt = 'uint8'; 91 | elseif maxval == 2^32-1, 92 | fmt = 'uint32'; 93 | end 94 | I = fread(fid, [cols*3 rows], fmt); 95 | else 96 | disp('Not a PPM file'); 97 | end 98 | end 99 | % 100 | % now the matrix has interleaved columns of R, G, B 101 | % 102 | I = I'; 103 | size(I); 104 | R = I(:,1:3:(cols*3)); 105 | G = I(:,2:3:(cols*3)); 106 | B = I(:,3:3:(cols*3)); 107 | fclose(fid); 108 | 109 | 110 | I = zeros(rows,cols,3); 111 | I(:,:,1) = R; 112 | I(:,:,2) = G; 113 | I(:,:,3) = B; 114 | I = uint8(I); 115 | -------------------------------------------------------------------------------- /rect.m: -------------------------------------------------------------------------------- 1 | function [Irec] = rect(I,R,f,c,k,alpha,KK_new); 2 | 3 | 4 | if nargin < 5, 5 | k = [0;0;0;0;0]; 6 | if nargin < 4, 7 | c = [0;0]; 8 | if nargin < 3, 9 | f = [1;1]; 10 | if nargin < 2, 11 | R = eye(3); 12 | if nargin < 1, 13 | error('ERROR: Need an image to rectify'); 14 | end; 15 | end; 16 | end; 17 | end; 18 | end; 19 | 20 | 21 | if nargin < 7, 22 | if nargin < 6, 23 | KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; 24 | else 25 | KK_new = alpha; % the 6th argument is actually KK_new 26 | end; 27 | alpha = 0; 28 | end; 29 | 30 | 31 | 32 | % Note: R is the motion of the points in space 33 | % So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. 34 | 35 | 36 | if ~exist('KK_new'), 37 | KK_new = [f(1) alpha*f(1) c(1);0 f(2) c(2);0 0 1]; 38 | end; 39 | 40 | 41 | [nr,nc] = size(I); 42 | 43 | Irec = 255*ones(nr,nc); 44 | 45 | [mx,my] = meshgrid(1:nc, 1:nr); 46 | px = reshape(mx',nc*nr,1); 47 | py = reshape(my',nc*nr,1); 48 | 49 | rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; 50 | 51 | 52 | % Rotation: (or affine transformation): 53 | 54 | rays2 = R'*rays; 55 | 56 | x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; 57 | 58 | 59 | % Add distortion: 60 | xd = apply_distortion(x,k); 61 | 62 | 63 | % Reconvert in pixels: 64 | 65 | px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); 66 | py2 = f(2)*xd(2,:)+c(2); 67 | 68 | 69 | % Interpolate between the closest pixels: 70 | 71 | px_0 = floor(px2); 72 | 73 | 74 | py_0 = floor(py2); 75 | py_1 = py_0 + 1; 76 | 77 | 78 | good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); 79 | 80 | px2 = px2(good_points); 81 | py2 = py2(good_points); 82 | px_0 = px_0(good_points); 83 | py_0 = py_0(good_points); 84 | 85 | alpha_x = px2 - px_0; 86 | alpha_y = py2 - py_0; 87 | 88 | a1 = (1 - alpha_y).*(1 - alpha_x); 89 | a2 = (1 - alpha_y).*alpha_x; 90 | a3 = alpha_y .* (1 - alpha_x); 91 | a4 = alpha_y .* alpha_x; 92 | 93 | ind_lu = px_0 * nr + py_0 + 1; 94 | ind_ru = (px_0 + 1) * nr + py_0 + 1; 95 | ind_ld = px_0 * nr + (py_0 + 1) + 1; 96 | ind_rd = (px_0 + 1) * nr + (py_0 + 1) + 1; 97 | 98 | ind_new = (px(good_points)-1)*nr + py(good_points); 99 | 100 | 101 | 102 | Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd); 103 | 104 | 105 | 106 | return; 107 | 108 | 109 | % Convert in indices: 110 | 111 | fact = 3; 112 | 113 | [XX,YY]= meshgrid(1:nc,1:nr); 114 | [XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); 115 | 116 | %tic; 117 | Iinterp = interp2(XX,YY,I,XXi,YYi); 118 | %toc 119 | 120 | [nri,nci] = size(Iinterp); 121 | 122 | 123 | ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; 124 | ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; 125 | 126 | good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); 127 | -------------------------------------------------------------------------------- /rect_index.m: -------------------------------------------------------------------------------- 1 | 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); 2 | 3 | 4 | if nargin < 5, 5 | k = [0;0;0;0;0]; 6 | if nargin < 4, 7 | c = [0;0]; 8 | if nargin < 3, 9 | f = [1;1]; 10 | if nargin < 2, 11 | R = eye(3); 12 | if nargin < 1, 13 | error('ERROR: Need an image to rectify'); 14 | %break; 15 | end; 16 | end; 17 | end; 18 | end; 19 | end; 20 | 21 | 22 | if nargin < 7, 23 | if nargin < 6, 24 | KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; 25 | else 26 | KK_new = alpha; % the 6th argument is actually KK_new 27 | end; 28 | alpha = 0; 29 | end; 30 | 31 | 32 | 33 | % Note: R is the motion of the points in space 34 | % So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. 35 | 36 | 37 | if ~exist('KK_new'), 38 | KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1]; 39 | end; 40 | 41 | 42 | [nr,nc] = size(I); 43 | 44 | Irec = 255*ones(nr,nc); 45 | 46 | [mx,my] = meshgrid(1:nc, 1:nr); 47 | px = reshape(mx',nc*nr,1); 48 | py = reshape(my',nc*nr,1); 49 | 50 | rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; 51 | 52 | 53 | % Rotation: (or affine transformation): 54 | 55 | rays2 = R'*rays; 56 | 57 | x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; 58 | 59 | 60 | % Add distortion: 61 | xd = apply_distortion(x,k); 62 | 63 | 64 | % Reconvert in pixels: 65 | 66 | px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); 67 | py2 = f(2)*xd(2,:)+c(2); 68 | 69 | 70 | % Interpolate between the closest pixels: 71 | 72 | px_0 = floor(px2); 73 | py_0 = floor(py2); 74 | 75 | 76 | good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); 77 | 78 | px2 = px2(good_points); 79 | py2 = py2(good_points); 80 | px_0 = px_0(good_points); 81 | py_0 = py_0(good_points); 82 | 83 | alpha_x = px2 - px_0; 84 | alpha_y = py2 - py_0; 85 | 86 | a1 = (1 - alpha_y).*(1 - alpha_x); 87 | a2 = (1 - alpha_y).*alpha_x; 88 | a3 = alpha_y .* (1 - alpha_x); 89 | a4 = alpha_y .* alpha_x; 90 | 91 | ind_1 = px_0 * nr + py_0 + 1; 92 | ind_2 = (px_0 + 1) * nr + py_0 + 1; 93 | ind_3 = px_0 * nr + (py_0 + 1) + 1; 94 | ind_4 = (px_0 + 1) * nr + (py_0 + 1) + 1; 95 | 96 | ind_new = (px(good_points)-1)*nr + py(good_points); 97 | 98 | 99 | Irec(ind_new) = a1 .* I(ind_1) + a2 .* I(ind_2) + a3 .* I(ind_3) + a4 .* I(ind_4); 100 | 101 | 102 | 103 | return; 104 | 105 | 106 | % Convert in indices: 107 | 108 | fact = 3; 109 | 110 | [XX,YY]= meshgrid(1:nc,1:nr); 111 | [XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); 112 | 113 | %tic; 114 | Iinterp = interp2(XX,YY,I,XXi,YYi); 115 | %toc 116 | 117 | [nri,nci] = size(Iinterp); 118 | 119 | 120 | ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; 121 | ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; 122 | 123 | good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); 124 | -------------------------------------------------------------------------------- /data_calib_no_read.m: -------------------------------------------------------------------------------- 1 | %%% This script alets the user enter the name of the images (base name, numbering scheme,... 2 | 3 | 4 | % Checks that there are some images in the directory: 5 | 6 | l_ras = dir('*ras'); 7 | s_ras = size(l_ras,1); 8 | l_bmp = dir('*bmp'); 9 | s_bmp = size(l_bmp,1); 10 | l_tif = dir('*tif'); 11 | s_tif = size(l_tif,1); 12 | l_pgm = dir('*pgm'); 13 | s_pgm = size(l_pgm,1); 14 | l_ppm = dir('*ppm'); 15 | s_ppm = size(l_ppm,1); 16 | l_jpg = dir('*jpg'); 17 | s_jpg = size(l_jpg,1); 18 | l_jpeg = dir('*jpeg'); 19 | s_jpeg = size(l_jpeg,1); 20 | 21 | s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; 22 | 23 | if s_tot < 1, 24 | fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); 25 | return; 26 | end; 27 | 28 | 29 | % IF yes, display the directory content: 30 | 31 | dir; 32 | 33 | Nima_valid = 0; 34 | 35 | while (Nima_valid==0), 36 | 37 | fprintf(1,'\n'); 38 | calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); 39 | 40 | format_image = '0'; 41 | 42 | while format_image == '0', 43 | 44 | format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpeg'',''g''=''jpeg'', ''m''=''ppm'') ','s'); 45 | 46 | if isempty(format_image), 47 | format_image = 'ras'; 48 | end; 49 | 50 | if lower(format_image(1)) == 'm', 51 | format_image = 'ppm'; 52 | else 53 | if lower(format_image(1)) == 'b', 54 | format_image = 'bmp'; 55 | else 56 | if lower(format_image(1)) == 't', 57 | format_image = 'tif'; 58 | else 59 | if lower(format_image(1)) == 'p', 60 | format_image = 'pgm'; 61 | else 62 | if lower(format_image(1)) == 'j', 63 | format_image = 'jpg'; 64 | else 65 | if lower(format_image(1)) == 'r', 66 | format_image = 'ras'; 67 | else 68 | if lower(format_image(1)) == 'g', 69 | format_image = 'jpeg'; 70 | else 71 | disp('Invalid image format'); 72 | format_image = '0'; % Ask for format once again 73 | end; 74 | end; 75 | end; 76 | end; 77 | end; 78 | end; 79 | end; 80 | end; 81 | 82 | 83 | check_directory; 84 | 85 | end; 86 | 87 | 88 | 89 | if (Nima_valid~=0), 90 | % Reading images: 91 | 92 | ima_read_calib_no_read; % may be launched from the toolbox itself 93 | 94 | 95 | fprintf(1,'\n'); 96 | 97 | fprintf(1,'To display the thumbnail images of all the calibration images, you may run mosaic_no_read (may be slow)\n'); 98 | 99 | end; 100 | -------------------------------------------------------------------------------- /mosaic_no_read.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | if ~exist('n_ima'), 4 | data_calib_no_read; 5 | end; 6 | 7 | 8 | check_active_images; 9 | 10 | n_col = floor(sqrt(n_ima*nx/ny)); 11 | 12 | n_row = ceil(n_ima / n_col); 13 | 14 | 15 | ker2 = 1; 16 | for ii = 1:n_col, 17 | ker2 = conv(ker2,[1/4 1/2 1/4]); 18 | end; 19 | 20 | ny2 = length(1:n_col:ny); 21 | nx2 = length(1:n_col:nx); 22 | 23 | %II = I_1(1:n_col:end,1:n_col:end); 24 | %[ny2,nx2] = size(II); 25 | 26 | 27 | 28 | kk_c = 1; 29 | 30 | II_mosaic = []; 31 | 32 | for jj = 1:n_row, 33 | 34 | 35 | II_row = []; 36 | 37 | for ii = 1:n_col, 38 | 39 | 40 | if (kk_c <= n_ima), 41 | 42 | if ~type_numbering, 43 | number_ext = num2str(image_numbers(kk_c)); 44 | else 45 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk_c)); 46 | end; 47 | 48 | ima_name = [calib_name number_ext '.' format_image]; 49 | 50 | 51 | 52 | if (exist(ima_name)) & (kk_c <= n_ima), 53 | 54 | if active_images(kk_c), 55 | 56 | if format_image(1) == 'p', 57 | if format_image(2) == 'p', 58 | I = double(loadppm(ima_name)); 59 | else 60 | I = double(loadpgm(ima_name)); 61 | end; 62 | else 63 | if format_image(1) == 'r', 64 | I = readras(ima_name); 65 | else 66 | I = double(imread(ima_name)); 67 | end; 68 | end; 69 | 70 | %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing 71 | I = I(1:n_col:end,1:n_col:end); 72 | else 73 | I = zeros(ny2,nx2); 74 | end; 75 | 76 | else 77 | 78 | I = zeros(ny2,nx2); 79 | 80 | end; 81 | 82 | else 83 | 84 | I = zeros(ny2,nx2); 85 | 86 | end; 87 | 88 | II_row = [II_row I]; 89 | 90 | if ii ~= n_col, 91 | 92 | II_row = [II_row zeros(ny2,3)]; 93 | 94 | end; 95 | 96 | 97 | kk_c = kk_c + 1; 98 | 99 | end; 100 | 101 | nn2 = size(II_row,2); 102 | 103 | if jj ~= n_row, 104 | II_row = [II_row; zeros(3,nn2)]; 105 | end; 106 | 107 | II_mosaic = [II_mosaic ; II_row]; 108 | 109 | end; 110 | 111 | figure(2); 112 | image(II_mosaic); 113 | colormap(gray(256)); 114 | title('Calibration images'); 115 | set(gca,'Xtick',[]) 116 | set(gca,'Ytick',[]) 117 | axis('image'); 118 | 119 | -------------------------------------------------------------------------------- /readras.m: -------------------------------------------------------------------------------- 1 | function [X, map] = readras(filename, ys, ye, xs, xe); 2 | %READRAS Read an image file in sun raster format. 3 | % READRAS('imagefile.ras') reads a "sun.raster" image file. 4 | % [X, map] = READRAS('imagefile.ras') returns both the image and a 5 | % color map, so that 6 | % [X, map] = readras('imagefile.ras'); 7 | % image(X) 8 | % colormap(map) 9 | % axis('equal') 10 | % will display the result with the proper colors. 11 | % NOTE: readras cannot deal with complicated color maps. 12 | % In fact, Matlab doesn't quite allow to work with colormaps 13 | % with more than 64 entries. 14 | % 15 | 16 | %% 17 | %% (C) Thomas K. Leung 3/30/93. 18 | %% California Institute of Technology. 19 | %% Modified by Andrea Mennucci to deal with color images 20 | %% 21 | 22 | % PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998 23 | 24 | dot = max(find(filename == '.')); 25 | suffix = filename(dot+1:dot+3); 26 | 27 | if(strcmp(lower(suffix), 'ras')) % raster file format % 28 | fp = fopen(filename, 'rb'); 29 | if(fp<0) error(['Cannot open ' filename '.']), end 30 | 31 | %Read and crack the 32-byte header 32 | fseek(fp, 4, -1); 33 | 34 | width = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 35 | 36 | height = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 37 | 38 | depth = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 39 | 40 | length = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 41 | 42 | type = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 43 | 44 | maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 45 | 46 | maplen = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 47 | 48 | maplen = maplen / 3; 49 | 50 | if maptype == 2 % RMT_RAW 51 | map = fread(fp, [maplen, 3], 'uchar')/255; 52 | % if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end; 53 | elseif maptype == 1 % RMT_EQUAL_RGB 54 | map(:,1) = fread(fp, [maplen], 'uchar'); 55 | map(:,2) = fread(fp, [maplen], 'uchar'); 56 | map(:,3) = fread(fp, [maplen], 'uchar'); 57 | %maxmap = max(max(map)); 58 | map = map/255; 59 | if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end; 60 | else % RMT_NONE 61 | map = []; 62 | end 63 | % if maplen>64, 64 | % map=[map',zeros(3,256-maplen)]'; 65 | % end; 66 | 67 | % Read the image 68 | 69 | if rem(width,2) == 1 70 | Xt = fread(fp, [width+1, height], 'uchar'); 71 | X = Xt(1:width, :)'; 72 | else 73 | Xt = fread(fp, [width, height], 'uchar'); 74 | X = Xt'; 75 | end 76 | X = X + 1; 77 | fclose(fp); 78 | else 79 | error('Image file name must end in either ''ras'' or ''rast''.'); 80 | end 81 | 82 | 83 | if nargin == 5 84 | 85 | X = X(ys:ye, xs:xe); 86 | 87 | end -------------------------------------------------------------------------------- /data_calib.m: -------------------------------------------------------------------------------- 1 | %%% This script alets the user enter the name of the images (base name, numbering scheme,... 2 | 3 | 4 | % Checks that there are some images in the directory: 5 | 6 | l_ras = dir('*ras'); 7 | s_ras = size(l_ras,1); 8 | l_bmp = dir('*bmp'); 9 | s_bmp = size(l_bmp,1); 10 | l_tif = dir('*tif'); 11 | s_tif = size(l_tif,1); 12 | l_pgm = dir('*pgm'); 13 | s_pgm = size(l_pgm,1); 14 | l_ppm = dir('*ppm'); 15 | s_ppm = size(l_ppm,1); 16 | l_jpg = dir('*jpg'); 17 | s_jpg = size(l_jpg,1); 18 | l_jpeg = dir('*jpeg'); 19 | s_jpeg = size(l_jpeg,1); 20 | 21 | s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; 22 | 23 | if s_tot < 1, 24 | fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); 25 | return; 26 | end; 27 | 28 | 29 | % IF yes, display the directory content: 30 | 31 | dir; 32 | 33 | Nima_valid = 0; 34 | 35 | while (Nima_valid==0), 36 | 37 | fprintf(1,'\n'); 38 | calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); 39 | 40 | format_image = '0'; 41 | 42 | while format_image == '0', 43 | 44 | format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); 45 | 46 | if isempty(format_image), 47 | format_image = 'ras'; 48 | end; 49 | 50 | if lower(format_image(1)) == 'm', 51 | format_image = 'ppm'; 52 | else 53 | if lower(format_image(1)) == 'b', 54 | format_image = 'bmp'; 55 | else 56 | if lower(format_image(1)) == 't', 57 | format_image = 'tif'; 58 | else 59 | if lower(format_image(1)) == 'p', 60 | format_image = 'pgm'; 61 | else 62 | if lower(format_image(1)) == 'j', 63 | format_image = 'jpg'; 64 | else 65 | if lower(format_image(1)) == 'r', 66 | format_image = 'ras'; 67 | else 68 | if lower(format_image(1)) == 'g', 69 | format_image = 'jpeg'; 70 | else 71 | disp('Invalid image format'); 72 | format_image = '0'; % Ask for format once again 73 | end; 74 | end; 75 | end; 76 | end; 77 | end; 78 | end; 79 | end; 80 | end; 81 | 82 | 83 | check_directory; 84 | 85 | end; 86 | 87 | 88 | 89 | %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num'; 90 | 91 | %eval(string_save); 92 | 93 | 94 | 95 | if (Nima_valid~=0), 96 | % Reading images: 97 | 98 | ima_read_calib; % may be launched from the toolbox itself 99 | % Show all the calibration images: 100 | 101 | if ~isempty(ind_read), 102 | 103 | mosaic; 104 | 105 | end; 106 | 107 | end; 108 | 109 | -------------------------------------------------------------------------------- /cam_proj_calib_optim.m: -------------------------------------------------------------------------------- 1 | load camera_results; 2 | 3 | string_global = 'global n_ima'; 4 | for kk = 1:n_ima, 5 | string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; 6 | end; 7 | eval(string_global); 8 | 9 | 10 | %----------------- global optimization: --------------------- 11 | 12 | load projector_data; % load the projector corners (previously saved) 13 | load projector_results; 14 | 15 | solution_projector = solution; 16 | 17 | 18 | load camera_results; 19 | 20 | solution_camera = solution; 21 | 22 | param_cam = solution_camera([1:10 16:end]); 23 | param_proj = solution_projector([1:10 16:end]); 24 | 25 | param = [param_cam;param_proj]; 26 | 27 | 28 | % Restart the minimization from here (if need be): 29 | load camera_results; 30 | load calib_cam_proj_optim2; 31 | 32 | 33 | options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; 34 | 35 | %if 0, % use the full distortion model: 36 | 37 | % fprintf(1,'Take the complete distortion model\n'); 38 | 39 | % test the global error function: 40 | % e_global = error_cam_proj(param); 41 | 42 | % param_init = param; 43 | 44 | % param = leastsq('error_cam_proj',param,options); 45 | 46 | 47 | %else 48 | 49 | % Use a limitd distortion model (no 6th order) 50 | fprintf(1,'Take the 6th order distortion coefficient out\n'); 51 | 52 | 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]); 53 | 54 | % test the global error function: 55 | e_global2 = error_cam_proj2(param); 56 | 57 | param_init = param; 58 | 59 | param = leastsq('error_cam_proj2',param,options); 60 | 61 | 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)]; 62 | 63 | %end; 64 | 65 | 66 | 67 | 68 | % Extract the parameters: 69 | 70 | cam_proj_extract_param; 71 | 72 | 73 | % Relative prosition of camera wrt world: 74 | omc = omc_1; 75 | Rc = Rc_1; 76 | Tc = Tc_1; 77 | 78 | % relative position of projector wrt world: 79 | Rp = R*Rc; 80 | omp = rodrigues(Rp); 81 | Tp = T + R*Tc; 82 | 83 | 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']); 84 | 85 | no_image = 0; 86 | % Image size: (may or may not be available) 87 | nx = 640; 88 | ny = 480; 89 | 90 | comp_error_calib; 91 | 92 | % Save the optimal camera parameters: 93 | saving_calib; 94 | copyfile('Calib_Results.mat','camera_results_optim3.mat'); 95 | delete('Calib_Results.mat'); 96 | 97 | % Save the optimal camera parameters: 98 | fc = fp; 99 | cc = cp; 100 | alpha_c = alpha_p; 101 | kc = kp; 102 | 103 | n_ima = 1; 104 | X_1 = X_proj; 105 | x_1 = x_proj; 106 | omc_1 = om; 107 | Tc_1 = T; 108 | Rc_1 = R; 109 | 110 | % Image size: (may or may not be available) 111 | nx = 1024; 112 | ny = 768; 113 | 114 | % No calibration image is available (only the corner coordinates) 115 | no_image = 1; 116 | 117 | comp_error_calib; 118 | 119 | saving_calib; 120 | copyfile('Calib_Results.mat','projector_results_optim3.mat'); 121 | delete('Calib_Results.mat'); 122 | 123 | -------------------------------------------------------------------------------- /compute_extrinsic_refine.m: -------------------------------------------------------------------------------- 1 | function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), 2 | 3 | %compute_extrinsic 4 | % 5 | %[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) 6 | % 7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection 8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. 9 | %Works with planar and non-planar structures. 10 | % 11 | %INPUT: x_kk: Feature locations on the images 12 | % X_kk: Corresponding grid coordinates 13 | % fc: Camera focal length 14 | % cc: Principal point coordinates 15 | % kc: Distortion coefficients 16 | % alpha_c: Skew coefficient 17 | % MaxIter: Maximum number of iterations 18 | % 19 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space 20 | % Tckk: 3D translation vector attached to the grid positions in space 21 | % Rckk: 3D rotation matrices corresponding to the omc vectors 22 | 23 | % 24 | %Method: Computes the normalized point coordinates, then computes the 3D pose 25 | % 26 | %Important functions called within that program: 27 | % 28 | %normalize_pixel: Computes the normalize image point coordinates. 29 | % 30 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. 31 | % 32 | %project_points.m: Computes the 2D image projections of a set of 3D points 33 | 34 | 35 | if nargin < 10, 36 | thresh_cond = inf; 37 | end; 38 | 39 | 40 | if nargin < 9, 41 | MaxIter = 20; 42 | end; 43 | 44 | if nargin < 8, 45 | alpha_c = 0; 46 | if nargin < 7, 47 | kc = zeros(5,1); 48 | if nargin < 6, 49 | cc = zeros(2,1); 50 | if nargin < 5, 51 | fc = ones(2,1); 52 | if nargin < 4, 53 | error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); 54 | return; 55 | end; 56 | end; 57 | end; 58 | end; 59 | end; 60 | 61 | 62 | % Initialization: 63 | 64 | omckk = omc_init; 65 | Tckk = Tc_init; 66 | 67 | 68 | % Final optimization (minimize the reprojection error in pixel): 69 | % through Gradient Descent: 70 | 71 | param = [omckk;Tckk]; 72 | 73 | change = 1; 74 | 75 | iter = 0; 76 | 77 | %keyboard; 78 | 79 | %fprintf(1,'Gradient descent iterations: '); 80 | 81 | while (change > 1e-10)&(iter < MaxIter), 82 | 83 | %fprintf(1,'%d...',iter+1); 84 | 85 | [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); 86 | 87 | ex = x_kk - x; 88 | 89 | %keyboard; 90 | 91 | JJ = [dxdom dxdT]; 92 | 93 | if cond(JJ) > thresh_cond, 94 | change = 0; 95 | else 96 | 97 | JJ2 = JJ'*JJ; 98 | 99 | param_innov = inv(JJ2)*(JJ')*ex(:); 100 | param_up = param + param_innov; 101 | change = norm(param_innov)/norm(param_up); 102 | param = param_up; 103 | iter = iter + 1; 104 | 105 | omckk = param(1:3); 106 | Tckk = param(4:6); 107 | 108 | end; 109 | 110 | end; 111 | 112 | %fprintf(1,'\n'); 113 | 114 | Rckk = rodrigues(omckk); 115 | -------------------------------------------------------------------------------- /go_calib_optim_fisheye_no_read.asv: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | if ~exist('rosette_calibration','var') 33 | rosette_calibration = 0; 34 | end; 35 | 36 | if ~exist('n_ima'), 37 | data_calib_no_read; % Load the images 38 | click_calib_fisheye_no_read; % Extract the corners 39 | end; 40 | 41 | 42 | check_active_images; 43 | 44 | check_extracted_images; 45 | 46 | check_active_images; 47 | 48 | desactivated_images = []; 49 | 50 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 51 | 52 | if (rosette_calibration) 53 | %%% Special Setting for the Rosette: 54 | est_dist = [ones(2,1);zeros(2,1)]; 55 | end; 56 | 57 | 58 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 59 | go_calib_optim_iter_fisheye; 60 | 61 | 62 | if ~isempty(desactivated_images), 63 | 64 | param_list_save = param_list; 65 | 66 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 67 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 68 | desactivated_images = []; 69 | 70 | go_calib_optim_iter_fisheye; 71 | 72 | if ~isempty(desactivated_images), 73 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 74 | end; 75 | 76 | param_list = [param_list_save(:,1:end-1) param_list]; 77 | 78 | end; 79 | 80 | 81 | %%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%% 82 | 83 | %graphout_calib; 84 | 85 | -------------------------------------------------------------------------------- /compute_extrinsic_refine_fisheye.m: -------------------------------------------------------------------------------- 1 | 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) 2 | 3 | %compute_extrinsic 4 | % 5 | %[omckk,Tckk,Rckk] = compute_extrinsic_refine_fisheye(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) 6 | % 7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection 8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. 9 | %Works with planar and non-planar structures. 10 | % 11 | %INPUT: x_kk: Feature locations on the images 12 | % X_kk: Corresponding grid coordinates 13 | % fc: Camera focal length 14 | % cc: Principal point coordinates 15 | % kc: Fisheye Distortion coefficients 16 | % alpha_c: Skew coefficient 17 | % MaxIter: Maximum number of iterations 18 | % 19 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space 20 | % Tckk: 3D translation vector attached to the grid positions in space 21 | % Rckk: 3D rotation matrices corresponding to the omc vectors 22 | 23 | % 24 | %Method: Computes the normalized point coordinates, then computes the 3D pose 25 | % 26 | %Important functions called within that program: 27 | % 28 | %normalize_pixel: Computes the normalize image point coordinates. 29 | % 30 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. 31 | % 32 | %project_points.m: Computes the 2D image projections of a set of 3D points 33 | 34 | 35 | if nargin < 10, 36 | thresh_cond = inf; 37 | end; 38 | 39 | 40 | if nargin < 9, 41 | MaxIter = 20; 42 | end; 43 | 44 | if nargin < 8, 45 | alpha_c = 0; 46 | if nargin < 7, 47 | kc = zeros(5,1); 48 | if nargin < 6, 49 | cc = zeros(2,1); 50 | if nargin < 5, 51 | fc = ones(2,1); 52 | if nargin < 4, 53 | error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); 54 | return; 55 | end; 56 | end; 57 | end; 58 | end; 59 | end; 60 | 61 | 62 | % Initialization: 63 | 64 | omckk = omc_init; 65 | Tckk = Tc_init; 66 | 67 | 68 | % Final optimization (minimize the reprojection error in pixel): 69 | % through Gradient Descent: 70 | 71 | param = [omckk;Tckk]; 72 | 73 | change = 1; 74 | 75 | iter = 0; 76 | 77 | %keyboard; 78 | 79 | %fprintf(1,'Gradient descent iterations: '); 80 | 81 | while (change > 1e-10)&(iter < MaxIter), 82 | 83 | %fprintf(1,'%d...',iter+1); 84 | 85 | [x,dxdom,dxdT] = project_points_fisheye(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); 86 | 87 | ex = x_kk - x; 88 | 89 | %keyboard; 90 | 91 | JJ = [dxdom dxdT]; 92 | 93 | if cond(JJ) > thresh_cond, 94 | change = 0; 95 | else 96 | 97 | JJ2 = JJ'*JJ; 98 | 99 | param_innov = inv(JJ2)*(JJ')*ex(:); 100 | param_up = param + param_innov; 101 | change = norm(param_innov)/norm(param_up); 102 | param = param_up; 103 | iter = iter + 1; 104 | 105 | omckk = param(1:3); 106 | Tckk = param(4:6); 107 | 108 | end; 109 | 110 | end; 111 | 112 | %fprintf(1,'\n'); 113 | 114 | Rckk = rodrigues(omckk); 115 | -------------------------------------------------------------------------------- /ima_read_calib.m: -------------------------------------------------------------------------------- 1 | 2 | if ~exist('calib_name')|~exist('format_image'), 3 | data_calib; 4 | return; 5 | end; 6 | 7 | check_directory; 8 | 9 | if ~exist('n_ima'), 10 | data_calib; 11 | return; 12 | end; 13 | 14 | check_active_images; 15 | 16 | 17 | images_read = active_images; 18 | 19 | 20 | if exist('image_numbers'), 21 | first_num = image_numbers(1); 22 | end; 23 | 24 | 25 | % Just to fix a minor bug: 26 | if ~exist('first_num'), 27 | first_num = image_numbers(1); 28 | end; 29 | 30 | 31 | image_numbers = first_num:n_ima-1+first_num; 32 | 33 | no_image_file = 0; 34 | 35 | i = 1; 36 | 37 | while (i <= n_ima), % & (~no_image_file), 38 | 39 | if active_images(i), 40 | 41 | %fprintf(1,'Loading image %d...\n',i); 42 | 43 | if ~type_numbering, 44 | number_ext = num2str(image_numbers(i)); 45 | else 46 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); 47 | end; 48 | 49 | ima_name = [calib_name number_ext '.' format_image]; 50 | 51 | if i == ind_active(1), 52 | fprintf(1,'Loading image '); 53 | end; 54 | 55 | if exist(ima_name), 56 | 57 | fprintf(1,'%d...',i); 58 | 59 | if format_image(1) == 'p', 60 | if format_image(2) == 'p', 61 | Ii = double(loadppm(ima_name)); 62 | else 63 | Ii = double(loadpgm(ima_name)); 64 | end; 65 | else 66 | if format_image(1) == 'r', 67 | Ii = readras(ima_name); 68 | else 69 | Ii = double(imread(ima_name)); 70 | end; 71 | end; 72 | 73 | 74 | if size(Ii,3)>1, 75 | Ii = 0.299 * Ii(:,:,1) + 0.5870 * Ii(:,:,2) + 0.114 * Ii(:,:,3); 76 | end; 77 | 78 | eval(['I_' num2str(i) ' = Ii;']); 79 | 80 | else 81 | 82 | %fprintf(1,'%d...no image...',i); 83 | 84 | images_read(i) = 0; 85 | 86 | %no_image_file = 1; 87 | 88 | end; 89 | 90 | end; 91 | 92 | i = i+1; 93 | 94 | end; 95 | 96 | 97 | ind_read = find(images_read); 98 | 99 | 100 | 101 | 102 | if isempty(ind_read), 103 | 104 | fprintf(1,'\nWARNING! No image were read\n'); 105 | 106 | no_image_file = 1; 107 | 108 | 109 | else 110 | 111 | 112 | %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n'); 113 | 114 | 115 | if no_image_file, 116 | 117 | %fprintf(1,'WARNING! Some images were not read properly\n'); 118 | 119 | end; 120 | 121 | 122 | fprintf(1,'\n'); 123 | 124 | if size(I_1,1)~=480, 125 | small_calib_image = 1; 126 | else 127 | small_calib_image = 0; 128 | end; 129 | 130 | [Hcal,Wcal] = size(I_1); % size of the calibration image 131 | 132 | [ny,nx] = size(I_1); 133 | 134 | clickname = []; 135 | 136 | map = gray(256); 137 | 138 | %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image'; 139 | 140 | %eval(string_save); 141 | 142 | disp('done'); 143 | %click_calib; 144 | 145 | end; 146 | 147 | if ~(exist('map')==1), map = gray(256); end; 148 | 149 | active_images = images_read; 150 | 151 | -------------------------------------------------------------------------------- /compute_extrinsic_refine2.m: -------------------------------------------------------------------------------- 1 | function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine2(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), 2 | 3 | %compute_extrinsic 4 | % 5 | %[omckk,Tckk,Rckk] = compute_extrinsic_refine2(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) 6 | % 7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection 8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. 9 | %Works with planar and non-planar structures. 10 | % 11 | %INPUT: x_kk: Feature locations on the images 12 | % X_kk: Corresponding grid coordinates 13 | % fc: Camera focal length 14 | % cc: Principal point coordinates 15 | % kc: Distortion coefficients 16 | % alpha_c: Skew coefficient 17 | % MaxIter: Maximum number of iterations 18 | % 19 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space 20 | % Tckk: 3D translation vector attached to the grid positions in space 21 | % Rckk: 3D rotation matrices corresponding to the omc vectors 22 | 23 | % 24 | %Method: Computes the normalized point coordinates, then computes the 3D pose 25 | % 26 | %Important functions called within that program: 27 | % 28 | %normalize_pixel: Computes the normalize image point coordinates. 29 | % 30 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. 31 | % 32 | %project_points.m: Computes the 2D image projections of a set of 3D points 33 | 34 | 35 | if nargin < 10, 36 | thresh_cond = inf; 37 | end; 38 | 39 | 40 | if nargin < 9, 41 | MaxIter = 20; 42 | end; 43 | 44 | if nargin < 8, 45 | alpha_c = 0; 46 | if nargin < 7, 47 | kc = zeros(5,1); 48 | if nargin < 6, 49 | cc = zeros(2,1); 50 | if nargin < 5, 51 | fc = ones(2,1); 52 | if nargin < 4, 53 | error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); 54 | return; 55 | end; 56 | end; 57 | end; 58 | end; 59 | end; 60 | 61 | 62 | % Initialization: 63 | 64 | omckk = omc_init; 65 | Tckk = Tc_init; 66 | 67 | 68 | % Final optimization (minimize the reprojection error in pixel): 69 | % through Gradient Descent: 70 | 71 | param = [omckk;Tckk]; 72 | 73 | change = 1; 74 | 75 | iter = 0; 76 | 77 | %keyboard; 78 | 79 | %fprintf(1,'Gradient descent iterations: '); 80 | 81 | 82 | % Normalize the points: 83 | x_kk_n = normalize_pixel(x_kk,fc,cc,kc,alpha_c); 84 | 85 | 86 | while (change > 1e-10)&(iter < MaxIter), 87 | 88 | %fprintf(1,'%d...',iter+1); 89 | 90 | [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk); 91 | 92 | ex = x_kk_n - x; 93 | 94 | %keyboard; 95 | 96 | JJ = [dxdom dxdT]; 97 | 98 | if cond(JJ) > thresh_cond, 99 | change = 0; 100 | else 101 | 102 | JJ2 = JJ'*JJ; 103 | 104 | param_innov = inv(JJ2)*(JJ')*ex(:); 105 | param_up = param + param_innov; 106 | change = norm(param_innov)/norm(param_up); 107 | param = param_up; 108 | iter = iter + 1; 109 | 110 | omckk = param(1:3); 111 | Tckk = param(4:6); 112 | 113 | end; 114 | 115 | end; 116 | 117 | %fprintf(1,'\n'); 118 | 119 | Rckk = rodrigues(omckk); 120 | -------------------------------------------------------------------------------- /export_calib_data.m: -------------------------------------------------------------------------------- 1 | %% Export calibration data (corners + 3D coordinates) to 2 | %% text files (in Willson-Heikkila's format or Zhang's format) 3 | 4 | %% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion 5 | %% of adding this export function to the toolbox. 6 | 7 | 8 | if ~exist('n_ima'), 9 | fprintf(1,'ERROR: No calibration data to export\n'); 10 | 11 | else 12 | 13 | if n_ima == 0, 14 | fprintf(1,'ERROR: No calibration data to export\n'); 15 | return; 16 | end; 17 | 18 | check_active_images; 19 | 20 | check_extracted_images; 21 | 22 | check_active_images; 23 | 24 | fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n'); 25 | 26 | qformat = -1; 27 | 28 | while (qformat ~=0)&(qformat ~=1), 29 | 30 | fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n') 31 | qformat = input('Format of export (enter 0 or 1): '); 32 | 33 | if isempty(qformat) 34 | qformat = -1; 35 | end; 36 | 37 | if (qformat ~=0)&(qformat ~=1), 38 | 39 | fprintf(1,'Invalid entry. Try again.\n') 40 | 41 | end; 42 | 43 | end; 44 | 45 | if qformat == 0, 46 | 47 | fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n'); 48 | outputfile = input('File basename: ','s'); 49 | 50 | for kk = ind_active, 51 | 52 | eval(['X_kk = X_' num2str(kk) ';']); 53 | eval(['x_kk = x_' num2str(kk) ';']); 54 | 55 | Xx = [X_kk ; x_kk]'; 56 | 57 | file_name = [outputfile num2str(kk)]; 58 | 59 | disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']); 60 | 61 | eval(['save ' file_name ' Xx -ASCII']); 62 | 63 | end; 64 | 65 | else 66 | 67 | fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n'); 68 | modelfile = input('File basename for the 3D world coordinates: ','s'); 69 | datafile = input('File basename for the 2D image coordinates: ','s'); 70 | 71 | for kk = ind_active, 72 | 73 | eval(['X_kk = X_' num2str(kk) ';']); 74 | eval(['x_kk = x_' num2str(kk) ';']); 75 | 76 | if ~exist(['n_sq_x_' num2str(kk)]), 77 | n_sq_x = 1; 78 | n_sq_y = size(X_kk,2); 79 | else 80 | eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); 81 | eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); 82 | end; 83 | 84 | X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)'; 85 | Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)'; 86 | XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1)); 87 | 88 | x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)'; 89 | y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)'; 90 | xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1)); 91 | 92 | disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']); 93 | 94 | eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']); 95 | eval(['save ' datafile num2str(kk) '.txt xy -ASCII']); 96 | 97 | end; 98 | 99 | 100 | end; 101 | 102 | fprintf(1,'done\n'); 103 | 104 | end; 105 | -------------------------------------------------------------------------------- /compute_extrinsic.m: -------------------------------------------------------------------------------- 1 | function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), 2 | 3 | %compute_extrinsic 4 | % 5 | %[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection 8 | %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. 9 | %Works with planar and non-planar structures. 10 | % 11 | %INPUT: x_kk: Feature locations on the images 12 | % X_kk: Corresponding grid coordinates 13 | % fc: Camera focal length 14 | % cc: Principal point coordinates 15 | % kc: Distortion coefficients 16 | % alpha_c: Skew coefficient 17 | % 18 | %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space 19 | % Tckk: 3D translation vector attached to the grid positions in space 20 | % Rckk: 3D rotation matrices corresponding to the omc vectors 21 | % H: Homography between points on the grid and points on the image plane (in pixel) 22 | % This makes sense only if the planar that is used in planar. 23 | % x: Reprojections of the points on the image plane 24 | % ex: Reprojection error: ex = x_kk - x; 25 | % 26 | %Method: Computes the normalized point coordinates, then computes the 3D pose 27 | % 28 | %Important functions called within that program: 29 | % 30 | %normalize_pixel: Computes the normalize image point coordinates. 31 | % 32 | %pose3D: Computes the 3D pose of the structure given the normalized image projection. 33 | % 34 | %project_points.m: Computes the 2D image projections of a set of 3D points 35 | 36 | 37 | 38 | if nargin < 8, 39 | thresh_cond = inf; 40 | end; 41 | 42 | 43 | if nargin < 7, 44 | MaxIter = 20; 45 | end; 46 | 47 | 48 | if nargin < 6, 49 | alpha_c = 0; 50 | if nargin < 5, 51 | kc = zeros(5,1); 52 | if nargin < 4, 53 | cc = zeros(2,1); 54 | if nargin < 3, 55 | fc = ones(2,1); 56 | if nargin < 2, 57 | error('Need 2D projections and 3D points (in compute_extrinsic.m)'); 58 | return; 59 | end; 60 | end; 61 | end; 62 | end; 63 | end; 64 | 65 | % Initialization: 66 | 67 | [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); 68 | 69 | % Refinement: 70 | [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond); 71 | 72 | 73 | % computation of the homography (not useful in the end) 74 | 75 | H = [Rckk(:,1:2) Tckk]; 76 | 77 | % Computes the reprojection error in pixels: 78 | 79 | x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); 80 | 81 | ex = x_kk - x; 82 | 83 | 84 | % Converts the homography in pixel units: 85 | 86 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; 87 | 88 | H = KK*H; 89 | 90 | 91 | 92 | 93 | return; 94 | 95 | 96 | % Test of compte extrinsic: 97 | 98 | Np = 4; 99 | sx = 10; 100 | sy = 10; 101 | sz = 5; 102 | 103 | om = randn(3,1); 104 | T = [0;0;100]; 105 | 106 | noise = 2/1000; 107 | 108 | XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; 109 | xx = project_points(XX,om,T); 110 | 111 | xxn = xx + noise * randn(2,Np); 112 | 113 | [omckk,Tckk] = compute_extrinsic(xxn,XX); 114 | 115 | [om omckk om-omckk] 116 | [T Tckk T-Tckk] 117 | 118 | figure(3); 119 | plot(xx(1,:),xx(2,:),'r+'); 120 | hold on; 121 | plot(xxn(1,:),xxn(2,:),'g+'); 122 | hold off; 123 | -------------------------------------------------------------------------------- /ima_read_calib_no_read.m: -------------------------------------------------------------------------------- 1 | 2 | if ~exist('calib_name')|~exist('format_image'), 3 | data_calib_no_read; 4 | return; 5 | end; 6 | 7 | check_directory; 8 | 9 | if ~exist('n_ima'), 10 | data_calib_no_read; 11 | return; 12 | end; 13 | 14 | check_active_images; 15 | 16 | 17 | images_read = active_images; 18 | 19 | 20 | if exist('image_numbers'), 21 | first_num = image_numbers(1); 22 | end; 23 | 24 | 25 | % Just to fix a minor bug: 26 | if ~exist('first_num'), 27 | first_num = image_numbers(1); 28 | end; 29 | 30 | 31 | image_numbers = first_num:n_ima-1+first_num; 32 | 33 | 34 | no_image_file = 0; 35 | 36 | % Step used to clean the memory if a previous atttempt has been made to read the entire set of images into memory: 37 | for kk = 1:n_ima, 38 | if (exist(['I_' num2str(kk)])==1), 39 | clear(['I_' num2str(kk)]); 40 | end; 41 | end; 42 | 43 | 44 | fprintf(1,'\nChecking directory content for the calibration images (no global image loading in memory efficient mode)\n'); 45 | 46 | one_image_read = 0; 47 | 48 | i = 1; 49 | 50 | while (i <= n_ima), % & (~no_image_file), 51 | 52 | if active_images(i), 53 | 54 | %fprintf(1,'Loading image %d...\n',i); 55 | 56 | if ~type_numbering, 57 | number_ext = num2str(image_numbers(i)); 58 | else 59 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); 60 | end; 61 | 62 | ima_name = [calib_name number_ext '.' format_image]; 63 | 64 | if i == ind_active(1), 65 | fprintf(1,'Found images: '); 66 | end; 67 | 68 | if exist(ima_name), 69 | 70 | fprintf(1,'%d...',i); 71 | 72 | 73 | if ~one_image_read 74 | 75 | if format_image(1) == 'p', 76 | if format_image(2) == 'p', 77 | I = double(loadppm(ima_name)); 78 | else 79 | I = double(loadpgm(ima_name)); 80 | end; 81 | else 82 | if format_image(1) == 'r', 83 | I = readras(ima_name); 84 | else 85 | I = double(imread(ima_name)); 86 | end; 87 | end; 88 | 89 | 90 | if size(I,3)>1, 91 | I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); 92 | end; 93 | 94 | 95 | if size(I,1)~=480, 96 | small_calib_image = 1; 97 | else 98 | small_calib_image = 0; 99 | end; 100 | 101 | [Hcal,Wcal] = size(I); % size of the calibration image 102 | 103 | [ny,nx] = size(I); 104 | 105 | one_image_read = 1; 106 | 107 | end; 108 | 109 | 110 | else 111 | 112 | images_read(i) = 0; 113 | 114 | end; 115 | 116 | end; 117 | 118 | i = i+1; 119 | 120 | end; 121 | 122 | 123 | ind_read = find(images_read); 124 | 125 | 126 | if ~(exist('map')==1), map = gray(256); end; 127 | 128 | active_images = images_read; 129 | 130 | fprintf(1,'\ndone\n'); 131 | -------------------------------------------------------------------------------- /recomp_corner_calib.m: -------------------------------------------------------------------------------- 1 | % Re-select te corners after calibration 2 | 3 | if ~exist('n_ima')|~exist('fc'), 4 | fprintf(1,'No calibration data available.\n'); 5 | return; 6 | end; 7 | 8 | check_active_images; 9 | 10 | flag = 0; 11 | for kk = ind_active, 12 | if ~exist(['y_' num2str(kk)]), 13 | flag = 1; 14 | else 15 | eval(['ykk = y_' num2str(kk) ';']); 16 | if isnan(ykk(1,1)), 17 | flag = 1; 18 | end; 19 | end; 20 | end; 21 | 22 | if flag, 23 | fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); 24 | return; 25 | end; 26 | 27 | 28 | if n_ima == 0, 29 | 30 | fprintf(1,'No image data available\n'); 31 | 32 | else 33 | 34 | if ~exist(['I_' num2str(ind_active(1))]), 35 | n_ima_save = n_ima; 36 | active_images_save = active_images; 37 | ima_read_calib; 38 | n_ima = n_ima_save; 39 | active_images = active_images_save; 40 | check_active_images; 41 | if no_image_file, 42 | disp('Cannot extract corners without images'); 43 | return; 44 | end; 45 | end; 46 | 47 | fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); 48 | 49 | disp('Window size for corner finder (wintx and winty):'); 50 | wintx = input('wintx ([] = 5) = '); 51 | if isempty(wintx), wintx = 5; end; 52 | wintx = round(wintx); 53 | winty = input('winty ([] = 5) = '); 54 | if isempty(winty), winty = 5; end; 55 | winty = round(winty); 56 | 57 | fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); 58 | 59 | ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); 60 | 61 | if isempty(ima_numbers), 62 | ima_proc = 1:n_ima; 63 | else 64 | ima_proc = ima_numbers; 65 | end; 66 | 67 | q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): ','s'); 68 | 69 | fprintf(1,'Processing image '); 70 | 71 | for kk = ima_proc; 72 | 73 | if active_images(kk), 74 | 75 | fprintf(1,'%d...',kk); 76 | 77 | if isempty(q_auto), 78 | 79 | eval(['I = I_' num2str(kk) ';']); 80 | 81 | eval(['y = y_' num2str(kk) ';']); 82 | 83 | xc = cornerfinder(y+1,I,winty,wintx); % the four corners 84 | 85 | eval(['wintx_' num2str(kk) ' = wintx;']); 86 | eval(['winty_' num2str(kk) ' = winty;']); 87 | 88 | eval(['x_' num2str(kk) '= xc - 1;']); 89 | 90 | else 91 | 92 | fprintf(1,'\n'); 93 | 94 | eval(['wintx_' num2str(kk) ' = wintx;']); 95 | eval(['winty_' num2str(kk) ' = winty;']); 96 | 97 | click_ima_calib; 98 | 99 | end; 100 | 101 | else 102 | 103 | if ~exist(['omc_' num2str(kk)]), 104 | 105 | eval(['dX_' num2str(kk) ' = NaN;']); 106 | eval(['dY_' num2str(kk) ' = NaN;']); 107 | 108 | eval(['wintx_' num2str(kk) ' = NaN;']); 109 | eval(['winty_' num2str(kk) ' = NaN;']); 110 | 111 | eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); 112 | eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); 113 | 114 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 115 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 116 | 117 | end; 118 | 119 | end; 120 | 121 | 122 | end; 123 | 124 | % Recompute the error: 125 | 126 | comp_error_calib; 127 | 128 | fprintf(1,'\ndone\n'); 129 | 130 | end; 131 | -------------------------------------------------------------------------------- /saving_calib_no_results.m: -------------------------------------------------------------------------------- 1 | 2 | if ~exist('n_ima'), 3 | fprintf(1,'No calibration data available.\n'); 4 | return; 5 | end; 6 | 7 | check_active_images; 8 | 9 | for kk = 1:n_ima, 10 | if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; 11 | if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; 12 | end; 13 | 14 | if ~exist('wintx'), 15 | wintx = []; 16 | winty = []; 17 | end; 18 | 19 | if ~exist('dX_default'), 20 | dX_default = []; 21 | dY_default = []; 22 | end; 23 | 24 | if ~exist('dX'), 25 | dX = []; 26 | end; 27 | if ~exist('dY'), 28 | dY = dX; 29 | end; 30 | 31 | 32 | if ~exist('wintx_default')|~exist('winty_default'), 33 | wintx_default = max(round(nx/128),round(ny/96)); 34 | winty_default = wintx_default; 35 | end; 36 | 37 | if ~exist('alpha_c'), 38 | alpha_c = 0; 39 | end; 40 | 41 | if ~exist('err_std'), 42 | err_std = [NaN;NaN]; 43 | end; 44 | 45 | for kk = 1:n_ima, 46 | 47 | if ~exist(['n_sq_x_' num2str(kk)]), 48 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 49 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 50 | end; 51 | if ~exist(['wintx_' num2str(kk)]), 52 | eval(['wintx_' num2str(kk) ' = NaN;']); 53 | eval(['winty_' num2str(kk) ' = NaN;']); 54 | end; 55 | end; 56 | 57 | save_name = 'camera_data'; 58 | 59 | if exist([ save_name '.mat'])==2, 60 | disp('WARNING: File Calib_Results.mat already exists'); 61 | if exist('copyfile')==2, 62 | pfn = -1; 63 | cont = 1; 64 | while cont, 65 | pfn = pfn + 1; 66 | postfix = ['_old' num2str(pfn)]; 67 | save_name = [ 'camera_data' postfix]; 68 | cont = (exist([ save_name '.mat'])==2); 69 | end; 70 | copyfile('camera_data.mat',[save_name '.mat']); 71 | disp(['Copying the current camera_calib_data.mat file to ' save_name '.mat']); 72 | cont_save = 1; 73 | else 74 | disp('The file camera_data.mat is about to be changed.'); 75 | cont_save = input(1,'Do you want to continue? ([]=no,other=yes) ','s'); 76 | cont_save = ~isempty(cont_save); 77 | end; 78 | else 79 | cont_save = 1; 80 | end; 81 | 82 | 83 | if cont_save, 84 | 85 | save_name = 'camera_data'; 86 | 87 | if exist('calib_name'), 88 | 89 | fprintf(1,['\nSaving calibration data under ' save_name '.mat\n']); 90 | 91 | 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']; 92 | 93 | for kk = 1:n_ima, 94 | 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)]; 95 | end; 96 | 97 | else 98 | 99 | fprintf(1,['\nSaving calibration data under ' save_name '.mat (no image version)\n']); 100 | 101 | 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 ']; 102 | 103 | for kk = 1:n_ima, 104 | 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)]; 105 | end; 106 | 107 | end; 108 | 109 | 110 | 111 | %fprintf(1,'To load later click on Load\n'); 112 | 113 | eval(string_save); 114 | 115 | fprintf(1,'done\n'); 116 | 117 | end; 118 | -------------------------------------------------------------------------------- /TestFunction.m: -------------------------------------------------------------------------------- 1 | function [cdist, dcdistdom, dcdistdT, r, drdom, drdT] = TestFunction(X,om,T,k); 2 | 3 | [m,n] = size(X); 4 | 5 | [Y,dYdom,dYdT] = rigid_motion(X,om,T); 6 | 7 | 8 | inv_Z = 1./Y(3,:); 9 | 10 | x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; 11 | 12 | 13 | bb = (-x(1,:) .* inv_Z)'*ones(1,3); 14 | cc = (-x(2,:) .* inv_Z)'*ones(1,3); 15 | 16 | 17 | dxdom = zeros(2*n,3); 18 | dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); 19 | dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); 20 | 21 | dxdT = zeros(2*n,3); 22 | dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); 23 | dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); 24 | 25 | 26 | % Add fisheye distortion: 27 | 28 | r2 = x(1,:).^2 + x(2,:).^2; 29 | 30 | dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); 31 | dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); 32 | 33 | 34 | % Radial distance: 35 | r = sqrt(r2); 36 | drdr2 = 1 ./ (2*r); 37 | 38 | drdom = [ (drdr2').*dr2dom(:,1) (drdr2').*dr2dom(:,2) (drdr2').*dr2dom(:,3) ]; 39 | drdT = [ (drdr2').*dr2dT(:,1) (drdr2').*dr2dT(:,2) (drdr2').*dr2dT(:,3) ]; 40 | 41 | % Angle of the incoming ray: 42 | theta = atan(r); 43 | dthetadr = 1 ./ (1 + r2); 44 | 45 | dthetadom = [ (dthetadr').*drdom(:,1) (dthetadr').*drdom(:,2) (dthetadr').*drdom(:,3) ]; 46 | dthetadT = [ (dthetadr').*drdT(:,1) (dthetadr').*drdT(:,2) (dthetadr').*drdT(:,3) ]; 47 | 48 | 49 | 50 | % Add the distortion: 51 | 52 | theta2 = theta.^2; 53 | theta3 = theta.^3; 54 | theta4 = theta.^4; 55 | theta5 = theta.^5; 56 | theta6 = theta.^6; 57 | 58 | theta_d = theta + k(1)*theta2 + k(2)*theta3 + k(3)*theta4 + k(4)*theta5 + k(5)*theta6; 59 | 60 | dtheta_ddtheta = 1 + 2*k(1)*theta + 3*k(2)*theta2 + 4*k(3)*theta3 + 5*k(4)*theta4 + 6*k(5)*theta5; 61 | 62 | dtheta_ddom = [ (dtheta_ddtheta').*dthetadom(:,1) (dtheta_ddtheta').*dthetadom(:,2) (dtheta_ddtheta').*dthetadom(:,3) ]; 63 | dtheta_ddT = [ (dtheta_ddtheta').*dthetadT(:,1) (dtheta_ddtheta').*dthetadT(:,2) (dtheta_ddtheta').*dthetadT(:,3) ]; 64 | dtheta_ddk = [theta2' theta3' theta4' theta5' theta6']; 65 | 66 | 67 | r_d = tan(theta_d); 68 | dr_ddtheta_d = 1 ./ ((cos(theta_d)).^2); 69 | 70 | dr_ddom = [ (dr_ddtheta_d').*dtheta_ddom(:,1) (dr_ddtheta_d').*dtheta_ddom(:,2) (dr_ddtheta_d').*dtheta_ddom(:,3) ]; 71 | dr_ddT = [ (dr_ddtheta_d').*dtheta_ddT(:,1) (dr_ddtheta_d').*dtheta_ddT(:,2) (dr_ddtheta_d').*dtheta_ddT(:,3) ]; 72 | 73 | 74 | 75 | %cdist = r_d; 76 | %dcdistdom = dr_ddom; 77 | %dcdistdT = dr_ddT; 78 | 79 | %return; 80 | 81 | % ratio: 82 | inv_r = 1./r; 83 | cdist = r_d ./ r; 84 | dcdistdom = [ ((inv_r').*(dr_ddom(:,1) - (cdist').*drdom(:,1))) ((inv_r').*(dr_ddom(:,2) - (cdist').*drdom(:,2))) ((inv_r').*(dr_ddom(:,3) - (cdist').*drdom(:,3))) ]; 85 | dcdistdT = [ ((inv_r').*(dr_ddT(:,1) - (cdist').*drdT(:,1))) ((inv_r').*(dr_ddT(:,2) - (cdist').*drdT(:,2))) ((inv_r').*(dr_ddT(:,3) - (cdist').*drdT(:,3))) ]; 86 | 87 | 88 | 89 | 90 | 91 | 92 | return; 93 | 94 | % Test of the Jacobians: 95 | 96 | n = 10; 97 | 98 | X = 10*randn(3,n); 99 | om = randn(3,1); 100 | T = [10*randn(2,1);40]; 101 | k = 0.5*randn(5,1); 102 | 103 | [theta,dthetadom,dthetadT,r,drdom,drdT] = TestFunction(X,om,T,k); 104 | 105 | 106 | % Test on om: 107 | dom = 0.00000000001 * norm(om)*randn(3,1); 108 | om2 = om + dom; 109 | 110 | [theta2] = TestFunction(X,om2,T,k); 111 | theta_pred = theta + reshape(dthetadom*dom,1,n); 112 | norm(theta2 - theta)/norm(theta2 - theta_pred) 113 | 114 | 115 | % Test on T: 116 | dT = 0.0000001 * norm(T)*randn(3,1); 117 | T2 = T + dT; 118 | 119 | [theta2] = TestFunction(X,om,T2,k); 120 | theta_pred = theta + reshape(dthetadT*dT,1,n); 121 | norm(theta2 - theta)/norm(theta2 - theta_pred) 122 | 123 | -------------------------------------------------------------------------------- /saving_calib_ascii_fisheye.m: -------------------------------------------------------------------------------- 1 | if ~exist('save_name'), 2 | save_name = 'Calib_Results'; 3 | end; 4 | 5 | fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) 6 | 7 | 8 | fid = fopen([ save_name '.m'],'wt'); 9 | 10 | fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); 11 | fprintf(fid,'%%\n'); 12 | fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); 13 | fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); 14 | fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); 15 | fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); 16 | fprintf(fid,'\n\n'); 17 | fprintf(fid,'%%-- Focal length:\n'); 18 | fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); 19 | fprintf(fid,'\n'); 20 | fprintf(fid,'%%-- Principal point:\n'); 21 | fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); 22 | fprintf(fid,'\n'); 23 | fprintf(fid,'%%-- Skew coefficient:\n'); 24 | fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); 25 | fprintf(fid,'\n'); 26 | fprintf(fid,'%%-- Distortion coefficients:\n'); 27 | fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); 28 | fprintf(fid,'\n'); 29 | fprintf(fid,'%%-- Focal length uncertainty:\n'); 30 | fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); 31 | fprintf(fid,'\n'); 32 | fprintf(fid,'%%-- Principal point uncertainty:\n'); 33 | fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); 34 | fprintf(fid,'\n'); 35 | fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); 36 | fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); 37 | fprintf(fid,'\n'); 38 | fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); 39 | fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); 40 | fprintf(fid,'\n'); 41 | fprintf(fid,'%%-- Image size:\n'); 42 | fprintf(fid,'nx = %d;\n',nx); 43 | fprintf(fid,'ny = %d;\n',ny); 44 | fprintf(fid,'\n'); 45 | fprintf(fid,'\n'); 46 | fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); 47 | fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); 48 | fprintf(fid,'\n'); 49 | fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); 50 | fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); 51 | fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); 52 | fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); 53 | fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); 54 | fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); 55 | fprintf(fid,'\n\n'); 56 | fprintf(fid,'%%-- Extrinsic parameters:\n'); 57 | fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); 58 | fprintf(fid,'\n'); 59 | for kk = 1:n_ima, 60 | fprintf(fid,'%%-- Image #%d:\n',kk); 61 | eval(['omckk = omc_' num2str(kk) ';']); 62 | eval(['Tckk = Tc_' num2str(kk) ';']); 63 | fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); 64 | fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); 65 | if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), 66 | eval(['omckk_error = omc_error_' num2str(kk) ';']); 67 | eval(['Tckk_error = Tc_error_' num2str(kk) ';']); 68 | fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); 69 | fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); 70 | end; 71 | fprintf(fid,'\n'); 72 | end; 73 | 74 | fclose(fid); -------------------------------------------------------------------------------- /saving_calib_ascii.m: -------------------------------------------------------------------------------- 1 | if ~exist('save_name'), 2 | save_name = 'Calib_Results'; 3 | end; 4 | 5 | fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name) 6 | 7 | 8 | fid = fopen([ save_name '.m'],'wt'); 9 | 10 | fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n'); 11 | fprintf(fid,'%%\n'); 12 | fprintf(fid,'%% This script file can be directly executed under Matlab to recover the camera intrinsic and extrinsic parameters.\n'); 13 | fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n'); 14 | fprintf(fid,'%% All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n'); 15 | fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n'); 16 | fprintf(fid,'\n\n'); 17 | fprintf(fid,'%%-- Focal length:\n'); 18 | fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc); 19 | fprintf(fid,'\n'); 20 | fprintf(fid,'%%-- Principal point:\n'); 21 | fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc); 22 | fprintf(fid,'\n'); 23 | fprintf(fid,'%%-- Skew coefficient:\n'); 24 | fprintf(fid,'alpha_c = %5.15f;\n',alpha_c); 25 | fprintf(fid,'\n'); 26 | fprintf(fid,'%%-- Distortion coefficients:\n'); 27 | fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc); 28 | fprintf(fid,'\n'); 29 | fprintf(fid,'%%-- Focal length uncertainty:\n'); 30 | fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error); 31 | fprintf(fid,'\n'); 32 | fprintf(fid,'%%-- Principal point uncertainty:\n'); 33 | fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error); 34 | fprintf(fid,'\n'); 35 | fprintf(fid,'%%-- Skew coefficient uncertainty:\n'); 36 | fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error); 37 | fprintf(fid,'\n'); 38 | fprintf(fid,'%%-- Distortion coefficients uncertainty:\n'); 39 | fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error); 40 | fprintf(fid,'\n'); 41 | fprintf(fid,'%%-- Image size:\n'); 42 | fprintf(fid,'nx = %d;\n',nx); 43 | fprintf(fid,'ny = %d;\n',ny); 44 | fprintf(fid,'\n'); 45 | fprintf(fid,'\n'); 46 | fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n'); 47 | fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n'); 48 | fprintf(fid,'\n'); 49 | fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima); 50 | fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc); 51 | fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio); 52 | fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim); 53 | fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha); 54 | fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist); 55 | fprintf(fid,'\n\n'); 56 | fprintf(fid,'%%-- Extrinsic parameters:\n'); 57 | fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n'); 58 | fprintf(fid,'\n'); 59 | for kk = 1:n_ima, 60 | fprintf(fid,'%%-- Image #%d:\n',kk); 61 | eval(['omckk = omc_' num2str(kk) ';']); 62 | eval(['Tckk = Tc_' num2str(kk) ';']); 63 | fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk); 64 | fprintf(fid,'Tc_%d = [ %d ; %d ; %d ];\n',kk,Tckk); 65 | if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1), 66 | eval(['omckk_error = omc_error_' num2str(kk) ';']); 67 | eval(['Tckk_error = Tc_error_' num2str(kk) ';']); 68 | fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error); 69 | fprintf(fid,'Tc_error_%d = [ %d ; %d ; %d ];\n',kk,Tckk_error); 70 | end; 71 | fprintf(fid,'\n'); 72 | end; 73 | 74 | fclose(fid); --------------------------------------------------------------------------------