gusucode.com > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数 > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数/TOOLBOX_calib/Distor2Calib.m
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)