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_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-15,yorig-15,'O'); + set(h,'Color','m','FontSize',14); + h2 = text(dxpos(1)-10,dxpos(2)-10,'dX'); + set(h2,'Color','g','FontSize',14); + h3 = text(dypos(1)-25,dypos(2)-3,'dY'); + set(h3,'Color','g','FontSize',14); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + title('Extracted corners'); + zoom on; + drawnow; + hold off; + + + Xi = reshape(([0:n_sq_x]*dX)'*ones(1,n_sq_y+1),Np,1)'; + Yi = reshape(ones(n_sq_x+1,1)*[n_sq_y:-1:0]*dY,Np,1)'; + Zi = zeros(1,Np); + + Xgrid = [Xi;Yi;Zi]; + + + % All the point coordinates (on the image, and in 3D) - for global optimization: + + x = grid_pts; + X = Xgrid; + + + % The right pannel info: + + xr = x; + Xr = X; + nr_sq_x = n_sq_x; + nr_sq_y = n_sq_y; + Hr = H_total; + + + +%%%%%%%% REGROUP THE LEFT AND RIHT PATTERNS %%%%%%%%%%%%% + + +Xr2 = [0 0 1;0 1 0;-1 0 0]*Xr + [dX*nl_sq_x;0;0]*ones(1,length(Xr)); + + +x = [xl xr]; + +X = [Xl Xr2]; + + + + eval(['x_' num2str(kk) ' = x;']); + eval(['X_' num2str(kk) ' = X;']); + + eval(['nl_sq_x_' num2str(kk) ' = nl_sq_x;']); + eval(['nl_sq_y_' num2str(kk) ' = nl_sq_y;']); + + eval(['nr_sq_x_' num2str(kk) ' = nr_sq_x;']); + eval(['nr_sq_y_' num2str(kk) ' = nr_sq_y;']); + + % Save the global planar homography: + + eval(['Hl_' num2str(kk) ' = Hl;']); + eval(['Hr_' num2str(kk) ' = Hr;']); \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m new file mode 100755 index 0000000..a0f03de --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion.m @@ -0,0 +1,38 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% (do it up to a second order approximation) + +[two,N] = size(x_dist); + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + +if length(k2) > 2, + [x_comp] = comp_distortion_oulu(x_dist,k2); +else + +radius_2= x_dist(1,:).^2 + x_dist(2,:).^2; +radial_distortion = 1 + ones(2,1)*(k2 * radius_2); +radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:); +radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp); +x_comp = x_dist ./ radial_distortion; + +end; + +%% Function completely checked : It works fine !!! \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m new file mode 100755 index 0000000..532ee9a --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion2.m @@ -0,0 +1,71 @@ +function [x_comp] = comp_distortion(x_dist,k2); + +% [x_comp] = comp_distortion(x_dist,k2); +% +% compensates the radial distortion of the camera +% on the image plane. +% +% x_dist : the image points got without considering the +% radial distortion. +% k2: Radial distortion factor +% +% x : The image plane points after correction for the distortion +% +% x and x_dist are 2xN arrays +% +% NOTE : This compensation has to be done after the substraction +% of the center of projection, and division by the focal +% length. +% +% Solve for cubic roots using method from Numerical Recipes in C 2nd Ed. +% pages 184-185. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet - April 27th, 1998 + +% fully checked! JYB, april 27th, 1998 - 2am + +if k2 ~= 0, + +[two,N] = size(x_dist); + +if (two ~= 2 ), + error('ERROR : The dimension of the points should be 2xN'); +end; + + +ph = atan2(x_dist(2,:),x_dist(1,:)); + +Q = -1/(3*k2); +R = -x_dist(1,:)./(2*k2*cos(ph)); + +R2 = R.^2; +Q3 = Q^3; + + +if k2 < 0, + + % this works in all practical situations (it starts failing for very large + % values of k2) + + th = acos(R./sqrt(Q3)); + r = -2*sqrt(Q)*cos((th-2*pi)/3); + +else + + % note: this always works, even for ridiculous values of k2 + + A = (sqrt(R2-Q3)-R).^(1/3); + B = Q*(1./A); + r = (A+B); + +end; + +x_comp = [r.*cos(ph); r.*sin(ph)]; + +else + + x_comp = x_dist; + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m new file mode 100755 index 0000000..67d02d5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_distortion_oulu.m @@ -0,0 +1,47 @@ +function [x] = comp_distortion_oulu(xd,k); + +%comp_distortion_oulu.m +% +%[x] = comp_distortion_oulu(xd,k) +% +%Compensates for radial and tangential distortion. Model From Oulu university. +%For more informatino about the distortion model, check the forward projection mapping function: +%project_points.m +% +%INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) +% +%Method: Iterative method for compensation. +% +%NOTE: This compensation has to be done after the subtraction +% of the principal point, and division by the focal length. + + +if length(k) < 4, + + [x] = comp_distortion(xd,k); + +else + + + k1 = k(1); + k2 = k(2); + p1 = k(3); + p2 = k(4); + + x = xd; % initial guess + + for kk=1:5; + + r_2 = sum(x.^2); + k_radial = 1 + k1 * r_2 + k2 * r_2.^2; + delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2) ; + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + x = (xd - delta_x)./(ones(2,1)*k_radial); + + end; + +end; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m new file mode 100755 index 0000000..c7bf662 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/comp_error_calib.m @@ -0,0 +1,46 @@ +%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% + +check_active_images; + +% Reproject the patterns on the images, and compute the pixel errors: + +ex = []; % Global error vector +x = []; % Detected corners on the image plane +y = []; % Reprojected points + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + + eval(['omckk = omc_' num2str(kk) ';']); + eval(['Tckk = Tc_' num2str(kk) ';']); + + if active_images(kk) & (~isnan(omckk(1,1))), + + %Rkk = rodrigues(omckk); + + eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); + + eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' -y_' num2str(kk) ';']); + + eval(['x_kk = x_' num2str(kk) ';']); + + eval(['ex = [ex ex_' num2str(kk) '];']); + eval(['x = [x x_' num2str(kk) '];']); + eval(['y = [y y_' num2str(kk) '];']); + + else + + % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + + + % If inactivated image, the error does not make sense: + eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); + + end; + +end; + +err_std = std(ex')'; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m new file mode 100755 index 0000000..809c309 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_collineation.m @@ -0,0 +1,66 @@ +function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); + +% new formalism using homographies + +a00 = a00 / a00(3); +a10 = a10 / a10(3); +a11 = a11 / a11(3); +a01 = a01 / a01(3); + + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = [a00(1);a10(1);a11(1);a01(1)]; +ay = [a00(2);a10(2);a11(2);a01(2)]; + +mxx = mean(ax); +myy = mean(ay); +ax = ax - mxx; +ay = ay - myy; + +scxx = mean(abs(ax)); +scyy = mean(abs(ay)); + + +Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; +inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; + + +a00n = Hnorm*a00; +a10n = Hnorm*a10; +a11n = Hnorm*a11; +a01n = Hnorm*a01; + + +% Computation of the vanishing points: + +V1n = cross(cross(a00n,a10n),cross(a01n,a11n)); +V2n = cross(cross(a00n,a01n),cross(a10n,a11n)); + +V1 = inv_Hnorm*V1n; +V2 = inv_Hnorm*V2n; + + +% Normalizaion of the vanishing points: + +V1n = V1n/norm(V1n); +V2n = V2n/norm(V2n); + + +% Closed-form solution of the coefficients: + +alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2)); + +alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2)); + + +% Remaining Homography + +Hrem = [alpha_x*V1n alpha_y*V2n a00n]; + + +% Final homography: + +H = inv_Hnorm*Hrem; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m new file mode 100755 index 0000000..5217351 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic.m @@ -0,0 +1,123 @@ +function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% H: Homography between points on the grid and points on the image plane (in pixel) +% This makes sense only if the planar that is used in planar. +% x: Reprojections of the points on the image plane +% ex: Reprojection error: ex = x_kk - x; +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 8, + thresh_cond = inf; +end; + + +if nargin < 7, + MaxIter = 20; +end; + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(4,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + +% Initialization: + +[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + +% Refinement: + +[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond); + + +% computation of the homography (not useful in the end) + +H = [Rckk(:,1:2) Tckk]; + +% Computes the reprojection error in pixels: + +x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + +ex = x_kk - x; + + +% Converts the homography in pixel units: + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1]; + +H = KK*H; + + + + +return; + + +% Test of compte extrinsic: + +Np = 4; +sx = 10; +sy = 10; +sz = 5; + +om = randn(3,1); +T = [0;0;100]; + +noise = 2/1000; + +XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)]; +xx = project_points(XX,om,T); + +xxn = xx + noise * randn(2,Np); + +[omckk,Tckk] = compute_extrinsic(xxn,XX); + +[om omckk om-omckk] +[T Tckk T-Tckk] + +figure(3); +plot(xx(1,:),xx(2,:),'r+'); +hold on; +plot(xxn(1,:),xxn(2,:),'g+'); +hold off; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m new file mode 100755 index 0000000..2e6d821 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_init.m @@ -0,0 +1,151 @@ +function [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + + +if nargin < 6, + alpha_c = 0; + if nargin < 5, + kc = zeros(4,1); + if nargin < 4, + cc = zeros(2,1); + if nargin < 3, + fc = ones(2,1); + if nargin < 2, + error('Need 2D projections and 3D points (in compute_extrinsic.m)'); + return; + end; + end; + end; + end; +end; + + +% Compute the normalized coordinates: + +xn = normalize(x_kk,fc,cc,kc,alpha_c); + + + +Np = size(xn,2); + +%% Check for planarity of the structure: + +X_mean = mean(X_kk')'; + +Y = X_kk - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +if (r < 1e-3)|(Np < 6), %1e-3, %1e-4, %norm(X_kk(3,:)) < eps, % Test of planarity + + %fprintf(1,'Planar structure detected: r=%f\n',r); + + % Transform the plane to bring it in the Z=0 plane: + + R_transform = V'; + + if det(R_transform) < 0, R_transform = -R_transform; end; + + T_transform = -(R_transform)*X_mean; + + X_new = R_transform*X_kk + T_transform*ones(1,Np); + + + % Compute the planar homography: + + H = compute_homography (xn,X_new(1:2,:)); + + % De-embed the motion parameters from the homography: + + sc = mean([norm(H(:,1));norm(H(:,2))]); + + H = H/sc; + + omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]); + Rckk = rodrigues(omckk); + Tckk = H(:,3); + + %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform + + Tckk = Tckk + Rckk* T_transform; + Rckk = Rckk * R_transform; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + + +else + + %fprintf(1,'Non planar structure detected: r=%f\n',r); + + % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!): + % The DLT method is applied here!! + + J = zeros(2*Np,12); + + xX = (ones(3,1)*xn(1,:)).*X_kk; + yX = (ones(3,1)*xn(2,:)).*X_kk; + + J(1:2:end,[1 4 7]) = -X_kk'; + J(2:2:end,[2 5 8]) = X_kk'; + J(1:2:end,[3 6 9]) = xX'; + J(2:2:end,[3 6 9]) = -yX'; + J(1:2:end,12) = xn(1,:)'; + J(2:2:end,12) = -xn(2,:)'; + J(1:2:end,10) = -ones(Np,1); + J(2:2:end,11) = ones(Np,1); + + JJ = J'*J; + [U,S,V] = svd(JJ); + + RR = reshape(V(1:9,12),3,3); + + if det(RR) < 0, + V(:,12) = -V(:,12); + RR = -RR; + end; + + [Ur,Sr,Vr] = svd(RR); + + Rckk = Ur*Vr'; + + sc = norm(V(1:9,12)) / norm(Rckk(:)); + Tckk = V(10:12,12)/sc; + + omckk = rodrigues(Rckk); + Rckk = rodrigues(omckk); + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m new file mode 100755 index 0000000..a4d066c --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_extrinsic_refine.m @@ -0,0 +1,113 @@ +function [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond), + +%compute_extrinsic +% +%[omckk,Tckk,Rckk] = compute_extrinsic_refine(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter) +% +%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection +%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc. +%Works with planar and non-planar structures. +% +%INPUT: x_kk: Feature locations on the images +% X_kk: Corresponding grid coordinates +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% MaxIter: Maximum number of iterations +% +%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space +% Tckk: 3D translation vector attached to the grid positions in space +% Rckk: 3D rotation matrices corresponding to the omc vectors + +% +%Method: Computes the normalized point coordinates, then computes the 3D pose +% +%Important functions called within that program: +% +%normalize: Computes the normalize image point coordinates. +% +%pose3D: Computes the 3D pose of the structure given the normalized image projection. +% +%project_points.m: Computes the 2D image projections of a set of 3D points + + +if nargin < 10, + thresh_cond = inf; +end; + + +if nargin < 9, + MaxIter = 20; +end; + +if nargin < 8, + alpha_c = 0; + if nargin < 7, + kc = zeros(4,1); + if nargin < 6, + cc = zeros(2,1); + if nargin < 5, + fc = ones(2,1); + if nargin < 4, + error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)'); + return; + end; + end; + end; + end; +end; + + +% Initialization: + +omckk = omc_init; +Tckk = Tc_init; + + +% Final optimization (minimize the reprojection error in pixel): +% through Gradient Descent: + +param = [omckk;Tckk]; + +change = 1; + +iter = 0; + +%keyboard; + +%fprintf(1,'Gradient descent iterations: '); + +while (change > 1e-10)&(iter < MaxIter), + + %fprintf(1,'%d...',iter+1); + + [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c); + + ex = x_kk - x; + + %keyboard; + + JJ = [dxdom dxdT]; + + if cond(JJ) > thresh_cond, + change = 0; + else + + JJ2 = JJ'*JJ; + + param_innov = inv(JJ2)*(JJ')*ex(:); + param_up = param + param_innov; + change = norm(param_innov)/norm(param_up); + param = param_up; + iter = iter + 1; + + omckk = param(1:3); + Tckk = param(4:6); + end; + +end; + +%fprintf(1,'\n'); + +Rckk = rodrigues(omckk); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m new file mode 100755 index 0000000..fcc9003 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/compute_homography.m @@ -0,0 +1,163 @@ +function [H,Hnorm,inv_Hnorm] = compute_homography (m,M); + +%compute_homography +% +%[H,Hnorm,inv_Hnorm] = compute_homography (m,M) +% +%Computes the planar homography between the point coordinates on the plane (M) and the image +%point coordinates (m). +% +%INPUT: m: homogeneous coordinates in the image plane (3xN matrix) +% M: homogeneous coordinates in the plane in 3D (3xN matrix) +% +%OUTPUT: H: Homography matrix (3x3 homogeneous matrix) +% Hnorm: Normlization matrix used on the points before homography computation +% (useful for numerical stability is points in pixel coordinates) +% inv_Hnorm: The inverse of Hnorm +% +%Definition: m ~ H*M where "~" means equal up to a non zero scalar factor. +% +%Method: First computes an initial guess for the homography through quasi-linear method. +% Then, if the total number of points is larger than 4, optimize the solution by minimizing +% the reprojection error (in the least squares sense). +% +% +%Important functions called within that program: +% +%comp_distortion_oulu: Undistorts pixel coordinates. +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +%project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian +% matrix (derivative with respect to the intrinsic and extrinsic parameters). +% This function is called within the minimization loop. + + + + +Np = size(m,2); + +if size(m,1)<3, + m = [m;ones(1,Np)]; +end; + +if size(M,1)<3, + M = [M;ones(1,Np)]; +end; + + +m = m ./ (ones(3,1)*m(3,:)); +M = M ./ (ones(3,1)*M(3,:)); + +% Prenormalization of point coordinates (very important): +% (Affine normalization) + +ax = m(1,:); +ay = m(2,:); + +mxx = mean(ax); +myy = mean(ay); +ax = ax - mxx; +ay = ay - myy; + +scxx = mean(abs(ax)); +scyy = mean(abs(ay)); + + +Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; +inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; + +mn = Hnorm*m; + +% Compute the homography between m and mn: + +% Build the matrix: + +L = zeros(2*Np,9); + +L(1:2:2*Np,1:3) = M'; +L(2:2:2*Np,4:6) = M'; +L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)'; +L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)'; + +if Np > 4, + L = L'*L; +end; + +[U,S,V] = svd(L); + +hh = V(:,9); +hh = hh/hh(9); + +Hrem = reshape(hh,3,3)'; +%Hrem = Hrem / Hrem(3,3); + +% Final homography: + +H = inv_Hnorm*Hrem; + + +%%% Homography refinement if there are more than 4 points: + +if Np > 4, + + % Final refinement: + + hhv = reshape(H',9,1); + hhv = hhv(1:8); + + for iter=1:10, + + mrep = H * M; + + J = zeros(2*Np,8); + + MMM = (M ./ (ones(3,1)*mrep(3,:))); + + J(1:2:2*Np,1:3) = -MMM'; + J(2:2:2*Np,4:6) = -MMM'; + + mrep = mrep ./ (ones(3,1)*mrep(3,:)); + + m_err = m(1:2,:) - mrep(1:2,:); + m_err = m_err(:); + + MMM2 = (ones(3,1)*mrep(1,:)) .* MMM; + MMM3 = (ones(3,1)*mrep(2,:)) .* MMM; + + J(1:2:2*Np,7:8) = MMM2(1:2,:)'; + J(2:2:2*Np,7:8) = MMM3(1:2,:)'; + + MMM = (M ./ (ones(3,1)*mrep(3,:)))'; + + hh_innov = inv(J'*J)*J'*m_err; + + hhv_up = hhv - hh_innov; + + H_up = reshape([hhv_up;1],3,3)'; + + %norm(m_err) + %norm(hh_innov) + + hhv = hhv_up; + H = H_up; + + end; + +end; + + + + + +return; + +%test of Jacobian + +mrep = H*M; +mrep = mrep ./ (ones(3,1)*mrep(3,:)); + +m_err = mrep(1:2,:) - m(1:2,:); +figure(8); +plot(m_err(1,:),m_err(2,:),'r+'); +std(m_err') diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m new file mode 100755 index 0000000..9bfa51f --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/cornerfinder.m @@ -0,0 +1,215 @@ +function [xc,good,bad,type] = cornerfinder(xt,I,wintx,winty,wx2,wy2); + +%[xc] = cornerfinder(xt,I); +% +%Finds the sub-pixel corners on the image I with initial guess xt +%xt and xc are 2xN matrices. The first component is the x coordinate +%(horizontal) and the second component is the y coordinate (vertical) +% +%Based on Harris corner finder method +% +%Finds corners to a precision below .1 pixel! +%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!! +%Sept 1998 - UPDATED to handle diverged points: we keep the original points +%good is a binary vector indicating wether a feature point has been properly +%found. +% +%Add a zero zone of size wx2,wy2 +%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher +%resolution and larger number of iterations. + + +% California Institute of Technology +% (c) Jean-Yves Bouguet -- Oct. 14th, 1997 + + + +line_feat = 1; % set to 1 to allow for extraction of line features. + +xt = xt'; +xt = fliplr(xt); + + +if nargin < 4, + winty = 5; + if nargin < 3, + wintx = 5; + end; +end; + + +if nargin < 6, + wx2 = -1; + wy2 = -1; +end; + + +%mask = ones(2*wintx+1,2*winty+1); +mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2); + + +if (wx2>0) & (wy2>0), + if ((wintx - wx2)>=2)&((winty - wy2)>=2), + mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1); + end; +end; + +offx = [-wintx:wintx]'*ones(1,2*winty+1); +offy = ones(2*wintx+1,1)*[-winty:winty]; + +resolution = 0.005; + +MaxIter = 10; + +[nx,ny] = size(I); +N = size(xt,1); + +xc = xt; % first guess... they don't move !!! + +type = zeros(1,N); + + +for i=1:N, + + v_extra = resolution + 1; % just larger than resolution + + compt = 0; % no iteration yet + + while (norm(v_extra) > resolution) & (compt 0, % the sub pixel + vIx = [itIx 1-itIx 0]'; % accuracy. + else + vIx = [0 1+itIx -itIx]'; + end; + if itIy > 0, + vIy = [itIy 1-itIy 0]; + else + vIy = [0 1+itIy -itIy]; + end; + + + % What if the sub image is not in? + + if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5; + elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4; + else + xmin = crIx-wintx-2; xmax = crIx+wintx+2; + end; + + if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5; + elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4; + else + ymin = crIy-winty-2; ymax = crIy+winty+2; + end; + + + SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood + SI = conv2(conv2(SI,vIx,'same'),vIy,'same'); + SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood + [gy,gx] = gradient(SI); % The gradient image + gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only + gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients + + px = cIx + offx; + py = cIy + offy; + + gxx = gx .* gx .* mask; + gyy = gy .* gy .* mask; + gxy = gx .* gy .* mask; + + + bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))]; + + a = sum(sum(gxx)); + b = sum(sum(gxy)); + c = sum(sum(gyy)); + + dt = a*c - b^2; + + xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; + + + %keyboard; + + if line_feat, + + G = [a b;b c]; + [U,S,V] = svd(G); + + %keyboard; + + % If non-invertible, then project the point onto the edge orthogonal: + + if (S(1,1)/S(2,2) > 50), + % projection operation: + xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)'; + type(i) = 1; + end; + + end; + + + %keyboard; + +% G = [a b;b c]; +% [U,S,V] = svd(G); + + +% if S(1,1)/S(2,2) > 150, +% bb2 = U'*bb; +% xc2 = (V*[bb2(1)/S(1,1) ;0])'; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + + %if (abs(a)> 50*abs(c)), +% xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)]; +% elseif (abs(c)> 50*abs(a)) +% xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt]; +% else +% xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt; +% end; + + %keyboard; + + v_extra = xc(i,:) - xc2; + + xc(i,:) = xc2; + +% keyboard; + + compt = compt + 1; + + end +end; + + +% check for points that diverge: + +delta_x = xc(:,1) - xt(:,1); +delta_y = xc(:,2) - xt(:,2); + +%keyboard; + + +bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty); +good = ~bad; +in_bad = find(bad); + +% For the diverged points, keep the original guesses: + +xc(in_bad,:) = xt(in_bad,:); + +xc = fliplr(xc); +xc = xc'; + +bad = bad'; +good = good'; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m new file mode 100755 index 0000000..0e226c0 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/count_squares.m @@ -0,0 +1,74 @@ +function ns = count_squares(I,x1,y1,x2,y2,win); + +%keyboard; + +[ny,nx] = size(I); + +lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1]; + +lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda; + +l1 = lambda + [0;0;win]; +l2 = lambda - [0;0;win]; + + +dx = x2-x1; +dy = y2 - y1; + + +if abs(dx) > abs(dy), + + if x2 > x1, + xs = x1:x2; + else + xs = x1:-1:x2; + end; + + ys = -(lambda(3) + lambda(1)*xs)/lambda(2); + +else + + if y2 > y1, + ys = y1:y2; + else + ys = y1:-1:y2; + end; + xs = -(lambda(3) + lambda(2)*ys)/lambda(1); + +end; + + + + Np = length(xs); + + xs_mat = ones(2*win + 1,1)*xs; + ys_mat = ones(2*win + 1,1)*ys; + + win_mat = (-win:win)'*ones(1,Np); + + + xs_mat2 = round(xs_mat - win_mat * lambda(1)); + ys_mat2 = round(ys_mat - win_mat * lambda(2)); + + ind_mat = (xs_mat2 - 1) * ny + ys_mat2; + + ima_patch = zeros(2*win + 1,Np); + + ima_patch(:) = I(ind_mat(:)); + + %ima2 = ima_patch(:,win+1:end-win); + + filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; + + out_f = sum(filtk.*ima_patch); + + out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); + + out_f_f = out_f_f(win+1:end-win); + + ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; + + + + +return; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m new file mode 100755 index 0000000..422769b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/data_calib.m @@ -0,0 +1,92 @@ +%%% This script alets the user enter the name of the images (base name, numbering scheme,... + + +% Checks that there are some images in the directory: + +l_ras = dir('*ras'); +s_ras = size(l_ras,1); +l_bmp = dir('*bmp'); +s_bmp = size(l_bmp,1); +l_tif = dir('*tif'); +s_tif = size(l_tif,1); +l_pgm = dir('*pgm'); +s_pgm = size(l_pgm,1); +l_jpg = dir('*jpg'); +s_jpg = size(l_jpg,1); + +s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg; + +if s_tot < 1, + fprintf(1,'No image in this directory in either ras, bmp, tif, pgm or jpg format. Change directory and try again.\n'); + break; +end; + + +% IF yes, display the directory content: + +dir; + +Nima_valid = 0; + +while (Nima_valid==0), + + fprintf(1,'\n'); + calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); + + format_image = '0'; + + while format_image == '0', + + format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image), + format_image = 'ras'; + end; + + if lower(format_image(1)) == 'm', + format_image = 'ppm'; + else + if lower(format_image(1)) == 'b', + format_image = 'bmp'; + else + if lower(format_image(1)) == 't', + format_image = 'tif'; + else + if lower(format_image(1)) == 'p', + format_image = 'pgm'; + else + if lower(format_image(1)) == 'j', + format_image = 'jpg'; + else + if lower(format_image(1)) == 'r', + format_image = 'ras'; + else + disp('Invalid image format'); + format_image = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; + end; + + + check_directory; + +end; + + + +%string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num'; + +%eval(string_save); + + + +if (Nima_valid~=0), + % Reading images: + + ima_read_calib; % may be launched from the toolbox itself +end; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m new file mode 100755 index 0000000..85feac5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/error_analysis.m @@ -0,0 +1,182 @@ +%%% ERROR_ANALYSIS +%%% This simulation helps coputing the acturacies of calibration +%%% Run it after the main calibration + + + +N_runs = 200; + +%N_ima_active = 4; + +saving = 1; + +if 1, %~exist('fc_list'), % initialization + + % Initialization: + + load Calib_Results; + check_active_images; + + fc_list = []; + cc_list = []; + kc_list = []; + active_images_list = []; + + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [];']); + eval(['Tc_list_' num2str(kk) ' = [];']); + + end; + + %sx = median(abs(ex(1,:)))*1.4836; + %sy = median(abs(ex(2,:)))*1.4836; + + sx = std(ex(1,:)); + sy = std(ex(2,:)); + + % Saving the feature locations: + + for kk = 1:n_ima, + + eval(['x_save_' num2str(kk) ' = x_' num2str(kk) ';']); + eval(['y_save_' num2str(kk) ' = y_' num2str(kk) ';']); + + end; + + active_images_save = active_images; + ind_active_save = ind_active; + + fc_save = fc; + cc_save = cc; + kc_save = kc; + KK_save = KK; + + +end; + + + + +%%% The main loop: + + +for ntrial = 1:N_runs, + + fprintf(1,'\nRun number: %d\n',ntrial); + fprintf(1, '----------\n'); + + for kk = 1:n_ima, + + eval(['y_kk = y_save_' num2str(kk) ';']) + + if active_images(kk) & ~isnan(y_kk(1,1)), + + Nkk = size(y_kk,2); + + x_kk_new = y_kk + [sx * randn(1,Nkk);sy*randn(1,Nkk)]; + + eval(['x_' num2str(kk) ' = x_kk_new;']); + + end; + + end; + + N_active = length(ind_active_save); + junk = randn(1,N_active); + [junk,junk2] = sort(junk); + + active_images = zeros(1,n_ima); + active_images(ind_active_save(junk2(1:N_ima_active))) = ones(1,N_ima_active); + + fc = fc_save; + cc = cc_save; + kc = kc_save; + KK = KK_save; + + go_calib_optim; + + fc_list = [fc_list fc]; + cc_list = [cc_list cc]; + kc_list = [kc_list kc]; + active_images_list = [active_images_list active_images']; + + for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + + end; + +end; + + + + +if 0, + +% Restoring the feature locations: + +for kk = 1:n_ima, + + eval(['x_' num2str(kk) ' = x_save_' num2str(kk) ';']); + +end; + +fprintf(1,'\nFinal run (with the real data)\n'); +fprintf(1, '------------------------------\n'); + +active_images = active_images_save; +ind_active = ind_active_save; + +go_calib_optim; + +fc_list = [fc_list fc]; +cc_list = [cc_list cc]; +kc_list = [kc_list kc]; +active_images_list = [active_images_list active_images']; + +for kk=1:n_ima, + + eval(['omc_list_' num2str(kk) ' = [ omc_list_' num2str(kk) ' omc_' num2str(kk) ' ];']); + eval(['Tc_list_' num2str(kk) ' = [ Tc_list_' num2str(kk) ' Tc_' num2str(kk) ' ];']); + +end; + +end; + + + + + +if saving, + +disp(['Save Calibration accuracy results under Calib_Accuracies_' num2str(N_ima_active) '.mat']); + +string_save = ['save Calib_Accuracies_' num2str(N_ima_active) ' active_images n_ima N_ima_active N_runs active_images_list fc cc kc fc_list cc_list kc_list']; + +for kk = 1:n_ima, + string_save = [string_save ' Tc_list_' num2str(kk) ' omc_list_' num2str(kk) ' Tc_' num2str(kk) ' omc_' num2str(kk) ]; +end; + +eval(string_save); + +end; + + +return; + +std(fc_list') + +std(cc_list') + +std(kc_list') + +for kk = 1:n_ima, + + eval(['std(Tc_list_' num2str(kk) ''')']) + +end; + + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m new file mode 100755 index 0000000..39506a8 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/export_calib_data.m @@ -0,0 +1,99 @@ +%% Export calibration data (corners + 3D coordinates) to +%% text files (in Willson-Heikkila's format or Zhang's format) + +%% Thanks to Michael Goesele (from the Max-Planck-Institut) for the original suggestion +%% of adding thsi export function to the toolbox. + + +if ~exist('n_ima'), + fprintf(1,'ERROR: No calibration data to export\n'); + +else + + check_active_images; + + check_extracted_images; + + check_active_images; + + fprintf(1,'Tool that exports calibration data to Willson-Heikkila or Zhang formats\n'); + + qformat = -1; + + while (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Two possible formats of export: 0=Willson and Heikkila, 1=Zhang\n') + qformat = input('Format of export (enter 0 or 1): '); + + if isempty(qformat) + qformat = -1; + end; + + if (qformat ~=0)&(qformat ~=1), + + fprintf(1,'Invalid entry. Try again.\n') + + end; + + end; + + if qformat == 0, + + fprintf(1,'\nExport of calibration data to text files (Willson and Heikkila''s format)\n'); + outputfile = input('File basename: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Xx = [X_kk ; x_kk]'; + + file_name = [outputfile num2str(kk)]; + + disp(['Exporting calibration data (3D world + 2D image coordinates) of image ' num2str(kk) ' to file ' file_name '...']); + + eval(['save ' file_name ' Xx -ASCII']); + + end; + + else + + fprintf(1,'\nExport of calibration data to text files (Zhang''s format)\n'); + modelfile = input('File basename for the 3D world coordinates: ','s'); + datafile = input('File basename for the 2D image coordinates: ','s'); + + for kk = ind_active, + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + if ~exist(['n_sq_x_' num2str(kk)]), + n_sq_x = 1; + n_sq_y = size(X_kk,2); + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + end; + + X = reshape(X_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + Y = reshape(X_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + XY = reshape([X;Y],n_sq_y+1,2*(n_sq_x+1)); + + x = reshape(x_kk(1,:)',n_sq_x+1,n_sq_y+1)'; + y = reshape(x_kk(2,:)',n_sq_x+1,n_sq_y+1)'; + xy = reshape([x;y],n_sq_y+1,2*(n_sq_x+1)); + + disp(['Exporting calibration data of image ' num2str(kk) ' to files ' modelfile num2str(kk) '.txt and ' datafile num2str(kk) '.txt...']); + + eval(['save ' modelfile num2str(kk) '.txt XY -ASCII']); + eval(['save ' datafile num2str(kk) '.txt xy -ASCII']); + + end; + + +end; + +fprintf(1,'done\n'); + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m new file mode 100755 index 0000000..04d6319 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ext_calib.m @@ -0,0 +1,152 @@ + +%%%%%%%%%%%%%%%%%%%% SHOW EXTRINSIC RESULTS %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist(['omc_' num2str(ind_active(1))]), + fprintf(1,'No calibration data available.\n'); + return; +end; + +%if ~exist('no_grid'), + no_grid = 0; +%end; + +if ~exist(['n_sq_x_' num2str(ind_active(1))]), + no_grid = 1; +end; + + +if 0, + +err_std = std(ex'); + +fprintf(1,'\n\nCalibration results without principal point estimation:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f]\n',cc); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f]\n',kc); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f]\n\n',err_std); + +end; + + +% Color code for each image: + +colors = 'brgkcm'; + + +%%% Show the extrinsic parameters + +if ~exist('dX'), + eval(['dX = norm(Tc_' num2str(ind_active(1)) ')/10;']); + dY = dX; +end; + +IP = 5*dX*([0 nx-1 nx-1 0 0 ; 0 0 ny-1 ny-1 0;1 1 1 1 1] - [cc;0]*ones(1,5)) ./ ([fc;1]*ones(1,5)); +BASE = 5*dX*([0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 1]); +IP = reshape([IP;BASE(:,1)*ones(1,5);IP],3,15); + +figure(4); +[a,b] = view; + +figure(4); +plot3(BASE(1,:),BASE(3,:),-BASE(2,:),'b-','linewidth',2'); +hold on; +plot3(IP(1,:),IP(3,:),-IP(2,:),'r-','linewidth',2); +text(6*dX,0,0,'X_c'); +text(-dX,5*dX,0,'Z_c'); +text(0,0,-6*dX,'Y_c'); +text(-dX,-dX,dX,'O_c'); + + +for kk = 1:n_ima, + + if active_images(kk); + + if exist(['X_' num2str(kk)]) & exist(['omc_' num2str(kk)]), + + eval(['XX_kk = X_' num2str(kk) ';']); + + if ~isnan(XX_kk(1,1)) + + eval(['omc_kk = omc_' num2str(kk) ';']); + eval(['Tc_kk = Tc_' num2str(kk) ';']); + N_kk = size(XX_kk,2); + + if ~exist(['n_sq_x_' num2str(kk)]), + no_grid = 1; + else + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + if isnan(n_sq_x(1)), + no_grid = 1; + end; + end; + + + if ~no_grid, + eval(['n_sq_x = n_sq_x_' num2str(kk) ';']); + eval(['n_sq_y = n_sq_y_' num2str(kk) ';']); + if (N_kk ~= ((n_sq_x+1)*(n_sq_y+1))), + no_grid = 1; + end; + end; + + if ~isnan(omc_kk(1,1)), + + R_kk = rodrigues(omc_kk); + + YY_kk = R_kk * XX_kk + Tc_kk * ones(1,length(XX_kk)); + + uu = [-dX;-dY;0]/2; + uu = R_kk * uu + Tc_kk; + + if ~no_grid, + YYx = zeros(n_sq_x+1,n_sq_y+1); + YYy = zeros(n_sq_x+1,n_sq_y+1); + YYz = zeros(n_sq_x+1,n_sq_y+1); + + YYx(:) = YY_kk(1,:); + YYy(:) = YY_kk(2,:); + YYz(:) = YY_kk(3,:); + + %keyboard; + + figure(4); + hhh= mesh(YYx,YYz,-YYy); + set(hhh,'edgecolor',colors(rem(kk-1,6)+1),'linewidth',1); %,'facecolor','none'); + %plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['o' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + else + + figure(4); + plot3(YY_kk(1,:),YY_kk(3,:),-YY_kk(2,:),['.' colors(rem(kk-1,6)+1)]); + text(uu(1),uu(3),-uu(2),num2str(kk),'fontsize',14,'color',colors(rem(kk-1,6)+1)); + + end; + + end; + + end; + + end; + + end; + +end; + +figure(4);rotate3d on; +axis('equal'); +title('Extrinsic parameters'); +%view(60,30); +view(a,b); +hold off; + +set(4,'Name','3D','NumberTitle','off'); + +%fprintf(1,'To generate the complete movie associated to the optimization loop, try: check_convergence;\n'); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m new file mode 100755 index 0000000..0cf9abe --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_grid.m @@ -0,0 +1,234 @@ +function [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc,dX,dY); + +map = gray(256); + +minI = min(I(:)); +maxI = max(I(:)); + +Id = 255*(I - minI)/(maxI - minI); + + figure(2); + image(Id); + colormap(map); + + + if nargin < 2, + + disp('Window size for corner finder (wintx and winty):'); + wintx = input('wintx ([] = 5) = '); + if isempty(wintx), wintx = 5; end; + wintx = round(wintx); + winty = input('winty ([] = 5) = '); + if isempty(winty), winty = 5; end; + winty = round(winty); + + fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + end; + + + + title('Click on the four extreme corners of the rectangular pattern...'); + + 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; + + if ~exist('dX')|~exist('dY'), + + % Enter the size of each square + + dX = input(['Size dX of each square along the X direction ([]=30mm) = ']); + dY = input(['Size dY of each square along the Y direction ([]=30mm) = ']); + if isempty(dX), dX = 30; end; + if isempty(dY), dY = 30; end; + + end; + + + % Compute the inside points through computation of the planar homography (collineation) + + a00 = [x(1);y(1);1]; + a10 = [x(2);y(2);1]; + a11 = [x(3);y(3);1]; + a01 = [x(4);y(4);1]; + + + % Compute the planart collineation: (return the normalization matrice as well) + + [Homo,Hnorm,inv_Hnorm] = compute_homography ([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); + + + % Build the grid using the planar collineation: + + x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; + y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; + pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; + + XX = Homo*pts; + XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); + + + % Complete size of the rectangle + + W = n_sq_x*dX; + L = n_sq_y*dY; + + + + if nargin < 6, + + %%%%%%%%%%%%%%%%%%%%%%%% ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + figure(2); + hold on; + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be close to the image corners'); + hold off; + + disp('If the guessed grid corners (red crosses on the image) are not close to the actual corners,'); + disp('it is necessary to enter an initial guess for the radial distortion factor kc (useful for subpixel detection)'); + quest_distort = input('Need of an initial guess for distortion? ([]=no, other=yes) '); + + quest_distort = ~isempty(quest_distort); + + if quest_distort, + % Estimation of focal length: + c_g = [size(I,2);size(I,1)]/2 + .5; + f_g = Distor2Calib(0,[[x(1) x(2) x(4) x(3)] - c_g(1);[y(1) y(2) y(4) y(3)] - c_g(2)],1,1,4,W,L,[-W/2 W/2 W/2 -W/2;L/2 L/2 -L/2 -L/2; 0 0 0 0],100,1,1); + f_g = mean(f_g); + script_fit_distortion; + end; + %%%%%%%%%%%%%%%%%%%%% END ADDITIONAL STUFF IN THE CASE OF HIGHLY DISTORTED IMAGES %%%%%%%%%%%%% + + else + + xy_corners_undist = comp_distortion_oulu([(x' - cc(1))/fc(1);(y'-cc(2))/fc(1)],kc); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + r2 = sum(XXu.^2); + XX = (ones(2,1)*(1 + kc(1) * r2 + kc(2) * (r2.^2))) .* XXu; + XX(1,:) = fc(1)*XX(1,:)+cc(1); + XX(2,:) = fc(2)*XX(2,:)+cc(2); + + end; + + + Np = (n_sq_x+1)*(n_sq_y+1); + + disp('Corner extraction...'); + + grid_pts = cornerfinder(XX,I,winty,wintx); %%% Finds the exact corners at every points! + + grid_pts = grid_pts - 1; % subtract 1 to bring the origin to (0,0) instead of (1,1) in matlab (not necessary in C) + + ind_corners = [1 n_sq_x+1 (n_sq_x+1)*n_sq_y+1 (n_sq_x+1)*(n_sq_y+1)]; % index of the 4 corners + ind_orig = (n_sq_x+1)*n_sq_y + 1; + xorig = grid_pts(1,ind_orig); + yorig = grid_pts(2,ind_orig); + dxpos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig+1)]'); + dypos = mean([grid_pts(:,ind_orig) grid_pts(:,ind_orig-n_sq_x-1)]'); + + + ind_x = (n_sq_x+1)*(n_sq_y + 1); + ind_y = 1; + + x_box_kk = [grid_pts(1,:)-(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)+(wintx+.5);grid_pts(1,:)-(wintx+.5);grid_pts(1,:)-(wintx+.5)]; + y_box_kk = [grid_pts(2,:)-(winty+.5);grid_pts(2,:)-(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)+(winty+.5);grid_pts(2,:)-(winty+.5)]; + + + figure(3); + image(Id); colormap(map); hold on; + plot(grid_pts(1,:)+1,grid_pts(2,:)+1,'r+'); + plot(x_box_kk+1,y_box_kk+1,'-b'); + plot(grid_pts(1,ind_corners)+1,grid_pts(2,ind_corners)+1,'mo'); + plot(xorig+1,yorig+1,'*m'); + h = text(xorig-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; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m new file mode 100755 index 0000000..035b97d --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters.m @@ -0,0 +1,46 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +check_active_images; + +fc = solution(1:2);%*** +cc = solution(3:4);%*** +alpha_c = solution(5);%*** +kc = solution(6:9);%*** + + +% Calibration matrix: + +KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + if active_images(kk), + + omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** + Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** + + Rckk = rodrigues(omckk); + + Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hkk = Hkk / Hkk(3,3); + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + Rckk = NaN*ones(3,3); + Hkk = NaN*ones(3,3); + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + eval(['H_' num2str(kk) '= Hkk;']); + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m new file mode 100755 index 0000000..841c6ab --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extract_parameters3D.m @@ -0,0 +1,36 @@ + +%%% Extraction of the final intrinsic and extrinsic paramaters: + + +fc = solution(1:2); +kc = solution(3:6); +cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3); + +% Calibration matrix: + +KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1]; +inv_KK = inv(KK); + +% Extract the extrinsic paramters, and recomputer the collineations + +for kk = 1:n_ima, + + omckk = solution(4+6*(kk-1) + 3:6*kk + 3); + + Tckk = solution(6*kk+1 + 3:6*kk+3 + 3); + + Rckk = rodrigues(omckk); + + Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; + + Hlkk = Hlkk / Hlkk(3,3); + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Rc_' num2str(kk) ' = Rckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + + eval(['Hl_' num2str(kk) '=Hlkk;']); + +end; + + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m new file mode 100755 index 0000000..fbba78e --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/extrinsic_computation.m @@ -0,0 +1,185 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +dir; + +fprintf(1,'\n'); +disp('Computation of the extrinsic parameters from an image of a pattern'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); +image_name = input('Image name (full name without extension): ','s'); + +format_image2 = '0'; + +while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; +end; + +ima_name = [image_name '.' format_image2]; + + +%%% READ IN IMAGE: + +if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +if size(I,3)>1, + I = I(:,:,2); +end; + + +%%% EXTRACT GRID CORNERS: + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +[x_ext,X_ext,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(I,wintx,winty,fc,cc,kc); + + + +%%% Computation of the Extrinsic Parameters attached to the grid: + +[omc_ext,Tc_ext,Rc_ext,H_ext] = compute_extrinsic(x_ext,X_ext,fc,cc,kc,alpha_c); + + +%%% Reproject the points on the image: + +[x_reproj] = project_points2(X_ext,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +err_reproj = x_ext - x_reproj; + +err_std2 = std(err_reproj')'; + + +Basis = [X_ext(:,[ind_orig ind_x ind_orig ind_y ind_orig ])]; + +VX = Basis(:,2) - Basis(:,1); +VY = Basis(:,4) - Basis(:,1); + +nX = norm(VX); +nY = norm(VY); + +VZ = min(nX,nY) * cross(VX/nX,VY/nY); + +Basis = [Basis VZ]; + +[x_basis] = project_points2(Basis,omc_ext,Tc_ext,fc,cc,kc,alpha_c); + +dxpos = (x_basis(:,2) + x_basis(:,1))/2; +dypos = (x_basis(:,4) + x_basis(:,3))/2; +dzpos = (x_basis(:,6) + x_basis(:,5))/2; + + + +figure(2); +image(I); +colormap(gray(256)); +hold on; +plot(x_ext(1,:)+1,x_ext(2,:)+1,'r+'); +plot(x_reproj(1,:)+1,x_reproj(2,:)+1,'yo'); +h = text(x_ext(1,ind_orig)-25,x_ext(2,ind_orig)-25,'O'); +set(h,'Color','g','FontSize',14); +h2 = text(dxpos(1)+1,dxpos(2)-30,'X'); +set(h2,'Color','g','FontSize',14); +h3 = text(dypos(1)-30,dypos(2)+1,'Y'); +set(h3,'Color','g','FontSize',14); +h4 = text(dzpos(1)-10,dzpos(2)-20,'Z'); +set(h4,'Color','g','FontSize',14); +plot(x_basis(1,:)+1,x_basis(2,:)+1,'g-','linewidth',2); +title('Image points (+) and reprojected grid points (o)'); +hold off; + + +fprintf(1,'\n\nExtrinsic parameters:\n\n'); +fprintf(1,'Translation vector: Tc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',Tc_ext); +fprintf(1,'Rotation vector: omc_ext = [ %3.6f \t %3.6f \t %3.6f ]\n',omc_ext); +fprintf(1,'Rotation matrix: Rc_ext = [ %3.6f \t %3.6f \t %3.6f\n',Rc_ext(1,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f\n',Rc_ext(2,:)'); +fprintf(1,' %3.6f \t %3.6f \t %3.6f ]\n',Rc_ext(3,:)'); +fprintf(1,'Pixel error: err = [ %3.5f \t %3.5f ]\n\n',err_std2); + + + + + +return; + + +% Stores the results: + +kk = 1; + +% Stores location of grid wrt camera: + +eval(['omc_' num2str(kk) ' = omc_ext;']); +eval(['Tc_' num2str(kk) ' = Tc_ext;']); + +% Stores the projected points: + +eval(['y_' num2str(kk) ' = x_reproj;']); +eval(['X_' num2str(kk) ' = X_ext;']); +eval(['x_' num2str(kk) ' = x_ext;']); + + +% Organize the points in a grid: + +eval(['n_sq_x_' num2str(kk) ' = n_sq_x;']); +eval(['n_sq_y_' num2str(kk) ' = n_sq_y;']); + \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m new file mode 100755 index 0000000..b5808f3 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixallvariables.m @@ -0,0 +1,19 @@ +% Code that clears all empty or NaN variables + +varlist = whos; + +if ~isempty(varlist), + + Nvar = size(varlist,1); + + for c_var = 1:Nvar, + + var2fix = varlist(c_var).name; + + fixvariable; + + end; + +end; + +clear varlist var2fix Nvar c_var \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m new file mode 100755 index 0000000..2213431 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/fixvariable.m @@ -0,0 +1,18 @@ +% Code that clears an empty variable, or a NaN vsriable. +% Does not clear structures, or cells. + +if exist('var2fix'), + if eval(['exist(''' var2fix ''') == 1']), + if eval(['isempty(' var2fix ')']), + eval(['clear ' var2fix ]); + else + if eval(['~isstruct(' var2fix ')']), + if eval(['~iscell(' var2fix ')']), + if eval(['isnan(' var2fix '(1))']), + eval(['clear ' var2fix ]); + end; + end; + end; + end; + end; +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m new file mode 100755 index 0000000..56fe496 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ginput3.m @@ -0,0 +1,216 @@ +function [out1,out2,out3] = ginput2(arg1) +%GINPUT Graphical input from mouse. +% [X,Y] = GINPUT(N) gets N points from the current axes and returns +% the X- and Y-coordinates in length N vectors X and Y. The cursor +% can be positioned using a mouse (or by using the Arrow Keys on some +% systems). Data points are entered by pressing a mouse button +% or any key on the keyboard except carriage return, which terminates +% the input before N points are entered. +% +% [X,Y] = GINPUT gathers an unlimited number of points until the +% return key is pressed. +% +% [X,Y,BUTTON] = GINPUT(N) returns a third result, BUTTON, that +% contains a vector of integers specifying which mouse button was +% used (1,2,3 from left) or ASCII numbers if a key on the keyboard +% was used. + +% Copyright (c) 1984-96 by The MathWorks, Inc. +% $Revision: 5.18 $ $Date: 1996/11/10 17:48:08 $ + +% Fixed version by Jean-Yves Bouguet to have a cross instead of 2 lines +% More visible for images + +P = NaN*ones(16,16); +P(1:15,1:15) = 2*ones(15,15); +P(2:14,2:14) = ones(13,13); +P(3:13,3:13) = NaN*ones(11,11); +P(6:10,6:10) = 2*ones(5,5); +P(7:9,7:9) = 1*ones(3,3); + +out1 = []; out2 = []; out3 = []; y = []; +c = computer; +if ~strcmp(c(1:2),'PC') & ~strcmp(c(1:2),'MA') + tp = get(0,'TerminalProtocol'); +else + tp = 'micro'; +end + +if ~strcmp(tp,'none') & ~strcmp(tp,'x') & ~strcmp(tp,'micro'), + if nargout == 1, + if nargin == 1, + eval('out1 = trmginput(arg1);'); + else + eval('out1 = trmginput;'); + end + elseif nargout == 2 | nargout == 0, + if nargin == 1, + eval('[out1,out2] = trmginput(arg1);'); + else + eval('[out1,out2] = trmginput;'); + end + if nargout == 0 + out1 = [ out1 out2 ]; + end + elseif nargout == 3, + if nargin == 1, + eval('[out1,out2,out3] = trmginput(arg1);'); + else + eval('[out1,out2,out3] = trmginput;'); + end + end +else + + fig = gcf; + figure(gcf); + + if nargin == 0 + how_many = -1; + b = []; + else + how_many = arg1; + b = []; + if isstr(how_many) ... + | size(how_many,1) ~= 1 | size(how_many,2) ~= 1 ... + | ~(fix(how_many) == how_many) ... + | how_many < 0 + error('Requires a positive integer.') + end + if how_many == 0 + ptr_fig = 0; + while(ptr_fig ~= fig) + ptr_fig = get(0,'PointerWindow'); + end + scrn_pt = get(0,'PointerLocation'); + loc = get(fig,'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + out1 = pt(1); y = pt(2); + elseif how_many < 0 + error('Argument must be a positive integer.') + end + end + +pointer = get(gcf,'pointer'); + +set(gcf,'Pointer','custom','PointerShapeCData',P,'PointerShapeHotSpot',[8,8]); +%set(gcf,'pointer','crosshair'); + fig_units = get(fig,'units'); + char = 0; + + while how_many ~= 0 + % Use no-side effect WAITFORBUTTONPRESS + waserr = 0; + eval('keydown = wfbp;', 'waserr = 1;'); + if(waserr == 1) + if(ishandle(fig)) + set(fig,'pointer',pointer,'units',fig_units); + error('Interrupted'); + else + error('Interrupted by figure deletion'); + end + end + + ptr_fig = get(0,'CurrentFigure'); + if(ptr_fig == fig) + if keydown + char = get(fig, 'CurrentCharacter'); + button = abs(get(fig, 'CurrentCharacter')); + scrn_pt = get(0, 'PointerLocation'); + set(fig,'units','pixels') + loc = get(fig, 'Position'); + pt = [scrn_pt(1) - loc(1), scrn_pt(2) - loc(2)]; + set(fig,'CurrentPoint',pt); + else + button = get(fig, 'SelectionType'); + if strcmp(button,'open') + button = b(length(b)); + elseif strcmp(button,'normal') + button = 1; + elseif strcmp(button,'extend') + button = 2; + elseif strcmp(button,'alt') + button = 3; + else + error('Invalid mouse selection.') + end + end + pt = get(gca, 'CurrentPoint'); + + how_many = how_many - 1; + + if(char == 13) % & how_many ~= 0) + % if the return key was pressed, char will == 13, + % and that's our signal to break out of here whether + % or not we have collected all the requested data + % points. + % If this was an early breakout, don't include + % the key info in the return arrays. + % We will no longer count it if it's the last input. + break; + end + + out1 = [out1;pt(1,1)]; + y = [y;pt(1,2)]; + b = [b;button]; + end + end + + set(fig,'pointer',pointer,'units',fig_units); + + if nargout > 1 + out2 = y; + if nargout > 2 + out3 = b; + end + else + out1 = [out1 y]; + end + +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function key = wfbp +%WFBP Replacement for WAITFORBUTTONPRESS that has no side effects. + +% Remove figure button functions +fprops = {'windowbuttonupfcn','buttondownfcn', ... + 'windowbuttondownfcn','windowbuttonmotionfcn'}; +fig = gcf; +fvals = get(fig,fprops); +set(fig,fprops,{'','','',''}) + +% Remove all other buttondown functions +ax = findobj(fig,'type','axes'); +if isempty(ax) + ch = {}; +else + ch = get(ax,{'Children'}); +end +for i=1:length(ch), + ch{i} = ch{i}(:)'; +end +h = [ax(:)',ch{:}]; +vals = get(h,{'buttondownfcn'}); +mt = repmat({''},size(vals)); +set(h,{'buttondownfcn'},mt); + +% Now wait for that buttonpress, and check for error conditions +waserr = 0; +eval(['if nargout==0,', ... + ' waitforbuttonpress,', ... + 'else,', ... + ' keydown = waitforbuttonpress;',... + 'end' ], 'waserr = 1;'); + +% Put everything back +if(ishandle(fig)) + set(fig,fprops,fvals) + set(h,{'buttondownfcn'},vals) +end + +if(waserr == 1) + error('Interrupted'); +end + +if nargout>0, key = keydown; end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m new file mode 100755 index 0000000..ad19f64 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim.m @@ -0,0 +1,139 @@ +%go_calib_optim +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length + + +if ~exist('n_ima'), + data_calib; % Load the images + click_calib; % Extract the corners +end; + + +check_active_images; + +check_extracted_images; + +check_active_images; + + +desactivated_images = []; + + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point + %%% Required when using one image, and VERY RECOMMENDED WHEN USING LESS THAN 4 images +end; + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + + +if center_optim & (length(ind_active) < 2) & ~rig3D, + fprintf(1,'\nPrincipal point rejected from the optimization when using one image and planar rig (center_optim = 1).\n'); + center_optim = 0; %%% when using a single image, please, no principal point estimation!!! + est_alpha = 0; +end; + +if ~exist('dont_ask'), + dont_ask = 0; +end; + +if center_optim & (length(ind_active) < 5), + fprintf(1,'\nThe principal point estimation may be unreliable (using less than 5 images for calibration).\n'); + if ~dont_ask, + quest = input('Are you sure you want to keep the principal point in the optimization process? ([]=yes, other=no) '); + center_optim = isempty(quest); + end; +end; + +if center_optim, + fprintf(1,'\nINFO: To reject the principal point from the optimization, set center_optim = 0 in go_calib_optim.m\n'); +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + +if ~center_optim & (est_alpha), + fprintf(1,'WARNING: Since there is no principal point estimation, no skew estimation (est_alpha = 0)\n'); + est_alpha = 0; +else + if ~est_alpha, + fprintf(1,'WARNING: Skew not optimized. Check variable est_alpha.\n'); + alpha_c = 0; + else + fprintf(1,'WARNING: Skew is optimized. To disable skew estimation, set est_alpha=0.\n'); + end; +end; + + +if ~exist('est_dist'); + est_dist = [1;1;1;1]; +end; +if ~prod(est_dist), + fprintf(1,'\nWARNING: Distortion not fully estimated. Check variable est_dist.\n'); +end; + + + + +%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) +go_calib_optim_iter; + + + +if ~isempty(desactivated_images), + + param_list_save = param_list; + + fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); + active_images(desactivated_images) = ones(1,length(desactivated_images)); + desactivated_images = []; + + go_calib_optim_iter; + + if ~isempty(desactivated_images), + fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); + end; + + param_list = [param_list_save(:,1:end-1) param_list]; + +end; + + +%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%% + +%graphout_calib; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m new file mode 100755 index 0000000..e3d22f6 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/go_calib_optim_iter.m @@ -0,0 +1,394 @@ +%go_calib_optim_iter +% +%Main calibration function. Computes the intrinsic andextrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% KK: The camera matrix (containing fc and cc) +% omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space +% Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space +% Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors +% +%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic +% camera parameters, and the extrinsic parameters (3D locations of the grids in space) +% +%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through +% the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m +%that is so far implemented to work only with 2D rigs. +%In the future, a more general function will be there. +%For now, if using a 3D calibration rig, quick_init is set to 1 for an easy initialization of the focal length + + +if ~exist('center_optim'), + center_optim = 1; %%% Set this variable to 0 if your do not want to estimate the principal point +end; + +if ~exist('est_dist'), + est_dist = [1;1;1;1]; +end; + +if ~exist('est_alpha'), + est_alpha = 0; % by default, do not estimate skew +end; + + +% Little fix in case of stupid values in the binary variables: +center_optim = ~~center_optim; +est_alpha = ~~est_alpha; +est_dist = ~~est_dist; + + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +quick_init = 0; % Set to 1 for using a quick init (necessary when using 3D rigs) + + +if ~center_optim, % In the case where the principal point is not estimated, keep it at the center of the image + fprintf(1,'Principal point not optimized (center_optim=0). It is kept at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; +end; + + +if ~prod(est_dist), + fprintf(1,'\nDistortion not fully estimated. Check variable est_dist.\n'); +end; + +if ~est_alpha, + fprintf(1,'Skew not optimized (est_alpha=0).\n'); + alpha_c = 0; +end; + + +% Check 3D-ness of the calibration rig: +rig3D = 0; +for kk = ind_active, + eval(['X_kk = X_' num2str(kk) ';']); + if is3D(X_kk), + rig3D = 1; + end; +end; + +% If the rig is 3D, then no choice: the only valid initialization is manual! +if rig3D, + quick_init = 1; +end; + + + +alpha_smooth = 1; % set alpha_smooth = 1; for steepest gradient descent + + +% Conditioning threshold for view rejection +thresh_cond = 1e6; + + + +%% Initialization of the intrinsic parameters (if necessary) + +if ~exist('cc'), + fprintf(1,'Initialization of the principal point at the center of the image.\n'); + cc = [(nx-1)/2;(ny-1)/2]; + alpha_smooth = 0.4; % slow convergence +end; + + +if ~exist('kc'), + fprintf(1,'Initialization of the image distortion to zero.\n'); + kc = zeros(4,1); + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('alpha_c'), + fprintf(1,'Initialization of the image skew to zero.\n'); + alpha_c = 0; + alpha_smooth = 0.4; % slow convergence +end; + +if ~exist('fc')& quick_init, + FOV_angle = 35; % Initial camera field of view in degrees + fprintf(1,['Initialization of the focal length to a FOV of ' num2str(FOV_angle) ' degrees.\n']); + fc = (nx/2)/tan(pi*FOV_angle/360) * ones(2,1); + alpha_smooth = 0.4; % slow +end; + + +if ~exist('fc'), + % Initialization of the intrinsic parameters: + fprintf(1,'Initialization of the intrinsic parameters using the vanishing points of planar patterns.\n') + init_intrinsic_param; % The right way to go (if quick_init is not active)! + alpha_smooth = 0.4; % slow convergence +end; + + +if ~prod(est_dist), + % If no distortion estimated, set to zero the variables that are not estimated + kc = kc .* est_dist; +end; + + + + + +%% Initialization of the extrinsic parameters for global minimization: + + +init_param = [fc;cc;alpha_c;kc;zeros(6,1)]; + + + +for kk = 1:n_ima, + + if exist(['x_' num2str(kk)]), + + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + active_images(kk) = 0; + end; + if active_images(kk), + [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); + if cond(JJ_kk)> thresh_cond, + active_images(kk) = 0; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + end; + if isnan(omckk(1,1)), + %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) + active_images(kk) = 0; + end; + else + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + + else + + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + + if active_images(kk), + fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + + active_images(kk) = 0; + + end; + + eval(['omc_' num2str(kk) ' = omckk;']); + eval(['Tc_' num2str(kk) ' = Tckk;']); + + init_param = [init_param; omckk ; Tckk]; + +end; + + +check_active_images; + + + +%-------------------- Main Optimization: + +fprintf(1,'\nMain calibration optimization procedure - Number of images: %d\n',length(ind_active)); + + +param = init_param; +change = 1; + +iter = 0; + +fprintf(1,'Gradient descent iterations: '); + +param_list = param; + +MaxIter = 30; + + +while (change > 1e-6)&(iter < MaxIter), + + fprintf(1,'%d...',iter+1); + + + %% The first step consists of updating the whole vector of knowns (intrinsic + extrinsic of active + %% images) through a one step steepest gradient descent. + + JJ = []; + ex = []; + + f = param(1:2); + c = param(3:4); + alpha = param(5); + k = param(6:9); + + + for kk = 1:n_ima, + + if active_images(kk), + + %omckk = param(4+6*(kk-1) + 3:6*kk + 3); + + %Tckk = param(6*kk+1 + 3:6*kk+3 + 3); + + omckk = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + + Tckk = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + + if isnan(omckk(1)), + fprintf(1,'Intrinsic parameters at frame %d do not exist\n',kk); + return; + end; + + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + + Np = size(X_kk,2); + + JJkk = zeros(2*Np,n_ima * 6 + 15); + + [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X_kk,omckk,Tckk,f,c,k,alpha); + + exkk = x_kk - x; + + ex = [ex;exkk(:)]; + + JJkk(:,1:2) = dxdf; + JJkk(:,3:4) = dxdc; + JJkk(:,5) = dxdalpha; + JJkk(:,6:9) = dxdk; + JJkk(:,15+6*(kk-1) + 1:15+6*(kk-1) + 3) = dxdom; + JJkk(:,15+6*(kk-1) + 4:15+6*(kk-1) + 6) = dxdT; + + + + JJ = [JJ;JJkk]; + + + % Check if this view is ill-conditioned: + JJ_kk = [dxdom dxdT]; + if cond(JJ_kk)> thresh_cond, + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 6) = NaN*ones(6,1); + end; + + + end; + + end; + + + % List of active images (necessary if changed): + check_active_images; + + + % The following vector helps to select the variables to update (for only active images): + + ind_Jac = find([ones(2,1);center_optim*ones(2,1);est_alpha;est_dist;zeros(6,1);reshape(ones(6,1)*active_images,6*n_ima,1)])'; + + + JJ = JJ(:,ind_Jac); + + JJ2 = JJ'*JJ; + + + % Smoothing coefficient: + + alpha_smooth2 = 1-(1-alpha_smooth)^(iter+1); %set to 1 to undo any smoothing! + + + param_innov = alpha_smooth2*inv(JJ2)*(JJ')*ex; + param_up = param(ind_Jac) + param_innov; + param(ind_Jac) = param_up; + + + % New intrinsic parameters: + + fc_current = param(1:2); + cc_current = param(3:4); + alpha_current = param(5); + kc_current = param(6:9); + + + % Change on the intrinsic parameters: + change = norm([fc_current;cc_current] - [f;c])/norm([fc_current;cc_current]); + + + %% Second step: (optional) - It makes convergence faster, and the region of convergence LARGER!!! + %% Recompute the extrinsic parameters only using compute_extrinsic.m (this may be useful sometimes) + %% The complete gradient descent method is useful to precisely update the intrinsic parameters. + + MaxIter2 = 20; + + + for kk = 1:n_ima, + if active_images(kk), + omc_current = param(15+6*(kk-1) + 1:15+6*(kk-1) + 3); + Tc_current = param(15+6*(kk-1) + 4:15+6*(kk-1) + 6); + eval(['X_kk = X_' num2str(kk) ';']); + eval(['x_kk = x_' num2str(kk) ';']); + [omc_current,Tc_current] = compute_extrinsic_init(x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current); + [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omc_current,Tc_current,x_kk,X_kk,fc_current,cc_current,kc_current,alpha_current,MaxIter2,thresh_cond); + if cond(JJ_kk)> thresh_cond, + active_images(kk) = 0; + fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) + desactivated_images = [desactivated_images kk]; + omckk = NaN*ones(3,1); + Tckk = NaN*ones(3,1); + end; + param(15+6*(kk-1) + 1:15+6*(kk-1) + 3) = omckk; + param(15+6*(kk-1) + 4:15+6*(kk-1) + 6) = Tckk; + end; + end; + + param_list = [param_list param]; + + iter = iter + 1; + +end; + +fprintf(1,'\n'); + + +solution = param; + + +%%% Extraction of the final intrinsic and extrinsic paramaters: + +extract_parameters; + +comp_error_calib; + +fprintf(1,'\n\nCalibration results after optimization:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); +fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); +fprintf(1,'Pixel error: err = [ %3.5f %3.5f ]\n\n',err_std); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m new file mode 100755 index 0000000..dbbc4e0 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/ima_read_calib.m @@ -0,0 +1,158 @@ + +if ~exist('calib_name')|~exist('format_image'), + data_calib; + return; +end; + +check_directory; + +if ~exist('n_ima'), + data_calib; + return; +end; + +check_active_images; + + +images_read = active_images; + + +if exist('image_numbers'), + first_num = image_numbers(1); +end; + + +% Just to fix a minor bug: +if ~exist('first_num'), + first_num = image_numbers(1); +end; + + +image_numbers = first_num:n_ima-1+first_num; + +no_image_file = 0; + +i = 1; + +while (i <= n_ima), % & (~no_image_file), + + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_name = [calib_name number_ext '.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + if exist(ima_name), + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ii = double(loadppm(ima_name)); + else + Ii = double(loadpgm(ima_name)); + end; + else + if format_image(1) == 'r', + Ii = readras(ima_name); + else + Ii = double(imread(ima_name)); + end; + end; + + + if size(Ii,3)>1, + Ii = Ii(:,:,2); + end; + + eval(['I_' num2str(i) ' = Ii;']); + + else + + fprintf(1,'%d...no image...',i); + + images_read(i) = 0; + + %no_image_file = 1; + + end; + + end; + + i = i+1; + +end; + + +ind_read = find(images_read); + + + + +if isempty(ind_read), + + fprintf(1,'\nWARNING! No image were read\n'); + + no_image_file = 1; + + +else + + + %fprintf(1,'\nWARNING! Every exsisting image in the directory is set active.\n'); + + + if no_image_file, + + %fprintf(1,'WARNING! Some images were not read properly\n'); + + end; + + + fprintf(1,'\n'); + + if size(I_1,1)~=480, + small_calib_image = 1; + else + small_calib_image = 0; + end; + + [Hcal,Wcal] = size(I_1); % size of the calibration image + + [ny,nx] = size(I_1); + + clickname = []; + + map = gray(256); + + %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name Hcal Wcal nx ny map small_calib_image'; + + %eval(string_save); + + disp('done'); + %click_calib; + +end; + +if ~exist('map'), map = gray(256); end; + +active_images = images_read; + +% Show all the calibration images: + + +if ~isempty(ind_read), + + mosaic; + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m new file mode 100755 index 0000000..94a5240 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/init_intrinsic_param.m @@ -0,0 +1,158 @@ +%init_intrinsic_param +% +%Initialization of the intrinsic parameters. +%Runs as a script. +% +%INPUT: x_1,x_2,x_3,...: Feature locations on the images +% X_1,X_2,X_3,...: Corresponding grid coordinates +% +%OUTPUT: fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: skew coefficient +% KK: The camera matrix (containing fc, cc and alpha_c) +% +%Method: Computes the planar homographies H_1, H_2, H_3, ... and computes +% the focal length fc from orthogonal vanishing points constraint. +% The principal point cc is assumed at the center of the image. +% Assumes no image distortion (kc = [0;0;0;0]) +% +%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the +% corresponding entry in the active_images vector to zero. +% +% +%Important function called within that program: +% +%compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. +% +% +%VERY IMPORTANT: This function works only with 2D rigs. +%In the future, a more general function will be there (working with 3D rigs as well). + + + +check_active_images; + +if ~exist(['x_' num2str(ind_active(1)) ]), + click_calib; +end; + + +fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); + + +% Initialize the homographies: + +for kk = 1:n_ima, + eval(['x_kk = x_' num2str(kk) ';']); + eval(['X_kk = X_' num2str(kk) ';']); + if (isnan(x_kk(1,1))), + if active_images(kk), + fprintf(1,'WARNING: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) + fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) + end; + active_images(kk) = 0; + end; + if active_images(kk), + eval(['H_' num2str(kk) ' = compute_homography(x_kk,X_kk(1:2,:));']); + else + eval(['H_' num2str(kk) ' = NaN*ones(3,3);']); + end; +end; + +check_active_images; + +% initial guess for principal point and distortion: + +if ~exist('nx'), [ny,nx] = size(I); end; + +c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image +k_init = [0;0;0;0]; % initialize to zero (no distortion) + + + +% Compute explicitely the focal length using all the (mutually orthogonal) vanishing points +% note: The vanihing points are hidden in the planar collineations H_kk + +A = []; +b = []; + +% matrix that subtract the principal point: +Sub_cc = [1 0 -c_init(1);0 1 -c_init(2);0 0 1]; + +for kk=1:n_ima, + + if active_images(kk), + + eval(['Hkk = H_' num2str(kk) ';']); + + Hkk = Sub_cc * Hkk; + + % Extract vanishing points (direct and diagonals): + + V_hori_pix = Hkk(:,1); + V_vert_pix = Hkk(:,2); + V_diag1_pix = (Hkk(:,1)+Hkk(:,2))/2; + V_diag2_pix = (Hkk(:,1)-Hkk(:,2))/2; + + V_hori_pix = V_hori_pix/norm(V_hori_pix); + V_vert_pix = V_vert_pix/norm(V_vert_pix); + V_diag1_pix = V_diag1_pix/norm(V_diag1_pix); + V_diag2_pix = V_diag2_pix/norm(V_diag2_pix); + + a1 = V_hori_pix(1); + b1 = V_hori_pix(2); + c1 = V_hori_pix(3); + + a2 = V_vert_pix(1); + b2 = V_vert_pix(2); + c2 = V_vert_pix(3); + + a3 = V_diag1_pix(1); + b3 = V_diag1_pix(2); + c3 = V_diag1_pix(3); + + a4 = V_diag2_pix(1); + b4 = V_diag2_pix(2); + c4 = V_diag2_pix(3); + + A_kk = [a1*a2 b1*b2; + a3*a4 b3*b4]; + + b_kk = -[c1*c2;c3*c4]; + + + A = [A;A_kk]; + b = [b;b_kk]; + + end; + +end; + + +% use all the vanishing points to estimate focal length: + +f_init = sqrt(abs(1./(inv(A'*A)*A'*b))); % if using a two-focal model for initial guess + +alpha_init = 0; + +%f_init = sqrt(b'*(sum(A')') / (b'*b)) * ones(2,1); % if single focal length model is used + + +% Global calibration matrix (initial guess): + +KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; +inv_KK = inv(KK); + + +cc = c_init; +fc = f_init; +kc = k_init; +alpha_c = alpha_init; + + +fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); +fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); +fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); +fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); +fprintf(1,'Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m new file mode 100755 index 0000000..ab00b3d --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/is3D.m @@ -0,0 +1,19 @@ +function test = is3D(X), + + +Np = size(X,2); + +%% Check for planarity of the structure: + +X_mean = mean(X')'; + +Y = X - (X_mean*ones(1,Np)); + +YY = Y*Y'; + +[U,S,V] = svd(YY); + +r = S(3,3)/S(2,2); + +test = (r > 1e-3); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m new file mode 100755 index 0000000..a0f50d2 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loading_calib.m @@ -0,0 +1,10 @@ +if ~exist('Calib_Results.mat'), + fprintf(1,'\nCalibration file Calib_Results.mat not found!\n'); + return; +end; + +fprintf(1,'\nLoading calibration results from Calib_Results.mat\n'); + +load Calib_Results + +fprintf(1,'done\n'); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m new file mode 100755 index 0000000..91b6f89 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadinr.m @@ -0,0 +1,52 @@ +%LOADINR Load an INRIMAGE format file +% +% LOADINR(filename, im) +% +% Load an INRIA image format file and return it as a matrix +% +% SEE ALSO: saveinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1996 + +function im = loadinr(fname, im) + + fid = fopen(fname, 'r'); + + s = fgets(fid); + if strcmp(s(1:12), '#INRIMAGE-4#') == 0, + error('not INRIMAGE format'); + end + + % not very complete, only looks for the X/YDIM keys + while 1, + s = fgets(fid); + n = length(s) - 1; + if s(1) == '#', + break + end + if strcmp(s(1:5), 'XDIM='), + cols = str2num(s(6:n)); + end + if strcmp(s(1:5), 'YDIM='), + rows = str2num(s(6:n)); + end + if strcmp(s(1:4), 'CPU='), + if strcmp(s(5:n), 'sun') == 0, + error('not sun data ordering'); + end + end + + end + disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)]) + + % now the binary data + fseek(fid, 256, 'bof'); + [im count] = fread(fid, [cols rows], 'float32'); + im = im'; + if count ~= (rows*cols), + error('file too short'); + end + fclose(fid); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m new file mode 100755 index 0000000..dfa8b61 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadpgm.m @@ -0,0 +1,89 @@ +%LOADPGM Load a PGM image +% +% I = loadpgm(filename) +% +% Returns a matrix containing the image loaded from the PGM format +% file filename. Handles ASCII (P2) and binary (P5) PGM file formats. +% +% If the filename has no extension, and open fails, a '.pgm' will +% be appended. +% +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadpgm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.pgm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + if magic(1) == 'P', + if magic(2) == '2', + %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols rows])'; + elseif magic(2) == '5', + %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols rows], fmt)'; + else + disp('Not a PGM file'); + end + end + fclose(fid); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m new file mode 100755 index 0000000..0c004fc --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/loadppm.m @@ -0,0 +1,109 @@ +%LOADPPM Load a PPM image +% +% I = loadppm(filename) +% +% Returns a matrix containing the image loaded from the PPM format +% file filename. Handles ASCII (P3) and binary (P6) PPM file formats. +% +% If the filename has no extension, and open fails, a '.ppm' and +% '.pnm' extension will be tried. +% +% SEE ALSO: saveppm loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function I = loadppm(file) + white = [' ' 9 10 13]; % space, tab, lf, cr + white = setstr(white); + + fid = fopen(file, 'r'); + if fid < 0, + fid = fopen([file '.ppm'], 'r'); + end + if fid < 0, + fid = fopen([file '.pnm'], 'r'); + end + if fid < 0, + error('Couldn''t open file'); + end + + magic = fread(fid, 2, 'char'); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + cols = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + rows = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + maxval = fscanf(fid, '%d', 1); + while 1 + c = fread(fid,1,'char'); + if c == '#', + fgetl(fid); + elseif ~any(c == white) + fseek(fid, -1, 'cof'); % unputc() + break; + end + end + if magic(1) == 'P', + if magic(2) == '3', + %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)]) + I = fscanf(fid, '%d', [cols*3 rows]); + elseif magic(2) == '6', + %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)]) + if maxval == 1, + fmt = 'unint1'; + elseif maxval == 15, + fmt = 'uint4'; + elseif maxval == 255, + fmt = 'uint8'; + elseif maxval == 2^32-1, + fmt = 'uint32'; + end + I = fread(fid, [cols*3 rows], fmt); + else + disp('Not a PPM file'); + end + end + % + % now the matrix has interleaved columns of R, G, B + % + I = I'; + size(I); + R = I(:,1:3:(cols*3)); + G = I(:,2:3:(cols*3)); + B = I(:,3:3:(cols*3)); + fclose(fid); + + + I = zeros(rows,cols,3); + I(:,:,1) = R; + I(:,:,2) = G; + I(:,:,3) = B; + I = uint8(I); + \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m new file mode 100755 index 0000000..0d18a62 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mean_std_robust.m @@ -0,0 +1,7 @@ +function [m,s] = mean_std_robust(x); + +x = x(:); + +m = median(x); + +s = median(abs(x - m))*1.4836; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m new file mode 100755 index 0000000..b056661 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/mosaic.m @@ -0,0 +1,92 @@ + +if ~exist('I_1'), + active_images_save = active_images; + ima_read_calib; + active_images = active_images_save; + check_active_images; +end; + +check_active_images; + +if isempty(ind_read), + return; +end; + + +n_col = floor(sqrt(n_ima*nx/ny)); + +n_row = ceil(n_ima / n_col); + + +ker2 = 1; +for ii = 1:n_col, + ker2 = conv(ker2,[1/4 1/2 1/4]); +end; + + +II = I_1(1:n_col:end,1:n_col:end); + +[ny2,nx2] = size(II); + + + +kk_c = 1; + +II_mosaic = []; + +for jj = 1:n_row, + + + II_row = []; + + for ii = 1:n_col, + + if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima), + + if active_images(kk_c), + eval(['I = I_' num2str(kk_c) ';']); + %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing + I = I(1:n_col:end,1:n_col:end); + else + I = zeros(ny2,nx2); + end; + + else + + I = zeros(ny2,nx2); + + end; + + + + II_row = [II_row I]; + + if ii ~= n_col, + + II_row = [II_row zeros(ny2,3)]; + + end; + + + kk_c = kk_c + 1; + + end; + + nn2 = size(II_row,2); + + if jj ~= n_row, + II_row = [II_row; zeros(3,nn2)]; + end; + + II_mosaic = [II_mosaic ; II_row]; + +end; + +figure(2); +image(II_mosaic); +colormap(gray(256)); +title('Calibration images'); +set(gca,'Xtick',[]) +set(gca,'Ytick',[]) +axis('image'); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m new file mode 100755 index 0000000..6dc7149 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/normalize.m @@ -0,0 +1,43 @@ +function [xn] = normalize(x_kk,fc,cc,kc,alpha_c), + +%normalize +% +%[xn] = normalize(x_kk,fc,cc,kc,alpha_c) +% +%Computes the normalized coordinates xn given the pixel coordinates x_kk +%and the intrinsic camera parameters fc, cc and kc. +% +%INPUT: x_kk: Feature locations on the images +% fc: Camera focal length +% cc: Principal point coordinates +% kc: Distortion coefficients +% alpha_c: Skew coefficient +% +%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) +% +%Important functions called within that program: +% +%comp_distortion_oulu: undistort pixel coordinates. + +if nargin < 5, + alpha_c = 0; + if nargin < 4; + kc = [0;0;0;0]; + if nargin < 3; + cc = [0;0]; + if nargin < 2, + fc = [1;1]; + end; + end; + end; +end; + + +% First subtract principal point, and divide by the focal length: +temp = (x_kk(2,:) - cc(2))/fc(2); +x_distort = [(x_kk(1,:) - cc(1))/fc(1) - alpha_c*temp;temp]; + + +%Compensate for lens distortion: + +xn = comp_distortion_oulu(x_distort,kc); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m new file mode 100755 index 0000000..c96ccb7 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/pgmread.m @@ -0,0 +1,26 @@ +function img = pgmread(filename) +% function img = pgmread(filename) +% this is my version of pgmread for the pgm file created by XV. +% +% this program also corrects for the shifts in the image from pm file. + +fid = fopen(filename,'r'); +fscanf(fid, 'P5\n'); +cmt = '#'; +while findstr(cmt, '#'), + cmt = fgets(fid); + if length(findstr(cmt, '#')) ~= 1, + YX = sscanf(cmt, '%d %d'); + y = YX(1); x = YX(2); + end +end + +%fgets(fid); + +%img = fscanf(fid,'%d',size); +%img = img'; + +img = fread(fid,[y,x],'uint8'); +img = img'; +fclose(fid); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m new file mode 100755 index 0000000..c5c4a34 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project2_oulu.m @@ -0,0 +1,53 @@ +function [x] = project2_oulu(X,R,T,f,t,k) +%PROJECT Subsidiary to calib + +% (c) Pietro Perona -- March 24, 1994 +% California Institute of Technology +% Pasadena, CA +% +% Renamed because project exists in matlab 5.2!!! +% Now uses the more elaborate intrinsic model from Oulu + + + +[m,n] = size(X); + +Y = R*X + T*ones(1,n); +Z = Y(3,:); + +f = f(:); %% make a column vector +if length(f)==1, + f = [f f]'; +end; + +x = (Y(1:2,:) ./ (ones(2,1) * Z)) ; + + +radius_2 = x(1,:).^2 + x(2,:).^2; + +if length(k) > 1, + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2)); + + if length(k) < 4, + + delta_x = zeros(2,n); + + else + + delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ; + k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)]; + + end; + + +else + + radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2)); + + delta_x = zeros(2,n); + +end; + + +x = (x .* radial_distortion + delta_x).* (f * ones(1,n)) + t*ones(1,n); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m new file mode 100755 index 0000000..1823490 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points.m @@ -0,0 +1,276 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) + +%project_points.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion, the right terms correspond to tangential distortion +% +%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*xx + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + + +if nargin < 6, + k = zeros(4,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + +dxdom = zeros(2*n,3); +dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); +dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + +dxdT = zeros(2*n,3); +dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); +dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + + + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom; +dcdistdT = k(1) * dr2dT+ k(2) * dr4dT; +dcdistdk = [ r2' r4' zeros(n,2)]; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,4); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,4); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Pixel coordinates: + +xp = xd2 .* (f * ones(1,n)) + c*ones(1,n); + +coeff = reshape(f*ones(1,n),2*n,1); + +dxpdom = (coeff*ones(1,3)) .* dxd2dom; +dxpdT = (coeff*ones(1,3)) .* dxd2dT; +dxpdk = (coeff*ones(1,4)) .* dxd2dk; + +dxpdf = zeros(2*n,2); +dxpdf(1:2:end,1) = xd2(1,:)'; +dxpdf(2:2:end,2) = xd2(2,:)'; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(4,1); + + +[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k); + + +% Test on om: 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/project_points2.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m new file mode 100755 index 0000000..5bb1b91 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/project_points2.m @@ -0,0 +1,312 @@ +function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha) + +%project_points.m +% +%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha) +% +%Projects a 3D structure onto the image plane. +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) +% c: principal point location in pixel units (2x1 vector) +% k: Distortion coefficients (radial and tangential) (4x1 vector) +% alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels) +% +%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) +% dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) +% dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) +% dxpdf: Derivative of xp with respect to f ((2N)x2 matrix) +% dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) +% dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Xc = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); +%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. +%call r^2 = a^2 + b^2. +%The distorted point coordinates are: xd = [xx;yy] where: +% +%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); +%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; +% +%The left terms correspond to radial distortion, the right terms correspond to tangential distortion +% +%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: +% +%xxp = f(1)*(xx + alpha*yy) + c(1) +%yyp = f(2)*yy + c(2) +% +% +%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices +% +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector +% +%rigid_motion.m: Computes the rigid motion transformation of a given structure + + +if nargin < 7, + alpha = 0; + if nargin < 6, + k = zeros(4,1); + if nargin < 5, + c = zeros(2,1); + if nargin < 4, + f = ones(2,1); + if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure to project (in project_points.m)'); + return; + end; + end; + end; + end; + end; + end; +end; + + +[m,n] = size(X); + +[Y,dYdom,dYdT] = rigid_motion(X,om,T); + + +inv_Z = 1./Y(3,:); + +x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; + + +bb = (-x(1,:) .* inv_Z)'*ones(1,3); +cc = (-x(2,:) .* inv_Z)'*ones(1,3); + + +dxdom = zeros(2*n,3); +dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); +dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); + +dxdT = zeros(2*n,3); +dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); +dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); + + +% Add distortion: + +r2 = x(1,:).^2 + x(2,:).^2; + + + +dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); +dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); + + +r4 = r2.^2; + +dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; +dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; + + +% Radial distortion: + +cdist = 1 + k(1) * r2 + k(2) * r4; + +dcdistdom = k(1) * dr2dom + k(2) * dr4dom; +dcdistdT = k(1) * dr2dT+ k(2) * dr4dT; +dcdistdk = [ r2' r4' zeros(n,2)]; + + +xd1 = x .* (ones(2,1)*cdist); + +dxd1dom = zeros(2*n,3); +dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; +dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; +coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); +dxd1dom = dxd1dom + coeff.* dxdom; + +dxd1dT = zeros(2*n,3); +dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; +dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; +dxd1dT = dxd1dT + coeff.* dxdT; + +dxd1dk = zeros(2*n,4); +dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; +dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; + + + +% tangential distortion: + +a1 = 2.*x(1,:).*x(2,:); +a2 = r2 + 2*x(1,:).^2; +a3 = r2 + 2*x(2,:).^2; + +delta_x = [k(3)*a1 + k(4)*a2 ; + k(3) * a3 + k(4)*a1]; + + +ddelta_xdx = zeros(2*n,2*n); +aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); +bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); +cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); + +ddelta_xdom = zeros(2*n,3); +ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); +ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); + +ddelta_xdT = zeros(2*n,3); +ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); +ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); + +ddelta_xdk = zeros(2*n,4); +ddelta_xdk(1:2:end,3) = a1'; +ddelta_xdk(1:2:end,4) = a2'; +ddelta_xdk(2:2:end,3) = a3'; +ddelta_xdk(2:2:end,4) = a1'; + + + +xd2 = xd1 + delta_x; + +dxd2dom = dxd1dom + ddelta_xdom ; +dxd2dT = dxd1dT + ddelta_xdT; +dxd2dk = dxd1dk + ddelta_xdk ; + + +% Add Skew: + +xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)]; + +% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha + +dxd3dom = zeros(2*n,3); +dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:); +dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:); +dxd3dT = zeros(2*n,3); +dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:); +dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:); +dxd3dk = zeros(2*n,4); +dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:); +dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:); +dxd3dalpha = zeros(2*n,1); +dxd3dalpha(1:2:2*n,:) = xd2(2,:)'; + + + +% Pixel coordinates: + +xp = xd3 .* (f * ones(1,n)) + c*ones(1,n); + +coeff = reshape(f*ones(1,n),2*n,1); + +dxpdom = (coeff*ones(1,3)) .* dxd3dom; +dxpdT = (coeff*ones(1,3)) .* dxd3dT; +dxpdk = (coeff*ones(1,4)) .* dxd3dk; +dxpdalpha = (coeff) .* dxd3dalpha; + +dxpdf = zeros(2*n,2); +dxpdf(1:2:end,1) = xd2(1,:)'; +dxpdf(2:2:end,2) = xd2(2,:)'; + +dxpdc = zeros(2*n,2); +dxpdc(1:2:end,1) = ones(n,1); +dxpdc(2:2:end,2) = ones(n,1); + + +return; + +% Test of the Jacobians: + +n = 10; + +X = 10*randn(3,n); +om = randn(3,1); +T = [10*randn(2,1);40]; +f = 1000*rand(2,1); +c = 1000*randn(2,1); +k = 0.5*randn(4,1); +alpha = 0.01*randn(1,1); + +[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha); + + +% Test on om: NOT OK + +dom = 0.000000001 * norm(om)*randn(3,1); +om2 = om + dom; + +[x2] = project_points2(X,om2,T,f,c,k,alpha); + +x_pred = x + reshape(dxdom * dom,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on T: OK!! + +dT = 0.0001 * norm(T)*randn(3,1); +T2 = T + dT; + +[x2] = project_points2(X,om,T2,f,c,k,alpha); + +x_pred = x + reshape(dxdT * dT,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + + +% Test on f: OK!! + +df = 0.001 * norm(f)*randn(2,1); +f2 = f + df; + +[x2] = project_points2(X,om,T,f2,c,k,alpha); + +x_pred = x + reshape(dxdf * df,2,n); + + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on c: OK!! + +dc = 0.01 * norm(c)*randn(2,1); +c2 = c + dc; + +[x2] = project_points2(X,om,T,f,c2,k,alpha); + +x_pred = x + reshape(dxdc * dc,2,n); + +norm(x2-x)/norm(x2 - x_pred) + +% Test on k: OK!! + +dk = 0.001 * norm(k)*randn(4,1); +k2 = k + dk; + +[x2] = project_points2(X,om,T,f,c,k2,alpha); + +x_pred = x + reshape(dxdk * dk,2,n); + +norm(x2-x)/norm(x2 - x_pred) + + +% Test on alpha: OK!! + +dalpha = 0.001 * norm(k)*randn(1,1); +alpha2 = alpha + dalpha; + +[x2] = project_points2(X,om,T,f,c,k,alpha2); + +x_pred = x + reshape(dxdalpha * dalpha,2,n); + +norm(x2-x)/norm(x2 - x_pred) diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m new file mode 100755 index 0000000..561a7d0 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projectedGrid.m @@ -0,0 +1,24 @@ +function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny); + +% new formalism using homographies + +a00 = [P1;1]; +a10 = [P2;1]; +a11 = [P3;1]; +a01 = [P4;1]; + +% Compute the planart collineation: + +[H] = compute_collineation (a00, a10, a11, a01); + + +% Build the grid using the planar collineation: + +x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1); +y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1); + +pts = [x_l(:) y_l(:) ones(nx*ny,1)]'; + +XX = H*pts; + +XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m new file mode 100755 index 0000000..bb4ef86 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/projector_calib.m @@ -0,0 +1,258 @@ +%%% This code is an additional code that helps doing projector calibration in 3D scanning setup. +%%% This is not a useful code for anyone else but me. +%%% I included it in the toolbox for illustration only. + + +load camera_results; + + +proj_name = input('Basename projector calibration images (without number nor suffix): ','s'); + + +i = 1; + +while (i <= n_ima), % & (~no_image_file), + + if active_images(i), + + %fprintf(1,'Loading image %d...\n',i); + + if ~type_numbering, + number_ext = num2str(image_numbers(i)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(i)); + end; + + ima_namep = [proj_name number_ext 'p.' format_image]; + ima_namen = [proj_name number_ext 'n.' format_image]; + + if i == ind_active(1), + fprintf(1,'Loading image '); + end; + + fprintf(1,'%d...',i); + + if format_image(1) == 'p', + if format_image(2) == 'p', + Ip = double(loadppm(ima_namep)); + In = double(loadppm(ima_namen)); + else + Ip = double(loadpgm(ima_namep)); + In = double(loadpgm(ima_namen)); + end; + else + if format_image(1) == 'r', + Ip = readras(ima_namep); + In = readras(ima_namen); + else + Ip = double(imread(ima_namep)); + In = double(imread(ima_namen)); + end; + end; + + + if size(Ip,3)>1, + Ip = Ip(:,:,2); + In = In(:,:,2); + end; + + eval(['Ip_' num2str(i) ' = Ip;']); + eval(['In_' num2str(i) ' = In;']); + + end; + + i = i+1; + +end; + + +fprintf(1,'\nExtraction of the grid corners on the image\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + + +disp('The projector you are using is the DLP or Intel'); +nx = 800; +ny = 600; + +dX = input('Size dX in x of the squares (in pixels) [50] = '); +dY = input('Size dY in y of the squares (in pixels) [50] = '); + +if isempty(dX), dX=50; end; +if isempty(dY), dY=50; end; + +dXoff = input('Position in x of your reference (in pixels) [399.5] = '); +dYoff = input('Position in y of your reference (in pixels) [299.5] = '); + +if isempty(dXoff), dXoff=399.5; end; +if isempty(dYoff), dYoff=299.5; end; + +end; + + + +for kk = ind_active, + + eval(['Ip = Ip_' num2str(kk) ';']); + eval(['In = In_' num2str(kk) ';']); + + [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(In,wintx,winty,fc,cc,kc,dX,dY); + xproj = x; + + Np_proj = size(x,2); + + figure(2); + image(Ip); + hold on; + plot(xproj(1,:)+1,xproj(2,:)+1,'r+'); + title('Click on your reference point'); + xlabel('Xc (in camera frame)'); + ylabel('Yc (in camera frame)'); + hold off; + + disp('Click on your reference point...'); + + [xr,yr] = ginput2(1); + + err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2)); + ind_ref = find(err == min(err)); + + ref_pt = xproj(:,ind_ref); + + + figure(2); + hold on; + plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off; + + + nn = floor(ind_ref/(n_sq_x+1)); + off_x = ind_ref - nn*(n_sq_x+1) - 1; + off_y = n_sq_y - nn; + + + xprojn = xproj - cc * ones(1,Np_proj); + xprojn = xprojn ./ (fc * ones(1,Np_proj)); + xprojn = comp_distortion(xprojn,kc); + + eval(['Rc = Rc_' num2str(kk) ';']); + eval(['Tc = Tc_' num2str(kk) ';']); + + Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); + Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame + %Xproj = Rc'* Xcp - (Rc'*Tc)*ones(1,Np); % in the object frame !!! it works! + %Xproj(3,:) = zeros(1,Np); + + eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the + + x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj)); + + eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the + +end; + + + +X_proj = []; +x_proj = []; + +for kk = ind_active, + eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); + eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); +end; + + +%Save camera parameters: +fc_save = fc; +cc_save = cc; +kc_save = kc; + +omc_1_save = omc_1; +Rc_1_save = Rc_1; +Tc_1_save = Tc_1; + + +% Get started to calibrate projector: +clear fc cc kc + +n_ima = 1; +X_1 = X_proj; +x_1 = x_proj; + + +% Image size: (may or may not be available) + +nx = 800; +ny = 600; + +% No calibration image is available (only the corner coordinates) + +no_image = 1; + +% Set the toolbox not to prompt the user (choose default values) + +dont_ask = 1; + +% Do not estimate distortion: + +est_dist = [0;0;0;0]; +est_dist = ones(4,1); + +center_optim = 1; + +% Run the main calibration routine: + +go_calib_optim_iter; + +% Shows the extrinsic parameters: + +dX = 3; +dY = 3; + +ext_calib; + +% Reprojection on the original images: + +reproject_calib; + + + + +%----------------------- Retrieve results: + +% Intrinsic: + +% Projector: +fp = fc; +cp = cc; +kp = kc; + +% Camera: +fc = fc_save; +cc = cc_save; +kc = kc_save; + +% Extrinsic: + +% Relative position of projector and camera: +T = Tc_1; +om = omc_1; +R = rodrigues(om); + +% Relative prosition of camera wrt world: +omc = omc_1_save; +Rc = Rc_1_save; +Tc = Tc_1_save; + +% relative position of projector wrt world: +Rp = R*Rc; +omp = rodrigues(Rp); +Tp = T + R*Tc; + +eval(['save calib_cam_proj R om T fc fp cc cp kc kp Rc Rp Tc Tp']); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m new file mode 100755 index 0000000..fc1820b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/readras.m @@ -0,0 +1,87 @@ +function [X, map] = readras(filename, ys, ye, xs, xe); +%READRAS Read an image file in sun raster format. +% READRAS('imagefile.ras') reads a "sun.raster" image file. +% [X, map] = READRAS('imagefile.ras') returns both the image and a +% color map, so that +% [X, map] = readras('imagefile.ras'); +% image(X) +% colormap(map) +% axis('equal') +% will display the result with the proper colors. +% NOTE: readras cannot deal with complicated color maps. +% In fact, Matlab doesn't quite allow to work with colormaps +% with more than 64 entries. +% + +%% +%% (C) Thomas K. Leung 3/30/93. +%% California Institute of Technology. +%% Modified by Andrea Mennucci to deal with color images +%% + +% PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if(strcmp(suffix, 'ras')) % raster file format % + fp = fopen(filename, 'rb'); + if(fp<0) error(['Cannot open ' filename '.']), end + + %Read and crack the 32-byte header + fseek(fp, 4, -1); + + width = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + height = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + depth = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + length = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + type = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); + + maplen = maplen / 3; + + if maptype == 2 % RMT_RAW + map = fread(fp, [maplen, 3], 'uchar')/255; +% if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end; + elseif maptype == 1 % RMT_EQUAL_RGB + map(:,1) = fread(fp, [maplen], 'uchar'); + map(:,2) = fread(fp, [maplen], 'uchar'); + map(:,3) = fread(fp, [maplen], 'uchar'); + %maxmap = max(max(map)); + map = map/255; + if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end; + else % RMT_NONE + map = []; + end +% if maplen>64, +% map=[map',zeros(3,256-maplen)]'; +% end; + + % Read the image + + if rem(width,2) == 1 + Xt = fread(fp, [width+1, height], 'uchar'); + X = Xt(1:width, :)'; + else + Xt = fread(fp, [width, height], 'uchar'); + X = Xt'; + end + X = X + 1; + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end + + +if nargin == 5 + + X = X(ys:ye, xs:xe); + +end \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m new file mode 100755 index 0000000..0909c69 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/recomp_corner_calib.m @@ -0,0 +1,119 @@ +% Re-select te corners after calibration + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +flag = 0; +for kk = ind_active, + if ~exist(['y_' num2str(kk)]), + flag = 1; + else + eval(['ykk = y_' num2str(kk) ';']); + if isnan(ykk(1,1)), + flag = 1; + end; + end; +end; + +if flag, + fprintf(1,'Need to calibrate once before before recomputing image corners. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + + +if ~exist(['I_' num2str(ind_active(1))]), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + disp('Cannot extract corners without images'); + return; + end; +end; + +fprintf(1,'\nRe-extraction of the grid corners on the images (after first calibration)\n'); + +disp('Window size for corner finder (wintx and winty):'); +wintx = input('wintx ([] = 5) = '); +if isempty(wintx), wintx = 5; end; +wintx = round(wintx); +winty = input('winty ([] = 5) = '); +if isempty(winty), winty = 5; end; +winty = round(winty); + +fprintf(1,'Window size = %dx%d\n',2*wintx+1,2*winty+1); + +ima_numbers = input('Number(s) of image(s) to process ([] = all images) = '); + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + +q_auto = input('Use the projection of 3D grid or manual click ([]=auto, other=manual): '); + +fprintf(1,'Processing image '); + +for kk = ima_proc; + + if active_images(kk), + + fprintf(1,'%d...',kk); + + if isempty(q_auto), + + eval(['I = I_' num2str(kk) ';']); + + eval(['y = y_' num2str(kk) ';']); + + xc = cornerfinder(y+1,I,winty,wintx); % the four corners + + eval(['wintx_' num2str(kk) ' = wintx;']); + eval(['winty_' num2str(kk) ' = winty;']); + + eval(['x_' num2str(kk) '= xc - 1;']); + + else + + fprintf(1,'\n'); + + click_ima_calib; + + end; + + else + + if ~exist(['omc_' num2str(kk)]), + + eval(['dX_' num2str(kk) ' = NaN;']); + eval(['dY_' num2str(kk) ' = NaN;']); + + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + + eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); + eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); + + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + + end; + + end; + + +end; + +% Recompute the error: + +comp_error_calib; + +fprintf(1,'\ndone\n'); \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m new file mode 100755 index 0000000..ccac7a5 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rect.m @@ -0,0 +1,127 @@ +function [Irec] = rect(I,R,f,c,k,alpha,KK_new); + + +if nargin < 5, + k = 0; + if nargin < 4, + c = [0;0]; + if nargin < 3, + f = [1;1]; + if nargin < 2, + R = eye(3); + if nargin < 1, + error('ERROR: Need an image to rectify'); + break; + end; + end; + end; + end; +end; + + +if nargin < 7, + if nargin < 6, + KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; + else + KK_new = alpha; % the 6th argument is actually KK_new + end; + alpha = 0; +end; + + + +% Note: R is the motion of the points in space +% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. + + +if ~exist('KK_new'), + KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1]; +end; + + +[nr,nc] = size(I); + +Irec = 255*ones(nr,nc); + +[mx,my] = meshgrid(1:nc, 1:nr); +px = reshape(mx',nc*nr,1); +py = reshape(my',nc*nr,1); + +rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; + + +% Rotation: (or affine transformation): + +rays2 = R'*rays; + +x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; + + +% Add distortion: + +k1 = k(1); +k2 = k(2); + +p1 = k(3); +p2 = k(4); + +r_2 = sum(x.^2); + +delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2) ; + p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; + +xd = (ones(2,1)*( 1 + k1 * r_2 + k2 * r_2.^2)) .* x + delta_x; + + +% Reconvert in pixels: + +px2 = f(1)*(xd(1,:)+alpha_c*xd(2,:))+c(1); +py2 = f(2)*xd(2,:)+c(2); + + +% Interpolate between the closest pixels: + +px_0 = floor(px2); +px_1 = px_0 + 1; +alpha_x = px2 - px_0; + +py_0 = floor(py2); +py_1 = py_0 + 1; +alpha_y = py2 - py_0; + +good_points = find((px_0 >= 0) & (px_1 <= (nc-1)) & (py_0 >= 0) & (py_1 <= (nr-1))); + +I_lu = I(px_0(good_points) * nr + py_0(good_points) + 1); +I_ru = I(px_1(good_points) * nr + py_0(good_points) + 1); +I_ld = I(px_0(good_points) * nr + py_1(good_points) + 1); +I_rd = I(px_1(good_points) * nr + py_1(good_points) + 1); + + +I_interp = (1 - alpha_y(good_points)).*((1 - alpha_x(good_points)).* I_lu + alpha_x(good_points) .* I_ru) + alpha_y(good_points) .* ((1 - alpha_x(good_points)).* I_ld + alpha_x(good_points) .* I_rd); + + +Irec((px(good_points)-1)*nr + py(good_points)) = I_interp; + + + +return; + + +% Convert in indices: + +fact = 3; + +[XX,YY]= meshgrid(1:nc,1:nr); +[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); + +%tic; +Iinterp = interp2(XX,YY,I,XXi,YYi); +%toc + +[nri,nci] = size(Iinterp); + + +ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; +ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; + +good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m new file mode 100755 index 0000000..d3ad3d2 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/reproject_calib.m @@ -0,0 +1,121 @@ +%%%%%%%%%%%%%%%%%%%% REPROJECT ON THE IMAGES %%%%%%%%%%%%%%%%%%%%%%%% + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +if ~exist('no_image'), + no_image = 0; +end; + +if ~exist('nx')&~exist('ny'), + fprintf(1,'WARNING: No image size (nx,ny) available. Setting nx=640 and ny=480\n'); + nx = 640; + ny = 480; +end; + + +check_active_images; + + +% Color code for each image: + +colors = 'brgkcm'; + +% Reproject the patterns on the images, and compute the pixel errors: + +% Reload the images if necessary + +if ~exist(['omc_' num2str(ind_active(1)) ]), + fprintf(1,'Need to calibrate before showing image reprojection. Maybe need to load Calib_Results.mat file.\n'); + return; +end; + +if ~no_image, + if ~exist(['I_' num2str(ind_active(1)) ]'), + n_ima_save = n_ima; + active_images_save = active_images; + ima_read_calib; + n_ima = n_ima_save; + active_images = active_images_save; + check_active_images; + if no_image_file, + fprintf(1,'WARNING: Do not show the original images\n'); %return; + end; + end; +else + no_image_file = 1; +end; + + +if ~exist('dont_ask'), + dont_ask = 0; +end; + + +if ~dont_ask, + ima_numbers = input('Number(s) of image(s) to show ([] = all images) = '); +else + ima_numbers = []; +end; + + +if isempty(ima_numbers), + ima_proc = 1:n_ima; +else + ima_proc = ima_numbers; +end; + + +figure(5); +for kk = ima_proc, %1:n_ima, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + eval(['plot(ex_' num2str(kk) '(1,:),ex_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); + hold on; + end; + end; +end; +hold off; +axis('equal'); +title('Reprojection error (in pixel)'); +xlabel('x'); +ylabel('y'); +drawnow; + +set(5,'Name','error','NumberTitle','off'); + + + +for kk = ima_proc, + if exist(['y_' num2str(kk)]), + if active_images(kk) & eval(['~isnan(y_' num2str(kk) '(1,1))']), + + if exist(['I_' num2str(kk)]), + eval(['I = I_' num2str(kk) ';']); + else + I = 255*ones(ny,nx); + end; + + figure(5+kk); + image(I); hold on; + colormap(gray(256)); + title(['Image ' num2str(kk) ' - Image points (+) and reprojected grid points (o)']); + eval(['plot(x_' num2str(kk) '(1,:)+1,x_' num2str(kk) '(2,:)+1,''r+'');']); + eval(['plot(y_' num2str(kk) '(1,:)+1,y_' num2str(kk) '(2,:)+1,''' colors(rem(kk-1,6)+1) 'o'');']); + zoom on; + axis([1 nx 1 ny]); + hold off; + drawnow; + + set(5+kk,'Name',num2str(kk),'NumberTitle','off'); + + end; + end; +end; + + +err_std = std(ex')'; + +fprintf(1,'Pixel error: err = [%3.5f %3.5f] (all active images)\n\n',err_std); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m new file mode 100755 index 0000000..473405c --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rigid_motion.m @@ -0,0 +1,66 @@ +function [Y,dYdom,dYdT] = rigid_motion(X,om,T); + +%rigid_motion.m +% +%[Y,dYdom,dYdT] = rigid_motion(X,om,T) +% +%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). +% +%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) +% (om,T): Rigid motion parameters between world coordinate frame and camera reference frame +% om: rotation vector (3x1 vector); T: translation vector (3x1 vector) +% +%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) +% dYdom: Derivative of Y with respect to om ((3N)x3 matrix) +% dYdT: Derivative of Y with respect to T ((3N)x3 matrix) +% +%Definitions: +%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) +%The coordinate vector of P in the camera reference frame is: Y = R*X + T +%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); +% +%Important function called within that program: +% +%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector + + + +if nargin < 3, + T = zeros(3,1); + if nargin < 2, + om = zeros(3,1); + if nargin < 1, + error('Need at least a 3D structure as input (in rigid_motion.m)'); + return; + end; + end; +end; + + +[R,dRdom] = rodrigues(om); + +[m,n] = size(X); + +Y = R*X + T*ones(1,n); + +if nargout > 1, + + +dYdR = zeros(3*n,9); +dYdT = zeros(3*n,3); + +dYdR(1:3:end,1:3:end) = X'; +dYdR(2:3:end,2:3:end) = X'; +dYdR(3:3:end,3:3:end) = X'; + +dYdT(1:3:end,1) = ones(n,1); +dYdT(2:3:end,2) = ones(n,1); +dYdT(3:3:end,3) = ones(n,1); + +dYdom = dYdR * dRdom; + +end; + + + + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m new file mode 100755 index 0000000..9d55337 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rodrigues.m @@ -0,0 +1,217 @@ +function [out,dout]=rodrigues(in) + +% RODRIGUES Transform rotation matrix into rotation vector and viceversa. +% +% Sintax: [OUT]=RODRIGUES(IN) +% If IN is a 3x3 rotation matrix then OUT is the +% corresponding 3x1 rotation vector +% if IN is a rotation 3-vector then OUT is the +% corresponding 3x3 rotation matrix +% + +%% +%% Copyright (c) March 1993 -- Pietro Perona +%% California Institute of Technology +%% + +%% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995. +%% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !! + +%% BUG when norm(om)=pi fixed -- April 6th, 1997; +%% Jean-Yves Bouguet + + +[m,n] = size(in); +%bigeps = 10e+4*eps; +bigeps = 10e+20*eps; + +if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector + theta = norm(in); + if theta < eps + R = eye(3); + + %if nargout > 1, + + dRdin = [0 0 0; + 0 0 1; + 0 -1 0; + 0 0 -1; + 0 0 0; + 1 0 0; + 0 1 0; + -1 0 0; + 0 0 0]; + + %end; + + else + if n==length(in) in=in'; end; %% make it a column vec. if necess. + + %m3 = [in,theta] + + dm3din = [eye(3);in'/theta]; + + omega = in/theta; + + %m2 = [omega;theta] + + dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; + + alpha = cos(theta); + beta = sin(theta); + gamma = 1-cos(theta); + omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; + A = omega*omega'; + + %m1 = [alpha;beta;gamma;omegav;A]; + + dm1dm2 = zeros(21,4); + dm1dm2(1,4) = -sin(theta); + dm1dm2(2,4) = cos(theta); + dm1dm2(3,4) = sin(theta); + dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]'; + + w1 = omega(1); + w2 = omega(2); + w3 = omega(3); + + dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; + dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; + dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; + + R = eye(3)*alpha + omegav*beta + A*gamma; + + dRdm1 = zeros(9,21); + + dRdm1([1 5 9],1) = ones(3,1); + dRdm1(:,2) = omegav(:); + dRdm1(:,4:12) = beta*eye(9); + dRdm1(:,3) = A(:); + dRdm1(:,13:21) = gamma*eye(9); + + dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; + + + end; + out = R; + dout = dRdin; + + %% it is prob. a rot matr. + elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... + & (abs(det(in)-1) < bigeps)) + R = in; + + + + tr = (trace(R)-1)/2; + dtrdR = [1 0 0 0 1 0 0 0 1]/2; + theta = real(acos(tr)); + + + if sin(theta) >= 1e-5, + + dthetadtr = -1/sqrt(1-tr^2); + + dthetadR = dthetadtr * dtrdR; + % var1 = [vth;theta]; + vth = 1/(2*sin(theta)); + dvthdtheta = -vth*cos(theta)/sin(theta); + dvar1dtheta = [dvthdtheta;1]; + + dvar1dR = dvar1dtheta * dthetadR; + + + om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; + + dom1dR = [0 0 0 0 0 1 0 -1 0; + 0 0 -1 0 0 0 1 0 0; + 0 1 0 -1 0 0 0 0 0]; + + % var = [om1;vth;theta]; + dvardR = [dom1dR;dvar1dR]; + + % var2 = [om;theta]; + om = vth*om1; + domdvar = [vth*eye(3) om1 zeros(3,1)]; + dthetadvar = [0 0 0 0 1]; + dvar2dvar = [domdvar;dthetadvar]; + + + out = om*theta; + domegadvar2 = [theta*eye(3) om]; + + dout = domegadvar2 * dvar2dvar * dvardR; + + + else + if tr > 0; % case norm(om)=0; + + out = [0 0 0]'; + + dout = [0 0 0 0 0 1/2 0 -1/2 0; + 0 0 -1/2 0 0 0 1/2 0 0; + 0 1/2 0 -1/2 0 0 0 0 0]; + else % case norm(om)=pi; %% fixed April 6th + + + out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); + %keyboard; + + if nargout > 1, + fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); + dout = NaN*ones(3,9); + end; + end; + end; + + else + error('Neither a rotation matrix nor a rotation vector were provided'); + end; + +return; + +%% test of the Jacobians: + +%%%% TEST OF dRdom: +om = randn(3,1); +dom = randn(3,1)/1000000; + +[R1,dR1] = rodrigues(om); +R2 = rodrigues(om+dom); + +R2a = R1 + reshape(dR1 * dom,3,3); + +gain = norm(R2 - R1)/norm(R2 - R2a) + +%%% TEST OF dOmdR: +om = randn(3,1); +R = rodrigues(om); +dom = randn(3,1)/10000; +dR = rodrigues(om+dom) - R; + +[omc,domdR] = rodrigues(R); +[om2] = rodrigues(R+dR); + +om_app = omc + domdR*dR(:); + +gain = norm(om2 - omc)/norm(om2 - om_app) + + +%%% OTHER BUG: (FIXED NOW!!!) + +omu = randn(3,1); +omu = omu/norm(omu) +om = pi*omu; +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + +%%% NORMAL OPERATION + +om = randn(3,1); +[R,dR]= rodrigues(om); +[om2] = rodrigues(R); +[om om2] + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m new file mode 100755 index 0000000..87ee2fe --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/rotation.m @@ -0,0 +1,23 @@ +function [] = rotation(st); + +if nargin < 1, + st= 1; +end; + + +fig = gcf; + +ax = gca; + +vv = get(ax,'view'); + +az = vv(1); +el = vv(2); + +for azi = az:-abs(st):az-360, + + set(ax,'view',[azi el]); + figure(fig); + drawnow; + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m new file mode 100755 index 0000000..095e17e --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/run_error_analysis.m @@ -0,0 +1,65 @@ +%%% Program that launchs the complete + +for N_ima_active = 1:30, + + error_analysis; + +end; + + + +return; + + +f = []; +f_std = []; + +c = []; +c_std = []; + +k = []; +k_std = []; + +NN = 30; + +for rr = 1:NN, + + load(['Calib_Accuracies_' num2str(rr)]); + + [m1,s1] = mean_std_robust(fc_list(1,:)); + [m2,s2] = mean_std_robust(fc_list(2,:)); + + f = [f [m1;m2]]; + f_std = [f_std [s1;s2]]; + + [m1,s1] = mean_std_robust(cc_list(1,:)); + [m2,s2] = mean_std_robust(cc_list(2,:)); + + c = [c [m1;m2]]; + c_std = [c_std [s1;s2]]; + + [m1,s1] = mean_std_robust(kc_list(1,:)); + [m2,s2] = mean_std_robust(kc_list(2,:)); + [m3,s3] = mean_std_robust(kc_list(3,:)); + [m4,s4] = mean_std_robust(kc_list(4,:)); + + k = [k [m1;m2;m3;m4]]; + k_std = [k_std [s1;s2;s3;s4]]; + +end; + +figure(1); +errorbar([1:NN;1:NN]',f',f_std'); +figure(2); +errorbar([1:NN;1:NN]',c',c_std'); +figure(3); +errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std'); + +figure(4); +semilogy(f_std'); + +figure(5); +semilogy(c_std'); + +figure(6); +semilogy(k_std'); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m new file mode 100755 index 0000000..a176e39 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveinr.m @@ -0,0 +1,46 @@ +%SAVEINR Write an INRIMAGE format file +% +% SAVEINR(filename, im) +% +% Saves the specified image array in a INRIA image format file. +% +% SEE ALSO: loadinr +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + +% Peter Corke 1996 + +function saveinr(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + + % build the header + hdr = []; + s = sprintf('#INRIMAGE-4#{\n'); + hdr = [hdr s]; + s = sprintf('XDIM=%d\n',c); + hdr = [hdr s]; + s = sprintf('YDIM=%d\n',r); + hdr = [hdr s]; + s = sprintf('ZDIM=1\n'); + hdr = [hdr s]; + s = sprintf('VDIM=1\n'); + hdr = [hdr s]; + s = sprintf('TYPE=float\n'); + hdr = [hdr s]; + s = sprintf('PIXSIZE=32\n'); + hdr = [hdr s]; + s = sprintf('SCALE=2**0\n'); + hdr = [hdr s]; + s = sprintf('CPU=sun\n#'); + hdr = [hdr s]; + + % make it 256 bytes long and write it + hdr256 = zeros(1,256); + hdr256(1:length(hdr)) = hdr; + fwrite(fid, hdr256, 'uchar'); + + % now the binary data + fwrite(fid, im', 'float32'); + fclose(fid) diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m new file mode 100755 index 0000000..0cee75d --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/savepgm.m @@ -0,0 +1,22 @@ +%SAVEPGM Write a PGM format file +% +% SAVEPGM(filename, im) +% +% Saves the specified image array in a binary (P5) format PGM image file. +% +% SEE ALSO: loadpgm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function savepgm(fname, im) + + fid = fopen(fname, 'w'); + [r,c] = size(im'); + fprintf(fid, 'P5\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + fwrite(fid, im', 'uchar'); + fclose(fid); diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m new file mode 100755 index 0000000..ece092b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saveppm.m @@ -0,0 +1,45 @@ +%SAVEPPM Write a PPM format file +% +% SAVEPPM(filename, I) +% +% Saves the specified red, green and blue planes in a binary (P6) +% format PPM image file. +% +% SEE ALSO: loadppm +% +% Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab + + +% Peter Corke 1994 + +function saveppm(fname, I) + +I = double(I); + +if size(I,3) == 1, + R = I; + G = I; + B = I; +else + R = I(:,:,1); + G = I(:,:,2); + B = I(:,:,3); +end; + +%keyboard; + + fid = fopen(fname, 'w'); + [r,c] = size(R'); + fprintf(fid, 'P6\n'); + fprintf(fid, '%d %d\n', r, c); + fprintf(fid, '255\n'); + R = R'; + G = G'; + B = B'; + im = [R(:) G(:) B(:)]; + %im = reshape(im,r,c*3); + im = im'; + %im = im(:); + fwrite(fid, im, 'uchar'); + fclose(fid); + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m new file mode 100755 index 0000000..3f98a8b --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/saving_calib.m @@ -0,0 +1,95 @@ + +if ~exist('n_ima')|~exist('fc'), + fprintf(1,'No calibration data available.\n'); + return; +end; + +check_active_images; + +if ~exist('solution_init'), solution_init = []; end; + +for kk = 1:n_ima, + if ~exist(['dX_' num2str(kk)]), eval(['dX_' num2str(kk) '= dX;']); end; + if ~exist(['dY_' num2str(kk)]), eval(['dY_' num2str(kk) '= dY;']); end; +end; + +if ~exist('param_list'), + param_list = solution; +end; + +if ~exist('wintx'), + wintx = []; + winty = []; +end; + +if ~exist('dX_default'), + dX_default = []; + dY_default = []; +end; + +if ~exist('alpha_c'), + alpha_c = 0; +end; + +for kk = 1:n_ima, + if ~exist(['y_' num2str(kk)]), + eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); + end; + if ~exist(['n_sq_x_' num2str(kk)]), + eval(['n_sq_x_' num2str(kk) ' = NaN;']); + eval(['n_sq_y_' num2str(kk) ' = NaN;']); + end; + if ~exist(['wintx_' num2str(kk)]), + eval(['wintx_' num2str(kk) ' = NaN;']); + eval(['winty_' num2str(kk) ' = NaN;']); + end; +end; + +save_name = 'Calib_Results'; + +if exist([ save_name '.mat'])==2, + disp('WARNING: File Calib_Results.mat already exists'); + pfn = -1; + cont = 1; + while cont, + pfn = pfn + 1; + postfix = ['_old'num2str(pfn)]; + save_name = [ 'Calib_Results' postfix]; + cont = (exist([ save_name '.mat'])==2); + end; + copyfile('Calib_Results.mat',[save_name '.mat']); + disp(['Copying the current Calib_Results.mat file to ' save_name '.mat']); +end; + + +save_name = 'Calib_Results'; + +if exist('calib_name'), + + fprintf(1,['\nSaving calibration results under ' save_name '.mat\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active center_optim est_alpha est_dist fc kc cc alpha_c ex x y solution solution_init wintx winty n_ima type_numbering N_slots small_calib_image first_num image_numbers format_image calib_name Hcal Wcal nx ny map dX_default dY_default KK inv_KK dX dY']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + +else + + fprintf(1,['\nSaving calibration results under ' save_name '.mat (no image version)\n']); + + string_save = ['save ' save_name ' center_optim param_list active_images ind_active center_optim est_alpha est_dist fc kc cc alpha_c ex x y solution solution_init wintx winty n_ima nx ny dX_default dY_default KK inv_KK dX dY']; + + for kk = 1:n_ima, + string_save = [string_save ' X_' num2str(kk) ' x_' num2str(kk) ' y_' num2str(kk) ' ex_' num2str(kk) ' omc_' num2str(kk) ' Rc_' num2str(kk) ' Tc_' num2str(kk) ' H_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk) ' wintx_' num2str(kk) ' winty_' num2str(kk) ' dX_' num2str(kk) ' dY_' num2str(kk)]; + end; + +end; + + + +%fprintf(1,'To load later click on Load\n'); + +eval(string_save); + +fprintf(1,'done\n'); \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m new file mode 100755 index 0000000..c5e5430 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/script_fit_distortion.m @@ -0,0 +1,39 @@ + + satis_distort = 0; + + disp(['Estimated focal: ' num2str(f_g) ' pixels']); + + while ~satis_distort, + + k_g = input('Guess for distortion factor kc ([]=0): '); + + if isempty(k_g), k_g = 0; end; + + xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g); + + xu = xy_corners_undist(1,:)'; + yu = xy_corners_undist(2,:)'; + + [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid + + XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu; + XX(1,:) = f_g*XX(1,:)+c_g(1); + XX(2,:) = f_g*XX(2,:)+c_g(2); + + figure(2); + image(I); + colormap(map); + zoom on; + hold on; + %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro'); + plot(XX(1,:),XX(2,:),'r+'); + title('The red crosses should be on the grid corners...'); + hold off; + + satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); + + satis_distort = ~isempty(satis_distort); + + + end; + \ No newline at end of file diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m new file mode 100755 index 0000000..aad0fa4 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/startup.m @@ -0,0 +1,9 @@ +% Main camera calibration toolbox: + +calib_gui; + +%calib_gui; + +path(pwd,path); + +format compact diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m new file mode 100755 index 0000000..d9a7574 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/undistort_image.m @@ -0,0 +1,193 @@ +%%% INPUT THE IMAGE FILE NAME: + +if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'), + fprintf(1,'No intrinsic camera parameters available.\n'); + return; +end; + +KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; + +disp('Program that undistorts images'); +disp('The intrinsic camera parameters are assumed to be known (previously computed)'); + +fprintf(1,'\n'); + +quest = input('Do you want to undistort all the calibration images ([],0) or a new image (1)? '); + +if isempty(quest), + quest = 0; +end; + +if ~quest, + + if ~exist(['I_' num2str(ind_active(1))]), + ima_read_calib; + end; + + check_active_images; + + format_image2 = format_image; + if format_image2(1) == 'j', + format_image2 = 'bmp'; + end; + + for kk = 1:n_ima, + + if exist(['I_' num2str(kk)]), + + eval(['I = I_' num2str(kk) ';']); + [I2] = rect(I,eye(3),fc,cc,kc,KK); + + if ~type_numbering, + number_ext = num2str(image_numbers(kk)); + else + number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); + end; + + ima_name2 = [calib_name '_rect' number_ext '.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...\n']); + + + if format_image2(1) == 'p', + if format_images2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + + end; + + end; + + fprintf(1,'done\n'); + +else + + dir; + fprintf(1,'\n'); + + image_name = input('Image name (full name without extension): ','s'); + + format_image2 = '0'; + +while format_image2 == '0', + + format_image2 = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); + + if isempty(format_image2), + format_image2 = 'ras'; + end; + + if lower(format_image2(1)) == 'm', + format_image2 = 'ppm'; + else + if lower(format_image2(1)) == 'b', + format_image2 = 'bmp'; + else + if lower(format_image2(1)) == 't', + format_image2 = 'tif'; + else + if lower(format_image2(1)) == 'p', + format_image2 = 'pgm'; + else + if lower(format_image2(1)) == 'j', + format_image2 = 'jpg'; + else + if lower(format_image2(1)) == 'r', + format_image2 = 'ras'; + else + disp('Invalid image format'); + format_image2 = '0'; % Ask for format once again + end; + end; + end; + end; + end; + end; +end; + +ima_name = [image_name '.' format_image2]; + + +%%% READ IN IMAGE: + +if format_image2(1) == 'p', + if format_image2(2) == 'p', + I = double(loadppm(ima_name)); + else + I = double(loadpgm(ima_name)); + end; +else + if format_image2(1) == 'r', + I = readras(ima_name); + else + I = double(imread(ima_name)); + end; +end; + +if size(I,3)>1, + I = I(:,:,2); +end; + + +if (size(I,1)>ny)|(size(I,2)>nx), + I = I(1:ny,1:nx); +end; + + + %% SHOW THE ORIGINAL IMAGE: + + figure(2); + image(I); + colormap(gray(256)); + title('Original image (with distortion) - Stored in array I'); + drawnow; + + + %% UNDISTORT THE IMAGE: + + fprintf(1,'Computing the undistorted image...') + + [I2] = rect(I,eye(3),fc,cc,kc,alpha_c,KK); + + fprintf(1,'done\n'); + + figure(3); + image(I2); + colormap(gray(256)); + title('Undistorted image - Stored in array I2'); + drawnow; + + + %% SAVE THE IMAGE IN FILE: + + ima_name2 = [image_name '_rect.' format_image2]; + + fprintf(1,['Saving undistorted image under ' ima_name2 '...']); + + if format_image2(1) == 'p', + if format_images2(2) == 'p', + saveppm(ima_name2,uint8(round(I2))); + else + savepgm(ima_name2,uint8(round(I2))); + end; + else + if format_image2(1) == 'r', + writeras(ima_name2,round(I2),gray(256)); + else + imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); + end; + end; + + fprintf(1,'done\n'); + +end; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m new file mode 100755 index 0000000..8946349 --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_convert.m @@ -0,0 +1,89 @@ +function [fc,cc,kc,Rc,Tc,omc,nx,ny] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + +%Conversion from Reg Willson's calibration format to my format + +% Conversion: + +% Focal length: +fc = [sx/dpx ; 1/dpy]*f; + +% Principal point; +cc = [Cx;Cy]; + +% Extrinsic parameters: +Rx = rodrigues([Rx;0;0]); +Ry = rodrigues([0;Ry;0]); +Rz = rodrigues([0;0;Rz]); + +Rc = Rz * Ry * Rx; + +omc = rodrigues(Rc); + +Tc = [Tx;Ty;Tz]; + + +% More tricky: Take care of the distorsion: + +Nfy = round(Nfx * 3/4); + +nx = Nfx; +ny = Nfy; + +% Select a set of DISTORTED coordinates uniformely distributed across the image: + +[xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy); + +xp_dist = xp_dist(:)'; +yp_dist = yp_dist(:)'; + + +% Apply UNDISTORTION according to Willson: + +xp_sensor_dist = dpx*(xp_dist - Cx)/sx; +yp_sensor_dist = dpy*(yp_dist - Cy); + +dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2); + +xp_sensor = xp_sensor_dist .* dist_fact; +yp_sensor = yp_sensor_dist .* dist_fact; + +xp = xp_sensor * sx / dpx + Cx; +yp = yp_sensor / dpy + Cy; + +ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1)); + +xp = xp(ind); +yp = yp(ind); +xp_dist = xp_dist(ind); +yp_dist = yp_dist(ind); + + +% Now, find my own set of parameters: + +x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)]; +x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)]; + +k = [0;0;0;0]; + +for kk = 1:5, + + [xd,dxddk] = apply_distortion(x,k); + + err = x_dist - xd; + + %norm(err) + + k_step = inv(dxddk'*dxddk)*(dxddk')*err(:); + + k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:); + + %norm(k_step)/norm(k) + + if norm(k_step)/norm(k) < 10e-10, + break; + end; + +end; + + +kc = k; diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m new file mode 100755 index 0000000..bbde63c --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/willson_read.m @@ -0,0 +1,59 @@ +% Read in Reg Willson's data file, and convert it into my data format: + +%dir; + +%calib_file = input('Reg Willson calibration file name: ','s'); + +if exist(calib_file), + + + load(calib_file); + + inddot = find(calib_file == '.'); + + if isempty(inddot), + varname = calib_file; + else + varname = calib_file(1:inddot(1)-1); + end; + + eval(['calib_params = ' varname ';']) + + Ncx = calib_params(1); + Nfx = calib_params(2); + dx = calib_params(3); + dy = calib_params(4); + dpx = calib_params(5); + dpy = calib_params(6); + Cx = calib_params(7); + Cy = calib_params(8); + sx = calib_params(9); + f = calib_params(10); + kappa1 = calib_params(11); + Tx = calib_params(12); + Ty = calib_params(13); + Tz = calib_params(14); + Rx = calib_params(15); + Ry = calib_params(16); + Rz = calib_params(17); + p1 = calib_params(18); + p2 = calib_params(19); + + % Conversion: + [fc,cc,kc,Rc_1,Tc_1,omc_1,nx,ny] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); + + KK = [fc(1) 0 cc(1);0 fc(2) cc(2) ; 0 0 1]; + + n_ima = 1; + + X_1 = [NaN;NaN;NaN]; + x_1 = [NaN;NaN]; + + map = gray(256); + +else + + disp(['WARNING: Calibration file ' calib_file ' not found']); + +end; + diff --git a/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m new file mode 100755 index 0000000..c7eb7bc --- /dev/null +++ b/SD-VBS/common/toolbox/toolbox_basic/TOOLBOX_calib/writeras.m @@ -0,0 +1,105 @@ +function writeras(filename, image, map); +%WRITERAS Write an image file in sun raster format. +% WRITERAS('imagefile.ras', image_matrix, map) writes a +% "sun.raster" image file. + +% Written by Thomas K. Leung 3/30/93. +% @ California Institute of Technology. + + +% PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998 + +dot = max(find(filename == '.')); +suffix = filename(dot+1:dot+3); + +if nargin < 3, + map = []; +end; + +if(strcmp(suffix, 'ras')) + %Write header + + fp = fopen(filename, 'wb'); + if(fp < 0) error(['Cannot open ' filename '.']), end + + [height, width] = size(image); + image = image - 1; + mapsize = size(map, 1)*size(map,2); + %fwrite(fp, ... + % [1504078485, width, height, 8, height*width, 1, 1, mapsize], ... + % 'long'); + + + zero_str = '00000000'; + + % MAGIC NUMBER: + + + fwrite(fp,89,'uchar'); + fwrite(fp,166,'uchar'); + fwrite(fp,106,'uchar'); + fwrite(fp,149,'uchar'); + + width_str = dec2hex(width); + width_str = [zero_str(1:8-length(width_str)) width_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar'); + end; + + + height_str = dec2hex(height); + height_str = [zero_str(1:8-length(height_str)) height_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,8,'uchar'); + + ll = height*width; + ll_str = dec2hex(ll); + ll_str = [zero_str(1:8-length(ll_str)) ll_str]; + + for ii = 1:2:7, + fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar'); + end; + + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,1,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,0,'uchar'); + fwrite(fp,~~mapsize,'uchar'); + + mapsize_str = dec2hex(mapsize); + mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str]; + + %keyboard; + + for ii = 1:2:7, + fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar'); + end; + + + if mapsize ~= 0 + map = min(255, fix(255*map)); + fwrite(fp, map, 'uchar'); + end + if rem(width,2) == 1 + image = [image'; zeros(1, height)]'; + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + else + top = 255 * ones(size(image)); + fwrite(fp, min(top,image)', 'uchar'); + end + fclose(fp); +else + error('Image file name must end in either ''ras'' or ''rast''.'); +end -- cgit v1.2.2