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