From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001 From: leochanj105 Date: Mon, 19 Oct 2020 23:09:30 -0400 Subject: initial sd-vbs initial sd-vbs add sd-vbs sd-vbs --- .../toolbox_basic/TOOLBOX_calib/Distor2Calib.m | 391 +++++++++++++++++ .../toolbox_basic/TOOLBOX_calib/Rectangle2Square.m | 19 + .../toolbox_basic/TOOLBOX_calib/UnWarpPlane.m | 54 +++ .../toolbox_basic/TOOLBOX_calib/add_suppress.m | 98 +++++ .../toolbox_basic/TOOLBOX_calib/analyse_error.m | 141 ++++++ .../toolbox_basic/TOOLBOX_calib/apply_distortion.m | 137 ++++++ .../toolbox_basic/TOOLBOX_calib/calib_gui.m | 117 +++++ .../TOOLBOX_calib/check_active_images.m | 19 + .../TOOLBOX_calib/check_convergence.m | 48 ++ .../toolbox_basic/TOOLBOX_calib/check_directory.m | 97 +++++ .../TOOLBOX_calib/check_extracted_images.m | 37 ++ .../toolbox_basic/TOOLBOX_calib/clear_windows.m | 4 + .../toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m | 10 + .../toolbox_basic/TOOLBOX_calib/click_calib.m | 193 +++++++++ .../toolbox_basic/TOOLBOX_calib/click_ima_calib.m | 230 ++++++++++ .../TOOLBOX_calib/click_ima_calib3D.m | 482 +++++++++++++++++++++ .../toolbox_basic/TOOLBOX_calib/comp_distortion.m | 38 ++ .../toolbox_basic/TOOLBOX_calib/comp_distortion2.m | 71 +++ .../TOOLBOX_calib/comp_distortion_oulu.m | 47 ++ .../toolbox_basic/TOOLBOX_calib/comp_error_calib.m | 46 ++ .../TOOLBOX_calib/compute_collineation.m | 66 +++ .../TOOLBOX_calib/compute_extrinsic.m | 123 ++++++ .../TOOLBOX_calib/compute_extrinsic_init.m | 151 +++++++ .../TOOLBOX_calib/compute_extrinsic_refine.m | 113 +++++ .../TOOLBOX_calib/compute_homography.m | 163 +++++++ .../toolbox_basic/TOOLBOX_calib/cornerfinder.m | 215 +++++++++ .../toolbox_basic/TOOLBOX_calib/count_squares.m | 74 ++++ .../toolbox_basic/TOOLBOX_calib/data_calib.m | 92 ++++ .../toolbox_basic/TOOLBOX_calib/error_analysis.m | 182 ++++++++ .../TOOLBOX_calib/export_calib_data.m | 99 +++++ .../toolbox_basic/TOOLBOX_calib/ext_calib.m | 152 +++++++ .../toolbox_basic/TOOLBOX_calib/extract_grid.m | 234 ++++++++++ .../TOOLBOX_calib/extract_parameters.m | 46 ++ .../TOOLBOX_calib/extract_parameters3D.m | 36 ++ .../TOOLBOX_calib/extrinsic_computation.m | 185 ++++++++ .../toolbox_basic/TOOLBOX_calib/fixallvariables.m | 19 + .../toolbox_basic/TOOLBOX_calib/fixvariable.m | 18 + .../toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m | 216 +++++++++ .../toolbox_basic/TOOLBOX_calib/go_calib_optim.m | 139 ++++++ .../TOOLBOX_calib/go_calib_optim_iter.m | 394 +++++++++++++++++ .../toolbox_basic/TOOLBOX_calib/ima_read_calib.m | 158 +++++++ .../TOOLBOX_calib/init_intrinsic_param.m | 158 +++++++ .../toolbox/toolbox_basic/TOOLBOX_calib/is3D.m | 19 + .../toolbox_basic/TOOLBOX_calib/loading_calib.m | 10 + .../toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m | 52 +++ .../toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m | 89 ++++ .../toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m | 109 +++++ .../toolbox_basic/TOOLBOX_calib/mean_std_robust.m | 7 + .../toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m | 92 ++++ .../toolbox_basic/TOOLBOX_calib/normalize.m | 43 ++ .../toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m | 26 ++ .../toolbox_basic/TOOLBOX_calib/project2_oulu.m | 53 +++ .../toolbox_basic/TOOLBOX_calib/project_points.m | 276 ++++++++++++ .../toolbox_basic/TOOLBOX_calib/project_points2.m | 312 +++++++++++++ .../toolbox_basic/TOOLBOX_calib/projectedGrid.m | 24 + .../toolbox_basic/TOOLBOX_calib/projector_calib.m | 258 +++++++++++ .../toolbox/toolbox_basic/TOOLBOX_calib/readras.m | 87 ++++ .../TOOLBOX_calib/recomp_corner_calib.m | 119 +++++ .../toolbox/toolbox_basic/TOOLBOX_calib/rect.m | 127 ++++++ .../toolbox_basic/TOOLBOX_calib/reproject_calib.m | 121 ++++++ .../toolbox_basic/TOOLBOX_calib/rigid_motion.m | 66 +++ .../toolbox_basic/TOOLBOX_calib/rodrigues.m | 217 ++++++++++ .../toolbox/toolbox_basic/TOOLBOX_calib/rotation.m | 23 + .../TOOLBOX_calib/run_error_analysis.m | 65 +++ .../toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m | 46 ++ .../toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m | 22 + .../toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m | 45 ++ .../toolbox_basic/TOOLBOX_calib/saving_calib.m | 95 ++++ .../TOOLBOX_calib/script_fit_distortion.m | 39 ++ .../toolbox/toolbox_basic/TOOLBOX_calib/startup.m | 9 + .../toolbox_basic/TOOLBOX_calib/undistort_image.m | 193 +++++++++ .../toolbox_basic/TOOLBOX_calib/willson_convert.m | 89 ++++ .../toolbox_basic/TOOLBOX_calib/willson_read.m | 59 +++ .../toolbox/toolbox_basic/TOOLBOX_calib/writeras.m | 105 +++++ 74 files changed, 8209 insertions(+) create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m (limited to 'SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib') diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m new file mode 100755 index 0000000..a82f583 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Distor2Calib.m @@ -0,0 +1,391 @@ +function [fc_2,Rc_2,Tc_2,H_2,distance,V_vert,V_hori,x_all_c,V_hori_pix,V_vert_pix,V_diag1_pix,V_diag2_pix]=Distor2Calib(k_dist,grid_pts_centered,n_sq_x,n_sq_y,Np,W,L,Xgrid_2,f_ini,N_iter,two_focal); + +% Computes the calibration parameters knowing the +% distortion factor k_dist + +% grid_pts_centered are the grid point coordinates after substraction of +% the optical center. + +% can give an optional guess for the focal length f_ini (can set to []) +% can provide the number of iterations for the Iterative Vanishing Point Algorithm + +% if the focal length is known perfectly, then, there is no need to iterate, +% and therefore, one can fix: N_iter = 0; + +% California Institute of Technology +% (c) Jean-Yves Bouguet - October 7th, 1997 + + + +%keyboard; + +if exist('two_focal'), + if isempty(two_focal), + two_focal=0; + end; +else + two_focal = 0; +end; + + +if exist('N_iter'), + if ~isempty(N_iter), + disp('Use number of iterations provided'); + else + N_iter = 10; + end; +else + N_iter = 10; +end; + +if exist('f_ini'), + if ~isempty(f_ini), + disp('Use focal provided'); + if length(f_ini)<2, f_ini=[f_ini;f_ini]; end; + fc_2 = f_ini; + x_all_c = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + x_all_c = comp_distortion(x_all_c,k_dist); % we can this time!!! + else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; + end; +else + fc_2 = [1;1]; + x_all_c = grid_pts_centered; +end; + + +dX = W/n_sq_x; +dY = L/n_sq_y; + + +N_x = n_sq_x+1; +N_y = n_sq_y+1; + + +x_grid = zeros(N_x,N_y); +y_grid = zeros(N_x,N_y); + + + + + +%%% Computation of the four vanishing points in pixels + + + x_grid(:) = grid_pts_centered(1,:); + y_grid(:) = grid_pts_centered(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + V_hori_pix = V_hori; + V_vert_pix = V_vert; + V_diag1_pix = V_diag1; + V_diag2_pix = V_diag2; + + +% end of computation of the vanishing points in pixels. + + + + + + + + +if two_focal, % only if we attempt to estimate two focals... + % Use diagonal lines also to add two extra vanishing points (?) + N_min = min(N_x,N_y); + + if N_min < 2, + use_diag = 0; + two_focal = 0; + disp('Cannot estimate two focals (no existing diagonals)'); + else + use_diag = 1; + Delta_N = abs(N_x-N_y); + N_extra = round((N_min - Delta_N - 1)/2); + diag_list = -N_extra:Delta_N+N_extra; + N_diag = length(diag_list); + diag_1 = zeros(3,N_diag); + diag_2 = zeros(3,N_diag); + end; +else + % Give up the use of the diagonals (so far) + % it seems that the error is increased + use_diag = 0; +end; + + + +% The vertical lines: vert, Horizontal lines: hori +vert = zeros(3,n_sq_x+1); +hori = zeros(3,n_sq_y+1); + +for counter_k = 1:N_iter, % the Iterative Vanishing Points Algorithm to + % estimate the focal length accurately + + x_grid(:) = x_all_c(1,:); + y_grid(:) = x_all_c(2,:); + + for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); + end; + + for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); + end; + + % 2 principle Vanishing points: + [U,S,V] = svd(vert); + V_vert = U(:,3); + [U,S,V] = svd(hori); + V_hori = U(:,3); + + + + % Square warping: + + + vert_first = vert(:,1) - dot(V_vert,vert(:,1))/dot(V_vert,V_vert) * V_vert; + vert_last = vert(:,n_sq_x+1) - dot(V_vert,vert(:,n_sq_x+1))/dot(V_vert,V_vert) * V_vert; + + hori_first = hori(:,1) - dot(V_hori,hori(:,1))/dot(V_hori,V_hori) * V_hori; + hori_last = hori(:,n_sq_y+1) - dot(V_hori,hori(:,n_sq_y+1))/dot(V_hori,V_hori) * V_hori; + + + x1 = cross(hori_first,vert_first); + x2 = cross(hori_first,vert_last); + x3 = cross(hori_last,vert_last); + x4 = cross(hori_last,vert_first); + + x1 = x1/x1(3); + x2 = x2/x2(3); + x3 = x3/x3(3); + x4 = x4/x4(3); + + + + [square] = Rectangle2Square([x1 x2 x3 x4],W,L); + + y1 = square(:,1); + y2 = square(:,2); + y3 = square(:,3); + y4 = square(:,4); + + H2 = cross(V_vert,V_hori); + + V_diag1 = cross(cross(y1,y3),H2); + V_diag2 = cross(cross(y2,y4),H2); + + V_diag1 = V_diag1 / norm(V_diag1); + V_diag2 = V_diag2 / norm(V_diag2); + + + + + % Estimation of the focal length, and normalization: + + % Compute the ellipsis of (1/f^2) positions: + % a * (1/fx)^2 + b * (1/fx)^2 = -c + + + a1 = V_hori(1); + b1 = V_hori(2); + c1 = V_hori(3); + + a2 = V_vert(1); + b2 = V_vert(2); + c2 = V_vert(3); + + a3 = V_diag1(1); + b3 = V_diag1(2); + c3 = V_diag1(3); + + a4 = V_diag2(1); + b4 = V_diag2(2); + c4 = V_diag2(3); + + + if two_focal, + + + A = [a1*a2 b1*b2;a3*a4 b3*b4]; + b = -[c1*c2;c3*c4]; + + f = sqrt(abs(1./(inv(A)*b))); + + else + + f = sqrt(abs(-(c1*c2*(a1*a2 + b1*b2) + c3*c4*(a3*a4 + b3*b4))/(c1^2*c2^2 + c3^2*c4^2))); + + f = [f;f]; + + end; + + + + % REMARK: + % if both a and b are small, the calibration is impossible. + % if one of them is small, only the other focal length is observable + % if none is small, both focals are observable + + + fc_2 = fc_2 .* f; + + + % DEBUG PART: fix focal to 500... + %fc_2= [500;500]; disp('Line 293 to be earased in Distor2Calib.m'); + + + % end of focal compensation + + % normalize by the current focal: + + x_all = [grid_pts_centered(1,:)/fc_2(1);grid_pts_centered(2,:)/fc_2(2)]; + + % Compensate by the distortion factor: + + x_all_c = comp_distortion(x_all,k_dist); + +end; + +% At that point, we hope that the distortion is gone... + +x_grid(:) = x_all_c(1,:); +y_grid(:) = x_all_c(2,:); + +for k=1:n_sq_x+1, + [U,S,V] = svd([x_grid(k,:);y_grid(k,:);ones(1,n_sq_y+1)]); + vert(:,k) = U(:,3); +end; + +for k=1:n_sq_y+1, + [U,S,V] = svd([x_grid(:,k)';y_grid(:,k)';ones(1,n_sq_x+1)]); + hori(:,k) = U(:,3); +end; + +% Vanishing points: +[U,S,V] = svd(vert); +V_vert = U(:,3); +[U,S,V] = svd(hori); +V_hori = U(:,3); + +% Horizon: + +H_2 = cross(V_vert,V_hori); + +% H_2 = cross(V_vert,V_hori); + +% pick a plane in front of the camera (positive depth) +if H_2(3) < 0, H_2 = -H_2; end; + + +% Rotation matrix: + +if V_hori(1) < 0, V_hori = -V_hori; end; + +V_hori = V_hori/norm(V_hori); +H_2 = H_2/norm(H_2); + +V_hori = V_hori - dot(V_hori,H_2)*H_2; + +Rc_2 = [V_hori cross(H_2,V_hori) H_2]; + +Rc_2 = Rc_2 / det(Rc_2); + +%omc_2 = rodrigues(Rc_2); + +%Rc_2 = rodrigues(omc_2); + +% Find the distance of the plane for translation vector: + +xc_2 = [x_all_c;ones(1,Np)]; + +Zc_2 = 1./sum(xc_2 .* (Rc_2(:,3)*ones(1,Np))); + +Xo_2 = [sum(xc_2 .* (Rc_2(:,1)*ones(1,Np))).*Zc_2 ; sum(xc_2 .* (Rc_2(:,2)*ones(1,Np))).*Zc_2]; + +XXo_2 = Xo_2 - mean(Xo_2')'*ones(1,Np); + +distance_x = norm(Xgrid_2(1,:))/norm(XXo_2(1,:)); +distance_y = norm(Xgrid_2(2,:))/norm(XXo_2(2,:)); + + +distance = sum(sum(XXo_2(1:2,:).*Xgrid_2(1:2,:)))/sum(sum(XXo_2(1:2,:).^2)); + +alpha = abs(distance_x - distance_y)/distance; + +if (alpha>0.1)&~two_focal, + disp('Should use two focals in x and y...'); +end; + +% Deduce the translation vector: + +Tc_2 = distance * H_2; + + + + + +return; + + V_hori_pix/V_hori_pix(3) + V_vert_pix/V_vert_pix(3) + V_diag1_pix/V_diag1_pix(3) + V_diag2_pix/V_diag2_pix(3) diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m new file mode 100755 index 0000000..a6bbbe5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/Rectangle2Square.m @@ -0,0 +1,19 @@ +function [square] = Rectangle2Square(rectangle,L,W); + +% Generate the square from a rectangle of known segment lengths +% from pt1 to pt2 : L +% from pt2 to pt3 : W + +[u_hori,u_vert] = UnWarpPlane(rectangle); + +coeff_x = sqrt(W/L); +coeff_y = 1/coeff_x; + +x_coord = [ 0 coeff_x coeff_x 0]; +y_coord = [ 0 0 coeff_y coeff_y]; + + +square = rectangle(:,1) * ones(1,4) + u_hori*x_coord + u_vert*y_coord; +square = square ./ (ones(3,1)*square(3,:)); + + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m new file mode 100755 index 0000000..8addf52 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/UnWarpPlane.m @@ -0,0 +1,54 @@ +function [u_hori,u_vert] = UnWarpPlane(x1,x2,x3,x4); + +% Recovers the two 3D directions of the rectangular patch x1x2x3x4 +% x1 is the origin point, ie any point of planar coordinate (x,y) on the +% rectangular patch will be projected on the image plane at: +% x1 + x * u_hori + y * u_vert +% +% Note: u_hori and u_vert are also the two vanishing points. + + +if nargin < 4, + + x4 = x1(:,4); + x3 = x1(:,3); + x2 = x1(:,2); + x1 = x1(:,1); + +end; + + +% Image Projection: +L1 = cross(x1,x2); +L2 = cross(x4,x3); +L3 = cross(x2,x3); +L4 = cross(x1,x4); + +% Vanishing point: +V1 = cross(L1,L2); +V2 = cross(L3,L4); + +% Horizon line: +H = cross(V1,V2); + +if H(3) < 0, H = -H; end; + + +H = H / norm(H); + + +X1 = x1 / dot(H,x1); +X2 = x2 / dot(H,x2); +X3 = x3 / dot(H,x3); +X4 = x4 / dot(H,x4); + +scale = X1(3); + +X1 = X1/scale; +X2 = X2/scale; +X3 = X3/scale; +X4 = X4/scale; + + +u_hori = X2 - X1; +u_vert = X4 - X1; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m new file mode 100755 index 0000000..a8a32c0 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/add_suppress.m @@ -0,0 +1,98 @@ + +if ~exist('n_ima'), + fprintf(1,'No data to process.\n'); + return; +end; + + +check_active_images; + + +fprintf(1,'\nThis function is useful to select a subset of images to calibrate\n'); + + fprintf(1,'\nThere are currently %d active images selected for calibration (out of %d):\n',length(ind_active),n_ima); + + if ~isempty(ind_active), + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + end; + + fprintf(1,'\n'); + + +fprintf(1,'\nDo you want to suppress or add images from that list?\n'); + +choice = 2; + +while (choice~=0)&(choice~=1), + choice = input('For suppressing images enter 0, for adding images enter 1 ([]=no change): '); + if isempty(choice), + fprintf(1,'No change applied to the list of active images.\n'); + return; + end; + if (choice~=0)&(choice~=1), + disp('Bad entry. Try again.'); + end; +end; + + +if choice, + + ima_numbers = input('Number(s) of image(s) to add ([] = all images) = '); + +if isempty(ima_numbers), + fprintf(1,'All %d images are now active\n',n_ima); + ima_proc = 1:n_ima; + else + ima_proc = ima_numbers; + end; + +else + + + ima_numbers = input('Number(s) of image(s) to suppress ([] = no image) = '); + + if isempty(ima_numbers), + fprintf(1,'No image has been suppressed. No modication of the list of active images.\n',n_ima); + ima_proc = []; + else + ima_proc = ima_numbers; + end; + +end; + +if ~isempty(ima_proc), + + active_images(ima_proc) = choice * ones(1,length(ima_proc)); + +end; + + + check_active_images; + + + fprintf(1,'\nThere is now a total of %d active images for calibration:\n',length(ind_active)); + + if ~isempty(ind_active), + + for ii = 1:length(ind_active)-2, + + fprintf(1,'%d, ',ind_active(ii)); + + end; + + fprintf(1,'%d and %d.',ind_active(end-1),ind_active(end)); + + end; + + fprintf(1,'\n\nYou may now run ''Calibration'' to recalibrate based on this new set of images.\n'); + + + \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m new file mode 100755 index 0000000..7a9cf0f --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/analyse_error.m @@ -0,0 +1,141 @@ +% Color code for each image: + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['ex_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + + +%if ~exist('no_grid'), + no_grid = 0; +%end; + +colors = 'brgkcm'; + + +figure(5); + +for kk = 1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if ~no_grid, + eval(['XX_kk = X_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + end; + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + end; + + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + + hold on; + end; + end; +end; + +hold off; +axis('equal'); +if 1, %~no_grid, + title('Reprojection error (in pixel) - To exit: right button'); +else + title('Reprojection error (in pixel)'); +end; +xlabel('x'); +ylabel('y'); + +set(5,'Name','error','NumberTitle','off'); + + + +err_std = std(ex')'; + +fprintf(1,'Pixel error: err = [ %3.5f %3.5f] (all active images)\n\n',err_std); + + +b = 1; + +while b==1, + +[xp,yp,b] = ginput3(1); + +if b==1, +ddd = (ex(1,:)-xp).^2 + (ex(2,:)-yp).^2; + +[mind,indmin] = min(ddd); + + +done = 0; +kk_ima = 1; +while (~done)&(kk_ima<=n_ima), + %fprintf(1,'%d...',kk_ima); + eval(['ex_kk = ex_' num2str(kk_ima) ';']); + sol_kk = find((ex_kk(1,:) == ex(1,indmin))&(ex_kk(2,:) == ex(2,indmin))); + if isempty(sol_kk), + kk_ima = kk_ima + 1; + else + done = 1; + end; +end; + +if ~no_grid, + +eval(['n_sq_x = n_sq_x_' num2str(kk_ima) ';']); +eval(['n_sq_y = n_sq_y_' num2str(kk_ima) ';']); + +Nx = n_sq_x+1; +Ny = n_sq_y+1; + +y1 = floor((sol_kk-1)./Nx); +x1 = sol_kk - 1 - Nx*y1; %rem(sol_kk-1,Nx); + +y1 = (n_sq_y+1) - y1; +x1 = x1 + 1; + +fprintf(1,'\nSelected image: %d\nSelected point: (col,row)=(%d,%d)\nNcol=%d, Nrow=%d\n',[kk_ima x1 y1 Nx Ny]); +fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + +else + + eval(['x_kk = x_' num2str(kk_ima) ';']); + + xpt = x_kk(:,sol_kk); + +fprintf(1,'\nSelected image: %d\nImage coordinates (in pixel): (%3.2f,%3.2f)\n',[kk_ima xpt']); +fprintf(1,'Pixel error = (%3.5f,%3.5f)\n',[ex(1,indmin) ex(2,indmin)]); + + +end; + + +if exist(['wintx_' num2str(kk_ima)]), + + eval(['wintx = wintx_' num2str(kk_ima) ';']); + eval(['winty = winty_' num2str(kk_ima) ';']); + + fprintf(1,'Window size: wintx = %d, winty = %d\n',[wintx winty]); +end; + + +end; + +end; + +disp('done'); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m new file mode 100755 index 0000000..f5c5b48 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/apply_distortion.m @@ -0,0 +1,137 @@ +function [xd,dxddk] = apply_distortion(x,k) + + +[m,n] = size(x); + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + +r4 = r2.^2; + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4; + +if nargout > 1, + dcdistdk = [ r2' r4' zeros(n,2)]; +end; + + +xd1 = x .* (ones(2,1)*cdist); + +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); + +if nargout > 1, + dxd1dk = zeros(2*n,4); + dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; + dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; +end; + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +if nargout > 1, + ddelta_xdk = zeros(2*n,4); + ddelta_xdk(1:2:end,3) = a1'; + ddelta_xdk(1:2:end,4) = a2'; + ddelta_xdk(2:2:end,3) = a3'; + ddelta_xdk(2:2:end,4) = a1'; +end; + +xd = xd1 + delta_x; + +if nargout > 1, + dxddk = dxd1dk + ddelta_xdk ; +end; + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(4,1); + + +[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k); + + +% Test on om: NOT OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points(X,om2,T,f,c,k); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points(X,om,T2,f,c,k); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points(X,om,T,f2,c,k); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points(X,om,T,f,c2,k); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(4)*randn(4,1); +k2 = k + dk; + +[x2] = project_points(X,om,T,f,c,k2); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m new file mode 100755 index 0000000..d591d03 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/calib_gui.m @@ -0,0 +1,117 @@ +fig_number = 1; + +n_row = 4; +n_col = 4; + +string_list = cell(n_row,n_col); +callback_list = cell(n_row,n_col); + +x_size = 85; +y_size = 14; +gap_x = 0; +font_name = 'clean'; +font_size = 8; + +title_figure = 'Camera Calibration Toolbox'; + +string_list{1,1} = 'Image names'; +string_list{1,2} = 'Read images'; +string_list{1,3} = 'Extract grid corners'; +string_list{1,4} = 'Calibration'; +string_list{2,1} = 'Show Extrinsic'; +string_list{2,2} = 'Reproject on images'; +string_list{2,3} = 'Analyse error'; +string_list{2,4} = 'Recomp. corners'; +string_list{3,1} = 'Add/Suppress images'; +string_list{3,2} = 'Save'; +string_list{3,3} = 'Load'; +string_list{3,4} = 'Exit'; + +string_list{4,1} = 'Comp. Extrinsic'; +string_list{4,2} = 'Undistort image'; +string_list{4,3} = 'Export calib data'; + + +callback_list{1,1} = 'data_calib;'; +callback_list{1,2} = 'ima_read_calib;'; +callback_list{1,3} = 'click_calib;'; +callback_list{1,4} = 'go_calib_optim;'; +callback_list{2,1} = 'ext_calib;'; +callback_list{2,2} = 'reproject_calib;'; +callback_list{2,3} = 'analyse_error;'; +callback_list{2,4} = 'recomp_corner_calib;'; +callback_list{3,1} = 'add_suppress;'; +callback_list{3,2} = 'saving_calib;'; +callback_list{3,3} = 'loading_calib;'; +callback_list{3,4} = ['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']; + +callback_list{4,1} = 'extrinsic_computation;'; +callback_list{4,2} = 'undistort_image;'; +callback_list{4,3} = 'export_calib_data;'; + + +%------- BEGIN PROECTED REGION -----------% +%------- DO NOT EDIT IN THIS REGION -----------% + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +fig_size_x = x_size*n_col+(n_col+1)*gap_x; +fig_size_y = y_size*n_row+(n_row+1)*gap_x; + +set(fig_number,'Units','points', ... + 'BackingStore','off', ... + 'Color',[0.8 0.8 0.8], ... + 'MenuBar','none', ... + 'Resize','off', ... + 'Name',title_figure, ... +'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... +'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); + +h_mat = zeros(n_row,n_col); + +posx = zeros(n_row,n_col); +posy = zeros(n_row,n_col); + +for i=n_row:-1:1, + for j = n_col:-1:1, + posx(i,j) = gap_x+(j-1)*(x_size+gap_x); + posy(i,j) = fig_size_y - i*(gap_x+y_size); + end; +end; + +for i=n_row:-1:1, + for j = n_col:-1:1, + if ~isempty(string_list{i,j}) & ~isempty(callback_list{i,j}), + h_mat(i,j) = uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',callback_list{i,j}, ... + 'ListboxTop',0, ... + 'Position',[posx(i,j) posy(i,j) x_size y_size], ... + 'String',string_list{i,j}, ... + 'fontsize',font_size,... + 'fontname',font_name,... + 'Tag','Pushbutton1'); + end; + end; +end; + +%------ END PROTECTED REGION ----------------% + +if 0, +%-- VERSION: + +uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'ListboxTop',0, ... + 'Position',[(fig_size_x-x_size/2)-2 -5 x_size/2 y_size], ... + 'String','ver. 1.0', ... + 'fontsize',8,... + 'BackgroundColor',[0.8 0.8 0.8], ... + 'fontname','clean',... + 'HorizontalAlignment','right', ... + 'Style','text'); +end; + + +%clear callback_list string_list fig_number fig_size_x fig_size_y i j n_col n_row pos string_list title_figure x_size y_size font_name font_size gap_x h_mat posx posy diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m new file mode 100755 index 0000000..fc365a5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_active_images.m @@ -0,0 +1,19 @@ + +if ~exist('active_images'), + active_images = ones(1,n_ima); +end; +n_act = length(active_images); +if n_act < n_ima, + active_images = [active_images ones(1,n_ima-n_act)]; +else + if n_act > n_ima, + active_images = active_images(1:n_ima); + end; +end; + +ind_active = find(active_images); + +if prod(active_images == 0), + disp('Error: There is no active image'); + break +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m new file mode 100755 index 0000000..c4b13fd --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_convergence.m @@ -0,0 +1,48 @@ +%%% Replay the set of solution vectors: + + +if ~exist('param_list'), + if ~exist('solution'); + fprintf(1,'Error: Need to calibrate first\n'); + return; + else + param_list = solution; + end; +end; + +N_iter = size(param_list,2); + +if N_iter == 1, + fprintf(1,'Warning: There is a unique state in the list of parameters.\n'); +end; + + + +%M = moviein(N_iter); + +for nn = 1:N_iter, + + solution = param_list(:,nn); + + extract_parameters; + comp_error_calib; + + ext_calib; + + drawnow; + +% Mnn = getframe(gcf); + +% M(:,nn) = Mnn; + +end; + +%fig = gcf; + + +%figure(fig+1); +%close; +%figure(fig+1); + +%clf; +%movie(M,20); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m new file mode 100755 index 0000000..dc23149 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_directory.m @@ -0,0 +1,97 @@ +% This small script looks in the direcory and checks if the images are there. +% +% This works only on Matlab 5.x (otherwise, the dir commands works differently) + +% (c) Jean-Yves Bouguet - Dec. 27th, 1999 + +l = dir([calib_name '*']); + +Nl = size(l,1); +Nima_valid = 0; +ind_valid = []; +loc_extension = []; +length_name = size(calib_name,2); + +if Nl > 0, + + for pp = 1:Nl, + filenamepp = l(pp).name; + iii = findstr(filenamepp,calib_name); + + loc_ext = findstr(filenamepp,format_image); + string_num = filenamepp(length_name+1:loc_ext - 2); + + if isempty(str2num(string_num)), + iii = []; + end; + + + if ~isempty(iii), + if (iii(1) ~= 1), + iii = []; + end; + end; + + + + if ~isempty(iii) & ~isempty(loc_ext), + + Nima_valid = Nima_valid + 1; + ind_valid = [ind_valid pp]; + loc_extension = [loc_extension loc_ext(1)]; + + end; + + end; + + if (Nima_valid==0), + + fprintf(1,'No image found. File format may be wrong.\n'); + + else + + % Get all the string numbers: + + string_length = zeros(1,Nima_valid); + indices = zeros(1,Nima_valid); + + + for ppp = 1:Nima_valid, + + name = l(ind_valid(ppp)).name; + string_num = name(length_name+1:loc_extension(ppp) - 2); + string_length(ppp) = size(string_num,2); + indices(ppp) = str2num(string_num); + + end; + + % Number of images... + first_num = min(indices); + n_ima = max(indices) - first_num + 1; + + if min(string_length) == max(string_length), + + N_slots = min(string_length); + type_numbering = 1; + + else + + N_slots = 1; + type_numbering = 0; + + end; + + image_numbers = first_num:n_ima-1+first_num; + + %%% By default, all the images are active for calibration: + + active_images = ones(1,n_ima); + + end; + +else + + fprintf(1,'No image found. Basename may be wrong.\n'); + +end; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m new file mode 100755 index 0000000..fa7df87 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/check_extracted_images.m @@ -0,0 +1,37 @@ +check_active_images; + +for kk = ind_active, + + if ~exist(['x_' num2str(kk)]), + + fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk); + + active_images(kk) = 0; + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + else + + eval(['xkk = x_' num2str(kk) ';']); + + if isnan(xkk(1)), + + fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk); + + active_images(kk) = 0; + + end; + + end; + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m new file mode 100755 index 0000000..1eccbd3 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clear_windows.m @@ -0,0 +1,4 @@ +for kk = 1:n_ima, + eval(['clear wintx_' num2str(kk)]); + eval(['clear winty_' num2str(kk)]); +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m new file mode 100755 index 0000000..a04be67 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/clearwin.m @@ -0,0 +1,10 @@ +% Function that clears all the wintx_i and winty_i +% In normal operation of the toolbox, this function should not be +% useful. +% only in cases where you want to re-extract corners using the Extract grid corners another time... not common. You might as well use the Recomp. corners. + +for kk = 1:n_ima, + + eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]); + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m new file mode 100755 index 0000000..1a6d2d7 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_calib.m @@ -0,0 +1,193 @@ +%if exist('images_read'); +% active_images = active_images & images_read; +%end; + +var2fix = 'dX_default'; + +fixvariable; + +var2fix = 'dY_default'; + +fixvariable; + +var2fix = 'map'; + +fixvariable; + + +if ~exist('n_ima'), + data_calib; +end; + +check_active_images; + +if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + if isempty(ind_read), + disp('Cannot extract corners without images'); + return; + end; +end; + + +%wintx = 10; % neigborhood of integration for +%winty = 10; % the corner finder + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + + +if ~exist('map'), map = gray(256); end; + + +disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + +if ~exist('dX_default'); + +% Default size of the pattern squares; + +% Setup of JY (old at Caltech) +%dX_default = 21.9250/11; +%dY_default = 18.1250/9; + +% Setup of JY (new at Intel) +%dX_default = 1.9750; +%dY_default = 1.9865; + + +% Setup of Luis and Enrico +%dX_default = 67.7/16; +%dY_default = 50.65/12; + + +% Setup of German +%dX_default = 10.16; +%dY_default = 10.16; + +% Setup of JY (new at Intel) +%dX_default = 1.9750*2.54; +%dY_default = 1.9865*2.54; + +% Setup of JY - 3D calibration rig at Intel (new at Intel) +%dX_default = 3; +%dY_default = 3; + +% Setup of JY - 3D calibration rig at Intel (new at Intel) - use units in mm to match Zhang +dX_default = 30; +dY_default = 30; + +end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); +else + ima_numbers = []; +end; + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +% Useful option to add images: +kk_first = ima_proc(1); %input('Start image number ([]=1=first): '); + +%if isempty(kk_first), kk_first = 1; end; + + +if exist(['wintx_' num2str(kk_first)]), + + eval(['wintxkk = wintx_' num2str(kk_first) ';']); + + if isempty(wintxkk) | isnan(wintxkk), + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + +else + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +end; + + +for kk = ima_proc, + if exist(['I_' num2str(kk)]), + click_ima_calib; + active_images(kk) = 1; + else + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +check_active_images; + + +% Fix potential non-existing variables: + +for kk = 1:n_ima, + if ~exist(['x_' num2str(kk)]), + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; +end; + + +string_save = 'save calib_data active_images ind_active wintx winty n_ima type_numbering N_slots first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default dX dY'; + +for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; +end; + +eval(string_save); + +disp('done'); + +return; + +go_calib_optim; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m new file mode 100755 index 0000000..f0fd4ca --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib.m @@ -0,0 +1,230 @@ + % Cleaned-up version of init_calib.m + + fprintf(1,'\nProcessing image %d...\n',kk); + + eval(['I = I_' num2str(kk) ';']); + + if exist(['wintx_' num2str(kk)]), + + eval(['wintxkk = wintx_' num2str(kk) ';']); + + if ~isempty(wintxkk) & ~isnan(wintxkk), + + eval(['wintx = wintx_' num2str(kk) ';']); + eval(['winty = winty_' num2str(kk) ';']); + + end; + end; + + + fprintf(1,'Using (wintx,winty)=(%d,%d) - Window size = %dx%d\n',wintx,winty,2*wintx+1,2*winty+1); + + + figure(2); + image(I); + colormap(map); + + title(['Click on the four extreme corners of the rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the four extreme corners of the rectangular complete pattern...'); + + [x,y] = ginput3(4); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + if (x(2) > x(1)), + x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2); + else + x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1); + end; + if (x(3) > x(4)), + x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4); + else + x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3); + end; + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','g','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','g','Fontsize',14); + hold off; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planar collineation: (return the normalization matrix as well) + + [Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + + + + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + image(I); colormap(map); hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % Saves all the data into variables: + + eval(['dX_' num2str(kk) ' = dX;']); + eval(['dY_' num2str(kk) ' = dY;']); + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) ' = x;']); + eval(['X_' num2str(kk) ' = X;']); + + eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); + eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); + \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m new file mode 100755 index 0000000..7718268 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/click_ima_calib3D.m @@ -0,0 +1,482 @@ + % Cleaned-up version of init_calib.m + + eval(['I = I_' num2str(kk) ';']); + + figure(2); + image(I); + colormap(map); + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% LEFT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + + title(['Click on the four extreme corners of the left rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the four extreme corners of the left rectangular pattern...'); + + [x,y] = ginput3(4); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + if (x(2) > x(1)), + x4 = x(1);y4 = y(1); x3 = x(2); y3 = y(2); + else + x4 = x(2);y4 = y(2); x3 = x(1); y3 = y(1); + end; + if (x(3) > x(4)), + x2 = x(3);y2 = y(3); x1 = x(4); y1 = y(4); + else + x2 = x(4);y2 = y(4); x1 = x(3); y1 = y(3); + end; + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'g-'); + plot(x,y,'og'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','g','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','g','Fontsize',14); + hold off; + + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + image(I); colormap(map); hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The left pannel info: + + xl = x; + Xl = X; + nl_sq_x = n_sq_x; + nl_sq_y = n_sq_y; + Hl = H_total; + + + + + + + %%%%%%%%%%%%%%%%%%%%%%%%% RIGHT PATTERN ACQUISITION %%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + + x1 = a10(1)/a10(3); + x4 = a11(1)/a11(3); + + y1 = a10(2)/a10(3); + y4 = a11(2)/a11(3); + + + figure(2); + hold on; + plot([x1 x4],[y1 y4],'c-'); + plot([x1 x4],[y1 y4],'co'); + hold off; + + title(['Click on the two remaining extreme corners of the right rectangular pattern... Image ' num2str(kk)]); + + disp('Click on the two remaining extreme corners of the right rectangular pattern...'); + + [x,y] = ginput3(2); + + [Xc,good,bad,type] = cornerfinder([x';y'],I,winty,wintx); % the four corners + + x = Xc(1,:)'; + y = Xc(2,:)'; + + [y,indy] = sort(y); + x = x(indy); + + x2 = x(2); + x3 = x(1); + + y2 = y(2); + y3 = y(1); + + + x = [x1;x2;x3;x4]; + y = [y1;y2;y3;y4]; + + figure(2); hold on; + plot([x;x(1)],[y;y(1)],'c-'); + plot(x,y,'oc'); + hx=text((x(4)+x(3))/2,(y(4)+y(3))/2 - 20,'X'); + set(hx,'color','c','Fontsize',14); + hy=text((x(4)+x(1))/2-20,(y(4)+y(1))/2,'Y'); + set(hy,'color','c','Fontsize',14); + hold off; + drawnow; + + + % Try to automatically count the number of squares in the grid + + n_sq_x1 = count_squares(I,x1,y1,x2,y2,wintx); + n_sq_x2 = count_squares(I,x3,y3,x4,y4,wintx); + n_sq_y1 = count_squares(I,x2,y2,x3,y3,wintx); + n_sq_y2 = count_squares(I,x4,y4,x1,y1,wintx); + + + + % If could not count the number of squares, enter manually + + if (n_sq_x1~=n_sq_x2)|(n_sq_y1~=n_sq_y2), + + + disp('Could not count the number of squares in the grid. Enter manually.'); + n_sq_x = input('Number of squares along the X direction ([]=10) = '); %6 + if isempty(n_sq_x), n_sq_x = 10; end; + n_sq_y = input('Number of squares along the Y direction ([]=10) = '); %6 + if isempty(n_sq_y), n_sq_y = 10; end; + + else + + n_sq_x = n_sq_x1; + n_sq_y = n_sq_y1; + + end; + + + if 1, + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'cm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'cm) = ']); + if isempty(dX), dX = dX_default; else dX_default = dX; end; + if isempty(dY), dY = dY_default; else dY_default = dY; end; + + else + + dX = 3; + dY = 3; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if 1, + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + %save all_corners x y grid_pts + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + + % Global Homography from plane to pixel coordinates: + + H_total = [1 0 -1 ; 0 1 -1 ; 0 0 1]*Homo*[1 0 0;0 -1 1;0 0 1]*[1/W 0 0 ; 0 1/L 0; 0 0 1]; + % WARNING!!! the first matrix (on the left side) takes care of the transformation of the pixel cooredinates by -1 (previous line) + % If it is not done, then this matrix should not appear (in C) + H_total = H_total / H_total(3,3); + + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_ori