From d17b33131c14864bd1eae275f49a3f148e21cf29 Mon Sep 17 00:00:00 2001 From: Leo Chan Date: Thu, 22 Oct 2020 01:53:21 -0400 Subject: Squashed commit of the sb-vbs branch. Includes the SD-VBS benchmarks modified to: - Use libextra to loop as realtime jobs - Preallocate memory before starting their main computation - Accept input via stdin instead of via argc Does not include the SD-VBS matlab code. Fixes libextra execution in LITMUS^RT. --- .../toolbox_basic/calib_bouguetj/Distor2Calib.m | 391 +++++++++++++++++ .../calib_bouguetj/Multi_Calib_oulu.m | 12 + .../calib_bouguetj/Rectangle2Square.m | 19 + .../toolbox_basic/calib_bouguetj/UnWarpPlane.m | 54 +++ .../toolbox_basic/calib_bouguetj/add_suppress.m | 91 ++++ .../toolbox_basic/calib_bouguetj/analyse_error.m | 104 +++++ .../toolbox/toolbox_basic/calib_bouguetj/calib.m | 74 ++++ .../toolbox_basic/calib_bouguetj/calib3D_gui.m | 115 +++++ .../toolbox_basic/calib_bouguetj/calib_gui.m | 81 ++++ .../calib_bouguetj/check_active_images.m | 14 + .../calib_bouguetj/check_convergence.m | 17 + .../toolbox_basic/calib_bouguetj/check_planarity.m | 41 ++ .../toolbox_basic/calib_bouguetj/click_calib.m | 99 +++++ .../toolbox_basic/calib_bouguetj/click_calib3D.m | 79 ++++ .../toolbox_basic/calib_bouguetj/click_ima_calib.m | 218 ++++++++++ .../calib_bouguetj/click_ima_calib3D.m | 482 +++++++++++++++++++++ .../toolbox_basic/calib_bouguetj/comp_distortion.m | 38 ++ .../calib_bouguetj/comp_distortion2.m | 71 +++ .../calib_bouguetj/comp_distortion_oulu.m | 47 ++ .../calib_bouguetj/comp_error_calib.m | 40 ++ .../calib_bouguetj/compute_collineation.m | 66 +++ .../calib_bouguetj/compute_extrinsic.m | 123 ++++++ .../calib_bouguetj/compute_extrinsic_init.m | 149 +++++++ .../calib_bouguetj/compute_extrinsic_refine.m | 110 +++++ .../calib_bouguetj/compute_homography.m | 163 +++++++ .../toolbox_basic/calib_bouguetj/convert_oulu.m | 35 ++ .../toolbox_basic/calib_bouguetj/cornerfinder.m | 215 +++++++++ .../toolbox_basic/calib_bouguetj/count_squares.m | 74 ++++ .../toolbox_basic/calib_bouguetj/data_calib.m | 89 ++++ .../toolbox_basic/calib_bouguetj/error_analysis.m | 182 ++++++++ .../toolbox_basic/calib_bouguetj/ext_calib.m | 130 ++++++ .../toolbox_basic/calib_bouguetj/extract_grid.m | 227 ++++++++++ .../calib_bouguetj/extract_parameters.m | 46 ++ .../calib_bouguetj/extract_parameters3D.m | 36 ++ .../calib_bouguetj/extrinsic_computation.m | 173 ++++++++ .../toolbox/toolbox_basic/calib_bouguetj/ginput3.m | 216 +++++++++ .../toolbox_basic/calib_bouguetj/go_calib_optim.m | 60 +++ .../calib_bouguetj/go_calib_optim3D.m | 264 +++++++++++ .../calib_bouguetj/go_calib_optim_cont.m | 142 ++++++ .../calib_bouguetj/go_calib_optim_iter.m | 332 ++++++++++++++ .../toolbox_basic/calib_bouguetj/graphout_calib.m | 12 + .../calib_bouguetj/graphout_calib3D.m | 153 +++++++ .../toolbox_basic/calib_bouguetj/ima_read_calib.m | 107 +++++ .../calib_bouguetj/init_calib_param.m | 210 +++++++++ .../calib_bouguetj/init_intrinsic_param.m | 153 +++++++ .../toolbox/toolbox_basic/calib_bouguetj/is3D.m | 19 + .../toolbox_basic/calib_bouguetj/loading_calib.m | 10 + .../toolbox/toolbox_basic/calib_bouguetj/loadinr.m | 52 +++ .../toolbox/toolbox_basic/calib_bouguetj/loadpgm.m | 89 ++++ .../toolbox/toolbox_basic/calib_bouguetj/loadppm.m | 101 +++++ .../toolbox_basic/calib_bouguetj/mean_std_robust.m | 7 + .../calib_bouguetj/multi_error_oulu.m | 49 +++ .../toolbox_basic/calib_bouguetj/normalize.m | 32 ++ .../toolbox/toolbox_basic/calib_bouguetj/pgmread.m | 26 ++ .../toolbox_basic/calib_bouguetj/project2_oulu.m | 53 +++ .../toolbox_basic/calib_bouguetj/project_points.m | 276 ++++++++++++ .../toolbox_basic/calib_bouguetj/projectedGrid.m | 24 + .../toolbox/toolbox_basic/calib_bouguetj/readras.m | 87 ++++ .../calib_bouguetj/recomp_corner_calib.m | 96 ++++ .../toolbox/toolbox_basic/calib_bouguetj/rect.m | 93 ++++ .../toolbox_basic/calib_bouguetj/reproject_calib.m | 92 ++++ .../toolbox_basic/calib_bouguetj/rigid_motion.m | 66 +++ .../toolbox_basic/calib_bouguetj/rodrigues.m | 217 ++++++++++ .../toolbox_basic/calib_bouguetj/rotation.m | 23 + .../calib_bouguetj/run_error_analysis.m | 65 +++ .../toolbox/toolbox_basic/calib_bouguetj/saveinr.m | 46 ++ .../toolbox/toolbox_basic/calib_bouguetj/savepgm.m | 22 + .../toolbox/toolbox_basic/calib_bouguetj/saveppm.m | 25 ++ .../toolbox_basic/calib_bouguetj/saving_calib.m | 27 ++ .../calib_bouguetj/script_fit_distortion.m | 39 ++ .../calib_bouguetj/select_sol_no_center.m | 19 + .../calib_bouguetj/select_sol_no_center3D.m | 20 + .../calib_bouguetj/select_sol_with_center.m | 19 + .../calib_bouguetj/select_sol_with_center3D.m | 20 + .../toolbox/toolbox_basic/calib_bouguetj/startup.m | 9 + .../toolbox/toolbox_basic/calib_bouguetj/test_3d.m | 80 ++++ .../toolbox_basic/calib_bouguetj/undistort_image.m | 88 ++++ .../toolbox_basic/calib_bouguetj/writeras.m | 105 +++++ 78 files changed, 7550 insertions(+) create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Distor2Calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Multi_Calib_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Rectangle2Square.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/UnWarpPlane.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/add_suppress.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/analyse_error.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion2.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_distortion_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/comp_error_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_collineation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_init.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_extrinsic_refine.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/compute_homography.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/convert_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/cornerfinder.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/count_squares.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/data_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/error_analysis.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ext_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_grid.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extract_parameters3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/extrinsic_computation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ginput3.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_cont.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/go_calib_optim_iter.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/graphout_calib3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/ima_read_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_calib_param.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/init_intrinsic_param.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/is3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loading_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadinr.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadpgm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/loadppm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/mean_std_robust.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/multi_error_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/normalize.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/pgmread.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/project2_oulu.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/project_points.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/projectedGrid.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/readras.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/recomp_corner_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rect.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/reproject_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rigid_motion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rodrigues.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/rotation.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/run_error_analysis.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saveinr.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/savepgm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saveppm.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/saving_calib.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/script_fit_distortion.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_no_center3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/select_sol_with_center3D.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/startup.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/test_3d.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/undistort_image.m create mode 100755 SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/writeras.m (limited to 'SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj') diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Distor2Calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Distor2Calib.m new file mode 100755 index 0000000..a82f583 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/Multi_Calib_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Multi_Calib_oulu.m new file mode 100755 index 0000000..62ca9ae --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Multi_Calib_oulu.m @@ -0,0 +1,12 @@ + +% enter image names, numbers, ... +data_calib; + +%read images from files +ima_read_calib; + +click_calib; + +%go_calib; % the original version + +go_calib_optim; diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Rectangle2Square.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/Rectangle2Square.m new file mode 100755 index 0000000..a6bbbe5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/UnWarpPlane.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/UnWarpPlane.m new file mode 100755 index 0000000..8addf52 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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/calib_bouguetj/add_suppress.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/add_suppress.m new file mode 100755 index 0000000..b9bcc57 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/add_suppress.m @@ -0,0 +1,91 @@ + 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/calib_bouguetj/analyse_error.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/analyse_error.m new file mode 100755 index 0000000..5bfa3b5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/analyse_error.m @@ -0,0 +1,104 @@ +% Color code for each image: + +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 active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; +end; +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]\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; + + +end; + +end; + +disp('done'); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m new file mode 100755 index 0000000..5b0fdac --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib.m @@ -0,0 +1,74 @@ +if ~exist('instructions'), instructions = 1; end; + +if instructions, + +fprintf(1,'\n'); +fprintf(1,'*----------------------------------------------------------------------------------------------------*\n'); +fprintf(1,'| Main Calibration toolbox (2D and 3D rigs) |\n'); +fprintf(1,'| (c) Jean-Yves Bouguet - September 9th, 1999 |\n'); +fprintf(1,'*----------------------------------------------------------------------------------------------------*\n\n\n'); + +fprintf(1,'CLICK ON:\n\n'); + +fprintf(1,'2D: To perform camera calibration from multiple views of a 2D planar grid. \n'); +fprintf(1,' Set default size of grid (in dX_default and dY_default) in click_calib.m.\n'); +fprintf(1,'3D: To perform camera calibration from multiple views of a 3D grid corner. \n'); +fprintf(1,' Set default size of grids (in dX_default and dY_default) in click_calib3D.m.\n'); +fprintf(1,'Exit: To close the calibration tool. \n'); + +end; + +instructions = 0; + +fig_number = 1; + +n_row = 1; +n_col = 3; + +string_list = cell(n_row,n_col); +callback_list = cell(n_row,n_col); + +x_size = 40; +y_size = 20; + +title_figure = 'Calibration tool'; + +string_list{1,1} = '2D rig'; +string_list{1,2} = '3D rig'; +string_list{1,3} = 'Exit'; + +callback_list{1,1} = 'calib_gui;'; +callback_list{1,2} = 'calib3D_gui;'; +callback_list{1,3} = ['disp(''Bye. To run again, type calib.''); close(' num2str(fig_number) ');']; + + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +fig_size_x = x_size*n_col+(n_col+1)*2; +fig_size_y = y_size*n_row+(n_row+1)*2; + +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'); + + +for i=n_row:-1:1, + for j = n_col:-1:1, + if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})), + uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',callback_list{i,j}, ... + 'ListboxTop',0, ... + 'Position',[(2+(j-1)*(x_size+2)) (fig_size_y - i*(2+y_size)) x_size y_size], ... + 'String',string_list{i,j}, ... + 'Tag','Pushbutton1'); + end; + end; +end; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m new file mode 100755 index 0000000..ff24f6b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib3D_gui.m @@ -0,0 +1,115 @@ +if ~exist('instructions3D'), instructions3D = 1; end; + +if instructions3D, + +fprintf(1,'\n'); +fprintf(1,'*----------------------------------------------------------------------------------------------------*\n'); +fprintf(1,'| Canera calibration from multiple images of the Intel 3D calibration rig |\n'); +fprintf(1,'| (c) Jean-Yves Bouguet - September 2nd, 1999 |\n'); +fprintf(1,'*----------------------------------------------------------------------------------------------------*\n\n\n'); + +fprintf(1,'LIST OF CALIBRATION COMMANDS (to be executed from 1 to 5):\n\n'); + +fprintf(1,'1- Image names: Lets the user enter the file names of the calibration images (max = 30 images).\n'); +fprintf(1,' It includes basename, image type (''tif'', ''bmp'' or ''ras''), numbering scheme.\n'); +fprintf(1,' Automatically launchs the next step (Read images).\n'); +fprintf(1,'2- Read images: Reads in the calibration images from files.\n'); +fprintf(1,' Does not automatically launch the next step (Extract grid corners).\n'); +fprintf(1,'3- Extract grid corners: Extracts the grid corners from the image.\n'); +fprintf(1,' Based six maual clicks per image.\n'); +fprintf(1,' The calibration data is saved under ''calib_data.mat''.\n'); +fprintf(1,' Automatically launchs the next step (Run calibration).\n'); +fprintf(1,'4- Run calibration: Main calibration procedure.\n'); +fprintf(1,' Optimization of intrinsic and extrinsic parameters to minimize\n'); +fprintf(1,' the reprojection error (in the least squares sense.\n'); +fprintf(1,' Estimated parameters: 2 focal lengths, principal point,\n'); +fprintf(1,' radial (2 coeff. -> 4 degree model) and tangential (2 coeff.) distortion,\n'); +fprintf(1,' and extrinsic parameters (6 parameters per image).\n'); +fprintf(1,' The final solution is saved under ''Calib_Results.mat''.\n'); +fprintf(1,' For a description of the intrinsic camera model, refer to the reference:\n'); +fprintf(1,' "A Four-step Camera Calibration Procedure with implicit Image Correction"\n'); +fprintf(1,' Janne Heikkila and Olli Silven, Infotech Oulu and Department of EE\n'); +fprintf(1,' University of Oulu, Appeared in CVPR''97, Puerto Rico.\n'); +fprintf(1,' Visit http://www.ee.oulu.fi/~jth/calibr/Calibration.html\n'); +fprintf(1,' Automatically launchs the next step (Graphic out).\n'); +fprintf(1,'5- Graphic out: Generates the graphical output associated to the current calibration solution.\n'); +fprintf(1,' It shows the 3D locations of the grids, and reprojects the 3D patterns on the\n'); +fprintf(1,' original calibration images.\n'); +fprintf(1,'6- sol. with center: Lets the user select the calibration solution with computed principal point.\n'); +fprintf(1,' This is the default case (solution retained after Run calibration).\n'); +fprintf(1,' Automatically (re)generates the graphical output associated to that solution.\n'); +fprintf(1,'7- sol. without center: Lets the users select the calibration solution without computed principal point.\n'); +fprintf(1,' In that case, the principal point is assumed at the center of the image.\n'); +fprintf(1,' Automatically generates the graphical output associated to that solution.\n'); +fprintf(1,' This option is sometimes useful when the principal point is difficult to\n'); +fprintf(1,' estimate (in particular when the camera field of view is small).\n'); +fprintf(1,'8- Back to main: Goes back to the main calbration toolbox window.\n\n\n'); + +end; + +instructions3D = 0; + +global X_1 x_1 X_2 x_2 X_3 x_3 X_4 x_4 X_5 x_5 X_6 x_6 X_7 x_7 X_8 x_8 X_9 x_9 X_10 x_10 X_11 x_11 X_12 x_12 X_13 x_13 X_14 x_14 X_15 x_15 X_16 x_16 X_17 x_17 X_18 x_18 X_19 x_19 X_20 x_20 X_21 x_21 X_22 x_22 X_23 x_23 X_24 x_24 X_25 x_25 X_26 x_26 X_27 x_27 X_28 x_28 X_29 x_29 X_30 x_30 + + +fig_number = 1; + +n_row = 2; +n_col = 4; + +string_list = cell(n_row,n_col); +callback_list = cell(n_row,n_col); + +x_size = 85; +y_size = 20; + +title_figure = 'Camera calibration tool (3D rig)'; + +string_list{1,1} = 'Image names'; +string_list{1,2} = 'Read images'; +string_list{1,3} = 'Extract grid corners'; +string_list{1,4} = 'Run calibration'; +string_list{2,1} = 'Graphic out'; +string_list{2,2} = 'sol. with center'; +string_list{2,3} = 'sol. without center'; +string_list{2,4} = 'Back to main'; + +callback_list{1,1} = 'data_calib;'; +callback_list{1,2} = 'ima_read_calib;'; +callback_list{1,3} = 'click_calib3D;'; +callback_list{1,4} = 'go_calib_optim3D;'; +callback_list{2,1} = 'graphout_calib3D;'; +callback_list{2,2} = 'select_sol_with_center3D;'; +callback_list{2,3} = 'select_sol_no_center3D;'; +callback_list{2,4} = 'calib;'; + + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +fig_size_x = x_size*n_col+(n_col+1)*2; +fig_size_y = y_size*n_row+(n_row+1)*2; + +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'); + + +for i=n_row:-1:1, + for j = n_col:-1:1, + if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})), + uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',callback_list{i,j}, ... + 'ListboxTop',0, ... + 'Position',[(2+(j-1)*(x_size+2)) (fig_size_y - i*(2+y_size)) x_size y_size], ... + 'String',string_list{i,j}, ... + 'Tag','Pushbutton1'); + end; + end; +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m new file mode 100755 index 0000000..62a45dd --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/calib_gui.m @@ -0,0 +1,81 @@ +fig_number = 1; + +n_row = 5; +n_col = 4; + +string_list = cell(n_row,n_col); +callback_list = cell(n_row,n_col); + +x_size = 85; +y_size = 20; + +title_figure = 'Camera calibration tool (2D rig)'; + +string_list{1,1} = 'Image names'; +string_list{1,2} = 'Read images'; +string_list{1,3} = 'Extract grid corners'; +%string_list{1,4} = 'Initialization'; +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{5,1} = 'Comp. Extrinsic'; +string_list{5,2} = 'Undistort image'; + + +callback_list{1,1} = 'data_calib;'; +callback_list{1,2} = 'ima_read_calib;'; +callback_list{1,3} = 'click_calib;'; +%callback_list{1,4} = 'init_calib_param;'; +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{5,1} = 'extrinsic_computation;'; +callback_list{5,2} = 'undistort_image;'; + + +figure(fig_number); clf; +pos = get(fig_number,'Position'); + +fig_size_x = x_size*n_col+(n_col+1)*2; +fig_size_y = y_size*n_row+(n_row+1)*2; + +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) ');']); + + +for i=n_row:-1:1, + for j = n_col:-1:1, + if (~isempty(callback_list{i,j}))&(~isempty(string_list{i,j})), + uicontrol('Parent',fig_number, ... + 'Units','points', ... + 'Callback',callback_list{i,j}, ... + 'ListboxTop',0, ... + 'Position',[(2+(j-1)*(x_size+2)) (fig_size_y - i*(2+y_size)) x_size y_size], ... + 'String',string_list{i,j}, ... + 'Tag','Pushbutton1'); + end; + end; +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 diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m new file mode 100755 index 0000000..4f09b62 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_active_images.m @@ -0,0 +1,14 @@ + +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); diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m new file mode 100755 index 0000000..8602c39 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_convergence.m @@ -0,0 +1,17 @@ +%%% Replay the set of solution vectors: + +N_iter = size(param_list,2); + +for nn = 1:N_iter, + + solution = param_list(:,nn); + + extract_parameters; + comp_error_calib; + + ext_calib; + + drawnow; + + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m new file mode 100755 index 0000000..be0410b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/check_planarity.m @@ -0,0 +1,41 @@ +% Check the planarity of a structure: + +X = X_1; +N = size(X,2); + +%X(3,:) = 0.1*randn(1,N); + +om = rand(3,1); +T = 10*rand(3,1); +R = rodrigues(om); + +X = R * X + T*ones(1,N); + + + + + + +N = size(X,2); +X_mean = mean(X')'; + +Y = X - (X_mean*ones(1,N)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +% if r is less than 1e-4: + +R_transform = V'; +T_transform = -(V')*X_mean; + + +% Thresh for r: 1e-4 + +X_new = R_transform*X + T_transform*ones(1,N); + + +% If Xc = Rc * X_new + Tc, then Xc = Rc * R_transform * X + Tc + T_transform diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m new file mode 100755 index 0000000..047cc7b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib.m @@ -0,0 +1,99 @@ + +if ~exist('I_1'), + ima_read_calib; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +check_active_images; + +%wintx = 10; % neigborhood of integration for +%winty = 10; % the corner finder + +fprintf(1,'\nExtraction of the grid corners on the images\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +if ~exist('map'), map = gray(256); end; + + +disp('WARNING!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + + +% 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; + +% Useful option to add images: +kk_first = input('Start image number ([]=1=first): '); + +if isempty(kk_first), kk_first = 1; end; + +for kk = kk_first:n_ima, + if active_images(kk), + click_ima_calib; + 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; + + + +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/calib_bouguetj/click_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib3D.m new file mode 100755 index 0000000..e761cd1 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_calib3D.m @@ -0,0 +1,79 @@ + +if ~exist('I_1'), + ima_read_calib; + if no_image_file, + 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'); + +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); + + +disp('WARNNG!!! Do not forget to change dX_default and dY_default in click_calib.m!!!') + + +% 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; + +% Useful option to add images: +kk_first = input('Start image number ([]=1=first): '); + +if isempty(kk_first), kk_first = 1; end; + +for kk = kk_first:n_ima, + click_ima_calib3D; %Simple version + %init_calib; %advanced vesion (more messy) +end; + + + +string_save = 'save calib_data 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) ' Hl_' num2str(kk) ' nl_sq_x_' num2str(kk) ' nl_sq_y_' num2str(kk) ' Hr_' num2str(kk) ' nr_sq_x_' num2str(kk) ' nr_sq_y_' num2str(kk)]; +end; + +eval(string_save); + +go_calib_optim3D; diff --git a/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m new file mode 100755 index 0000000..5197870 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib.m @@ -0,0 +1,218 @@ + % Cleaned-up version of init_calib.m + + fprintf(1,'\nProcessing image %d...\n',kk); + + eval(['I = I_' num2str(kk) ';']); + + 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) '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; + + % 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) + + + % Global Homography from plane to pixel coordinates: + + + + + 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/calib_bouguetj/click_ima_calib3D.m b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/click_ima_calib3D.m new file mode 100755 index 0000000..7718268 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/calib_bouguetj/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