% makeC % This function is use to calculate camera calibration matrix from anim8or camera % pos and pyr are the camera location and orientation wrt the world coordinate. % For information about anim8or free modelling software, see www.anim8or.com % % % Usage: [C,U,Tcru,im] = makeC(pos,pyr,fov,imWH,fig,points,name) % % Arguments: pos - positionof camera in world frame [X,Y,Z] % pyr - pitch yaw row, the orientation of camera % [angleX,angleY,angleZ] % fov - field of view angle in degree % imWH - image width and height, [width, height] % [Optional] % fig - figNo = 1 : display 2D image and calculated points % figNo = 3 : display 3D position of camera and points % figNo = 2 : display both % all other values no figure display (default) % points - 3d points in homogeneous coordinate (4 x n) to check % camera model result % default points are world origin and unit i,j,k vector. % name - the name of 'ground truth' image projection % default value is % sprintf('%d_%d_%d_%d_%d_%d_fov%dimw%d.bmp',... % angleX,angleY,angleZ,X,Y,Z,fov,width); % % Returns: C - the 3 x 4 camera projection matrix % U - the image coordinate of points % in homogeneous coordinate (3 x n) % [Note that the image coordinate used here assume % origin at Top Left, x axis go Right, y axis go Down (TLRD)] % Tcru - Tcru*U is the points in CRU coordinate system % im - an approximated image(photo) of the points % through the given camera % % Author: % Tzu Yen Wong % wongt AT csse DOT uwa DOT edu DOT au % Department of Computer Science & Software Engineering % The University of Western Australia % % Created: July 2004 % improved: Nov 2004 % % Example % [C1,Urgb1,T]=makeC([-100 80 100],[-30 -135 0],30,[400 300],1,rgbp,'rgbx1.bmp'); %1234567890123456789012345678901234567890123456789012345678901234567890123456789 function [C,U,Tcru,im] = makeC(pos,pyr,fov,imWH,fig,points,name) silent = 1; pyrRad = pyr*pi/180; % assume pyr in degree, so convert to radians x_cam = pos(1); x_cam0 = pos(1); y_cam = pos(2); y_cam0 = pos(2); z_cam = pos(3); z_cam0 = pos(3); pos = [x_cam y_cam z_cam]'; % rotation is in leftthumb rule in anim8or. pitch = -pyrRad(1); pitch0 = pyr(1); yaw = -pyrRad(2); yaw0 = pyr(2); row = -pyrRad(3); row0 = pyr(3); w = imWH(1); h = imWH(2); % FOCAL LENGTH % because the xy axis is left up in anim8or but right down in P % it can be taken care of by giving it a negative focal length x=>-x, y=>-y % but that's just for projection, it'll make other calculation confusing % so we apply a 180 deg rotation about z axis instead. f = (w/2)/(tan((fov/2)*pi/180)); % CAMERA PROJECTION MATRIX P = [f 0 w/2 0 0 f h/2 0 0 0 1 0]; % RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR % ROTATION Rz = [cos(row) -sin(row) 0 sin(row) cos(row) 0 0 0 1]; Ry = [cos(yaw) 0 sin(yaw) 0 1 0 -sin(yaw) 0 cos(yaw)]; Rx = [1 0 0 0 cos(pitch) -sin(pitch) 0 sin(pitch) cos(pitch)]; % Rotation convention in Anim8or is PYR % pitch, Yaw, Row ==> y -> x -> z R = Ry * Rx * Rz; Rinv = Rz' * Rx' * Ry'; RR = [ R zeros(3,1) zeros(1,3) 1 ]; RRinv = [ Rinv zeros(3,1) zeros(1,3) 1 ]; % RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR % TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT % TRANSLATION t = pos; T = [eye(3) t zeros(1,3) 1 ]; Tinv = [eye(3) -t zeros(1,3) 1 ]; % TTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT % KKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK % Homogeneous transformation matrix % K : transform camera frame into world frame % but denote as w --> c ; K^w_c % Kinv : transform world frame into camera frame % but denote as w <-- c ; K^c_w K = T * RR; Kinv = RRinv * Tinv; % KKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKKK % CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC % Camera calibration matrix % first transform points in world frame into % the camera frame, then rotate 180 deg about z axis % (because the xy axis is left up in anim8or but right down in P) % then apply the Projection C = P*diag([-1 -1 1 1])*Kinv; % C = P*Kinv; % CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC eval('points;','points=[];') if isempty(points), % Test Points X1 = [10 0 100 1 ]'; X2 = [ 0 10 100 1 ]'; X3 = [ 0 0 110 1 ]'; X0 = [ 0 0 100 1 ]'; X = [X1 X2 X3 X0] ; elseif (size(points,1)~=3)&&(size(points,2)==3) X = points'; else X = points; end % UUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUU % Calculate the image coordinate of points % using the camera calibration matrix U = C*X; U = U./(ones(3,1)*U(3,:)); % UUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUUU % TCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRU % Transformation matrix to change points % from TLRD to CRU coordinate system Tcru = [1 0 -w/2;0 -1 h/2;0 0 1]; % TCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRUTCRU % IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII % make an approximate PHOTO of the points if nargout>=4, im = zeros(h,w); rc = round((U(1,:)-1))*h + round(U(2,:)); im(rc) = 255; im = uint8(im); end % show(im), hold on % plot(U(1,:),U(2,:),'r.') % plot(round(U(1,:)),round(U(2,:)),'bs') % IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII % World Coordinate Frame Orig = [10 0 0 1 0 10 0 1 0 0 10 1 0 0 0 1 ]'; % Transforming X into camera's coordinates Xincam = Kinv*X; % Calculating the camera coordinate frame % Not needed, just for reference Xcam = K*Orig; Xcam = Xcam./(ones(4,1)*Xcam(4,:)); % DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD % DDDDDDDDDDDDDDDDDDDDD Display on screen DDDDDDDDDDDDDDDDDDDDD details = sprintf('%s [%d %d %d]; %s [%d %d %d]; %s %d; %s %d',... 'Camera: Rotation xyz =',pyr(1),pyr(2),pyr(3),... 'translation xyz =',x_cam0,y_cam0,z_cam0,... 'fov =',fov,'image width =',w); if ~silent, disp('P, Camera Projection Matrix = ') disp(P) disp('Kinv, Homogeneous Transformation from World to Camera frame = ') disp(Kinv) disp('C = P*Kinv, Camera Calibration Matrix = ') disp(C) disp('Xcam = K*Orig, Camera frame in world coordinate = ') disp(Xcam) disp('Xincam = Kinv*X, Point X in camera coordinate = ') disp(Xincam) disp(details); disp('========================================') end % DDDDDDDDDDDDDDDDDDDDD Display on screen DDDDDDDDDDDDDDDDDDDDD % DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD % DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD % DDDDDDDDDDDDDDDDDDD Figure plot DDDDDDDDDDDDDDDDDDDDDDD eval('fig;','fig=0;') if fig == 3 || fig == 2, % 3d plotting figure%(2), clf, hold on plot3([Orig(1,1);Orig(1,4)],[Orig(2,1);Orig(2,4)],[Orig(3,1);Orig(3,4)],':r') plot3([Orig(1,2);Orig(1,4)],[Orig(2,2);Orig(2,4)],[Orig(3,2);Orig(3,4)],':g') plot3([Orig(1,3);Orig(1,4)],[Orig(2,3);Orig(2,4)],[Orig(3,3);Orig(3,4)],':b') if isempty(points), plot3([X(1,1);X(1,4)],[X(2,1);X(2,4)],[X(3,1);X(3,4)],'-ro') plot3([X(1,2);X(1,4)],[X(2,2);X(2,4)],[X(3,2);X(3,4)],'-go') plot3([X(1,3);X(1,4)],[X(2,3);X(2,4)],[X(3,3);X(3,4)],'-bo') else for ii = 1:size(X,2), plot3(X(1,ii),X(2,ii),X(3,ii),'ro') end end plot3([Xcam(1,1);Xcam(1,4)],[Xcam(2,1);Xcam(2,4)],[Xcam(3,1);Xcam(3,4)],'r') plot3([Xcam(1,2);Xcam(1,4)],[Xcam(2,2);Xcam(2,4)],[Xcam(3,2);Xcam(3,4)],'g') plot3([Xcam(1,3);Xcam(1,4)],[Xcam(2,3);Xcam(2,4)],[Xcam(3,3);Xcam(3,4)],'b') plot3([Orig(1,1);Orig(1,4)],[Orig(2,1);Orig(2,4)],[Orig(3,1);Orig(3,4)],':r') plot3([Orig(1,2);Orig(1,4)],[Orig(2,2);Orig(2,4)],[Orig(3,2);Orig(3,4)],':g') plot3([Orig(1,3);Orig(1,4)],[Orig(2,3);Orig(2,4)],[Orig(3,3);Orig(3,4)],':b') plot3([Xincam(1,1);Xincam(1,4)],... [Xincam(2,1);Xincam(2,4)],... [Xincam(3,1);Xincam(3,4)],'-rx') plot3([Xincam(1,2);Xincam(1,4)],... [Xincam(2,2);Xincam(2,4)],... [Xincam(3,2);Xincam(3,4)],'-gx') plot3([Xincam(1,3);Xincam(1,4)],... [Xincam(2,3);Xincam(2,4)],... [Xincam(3,3);Xincam(3,4)],'-bx') axis equal; end if fig == 2 || fig == 1, % checking projection plot figure;%(1), clf, eval('name;','name=[];') if isempty(name), name = sprintf('%d_%d_%d_%d_%d_%d_fov%dimw%d.bmp',... pyr(1),pyr(2),pyr(3),x_cam0,y_cam0,z_cam0,fov,w); end try img = imread(name); imshow(img); catch disp(sprintf('Cannot read file ''%s''',name)) end hold on, os = 0.5; if isempty(points), plot(os+[U(1,1);U(1,4)],os+[U(2,1);U(2,4)],'-r','EraseMode','xor'); plot(os+[U(1,2);U(1,4)],os+[U(2,2);U(2,4)],'-g','EraseMode','xor'); plot(os+[U(1,3);U(1,4)],os+[U(2,3);U(2,4)],'-b','EraseMode','xor'); plot(os+U(1,1),os+U(2,1),'ro'); plot(os+U(1,2),os+U(2,2),'go'); plot(os+U(1,3),os+U(2,3),'bo'); set(gcf,'name',details); plot(os+[U(1,4) 0;U(1,4) w],os+[0 U(2,4);h U(2,4)],':y'); else for ii = 1:size(X,2), plot(os+U(1,ii),os+U(2,ii),'ro'); text(os+U(1,ii),os+U(2,ii),sprintf('%d',ii)); end end axis ij; end % DDDDDDDDDDDDDDDDDDD Figure plot DDDDDDDDDDDDDDDDDDDDDDD % DDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDD