GetCameraPlanes

 GETCAMERAPLANES gets the calibration planes from the camera calibration results.
 
 GETCAMERAPLANES retrieves the calibration planes from the camera
 calibration results in the form of normal vectors and chessboard corner
 coordinates.
 
 USAGE:
   [cameraPlanes] = GetCameraPlanes(fname,noscans)
   After running the Camera Calibration Toolbox and saving Calib_Results.mat
   This function will load that file (current directory assumed)
   For every available Rc_n and Tc_n now in the workspace it will create a
   normal vector N ~ 3x1 describing the plane.  Magnitude of vector is the
   distance to the plane from the camera origin in metres

   NOTE:   Magnitude describes distance to plane in METRES
           Vector is from plane to camera origin
 
 INPUTS:
     fname: name of camera calibration file.
 
     noscans: the total number of laser scans.
 
 OUTPUTS:
     cameraPlanes: 3xnoscans array containing the normal vector of the
     calibration plane of the corresponding laser scan.
 
     BoardCorners: is a 1xnoscans array of structures. Each structure has
     the following elements:
 
         n_sq_x: number of squares of the calibration chessboard along the
         x direction.
 
         n_sq_y: number of squares of the calibration chessboard along the
         y direction.
 
         corners: 3x((n_sq_x+1)*(n_sq_y+1)) array with the coordinates of
         the chessboard corners in the camera frame.

 Written by James Underwood 10/07/06

 Modified by Abdallah Kassir 1/3/2010

0001 function [cameraPlanes,BoardCorners] = GetCameraPlanes(fname,noscans)
0002 % GETCAMERAPLANES gets the calibration planes from the camera calibration results.
0003 %
0004 % GETCAMERAPLANES retrieves the calibration planes from the camera
0005 % calibration results in the form of normal vectors and chessboard corner
0006 % coordinates.
0007 %
0008 % USAGE:
0009 %   [cameraPlanes] = GetCameraPlanes(fname,noscans)
0010 %   After running the Camera Calibration Toolbox and saving Calib_Results.mat
0011 %   This function will load that file (current directory assumed)
0012 %   For every available Rc_n and Tc_n now in the workspace it will create a
0013 %   normal vector N ~ 3x1 describing the plane.  Magnitude of vector is the
0014 %   distance to the plane from the camera origin in metres
0015 %
0016 %   NOTE:   Magnitude describes distance to plane in METRES
0017 %           Vector is from plane to camera origin
0018 %
0019 % INPUTS:
0020 %     fname: name of camera calibration file.
0021 %
0022 %     noscans: the total number of laser scans.
0023 %
0024 % OUTPUTS:
0025 %     cameraPlanes: 3xnoscans array containing the normal vector of the
0026 %     calibration plane of the corresponding laser scan.
0027 %
0028 %     BoardCorners: is a 1xnoscans array of structures. Each structure has
0029 %     the following elements:
0030 %
0031 %         n_sq_x: number of squares of the calibration chessboard along the
0032 %         x direction.
0033 %
0034 %         n_sq_y: number of squares of the calibration chessboard along the
0035 %         y direction.
0036 %
0037 %         corners: 3x((n_sq_x+1)*(n_sq_y+1)) array with the coordinates of
0038 %         the chessboard corners in the camera frame.
0039 %
0040 % Written by James Underwood 10/07/06
0041 %
0042 % Modified by Abdallah Kassir 1/3/2010
0043 
0044 
0045 cameraPlanes=[];
0046 load(fname);
0047 
0048 stringRBase = 'Rc_';
0049 stringTBase = 'Tc_';
0050 
0051 base = 1;
0052 %for n = selectionNumbers
0053 while( exist([stringRBase,num2str(base)]) && exist([stringTBase,num2str(base)]) )
0054   
0055     rc = eval([stringRBase,num2str(base)]);
0056     tc = eval([stringTBase,num2str(base)]);
0057     
0058     plane = -rc(:,3) * dot(rc(:,3)', tc); % see cam/laser paper
0059     plane = -plane./1000; % in mm not m and from camera to plane not the other way around
0060     cameraPlanes=[cameraPlanes,plane];
0061     
0062     base = base + 1;
0063 end
0064 
0065 if isempty(cameraPlanes)
0066     error('No Rc_# or Tc_# variables found in Calib_Results.mat - check the camera calibration');
0067 end
0068 
0069 stringXBase = 'X_';
0070 stringnsqxBase='n_sq_x_';
0071 stringnsqyBase='n_sq_y_';
0072 
0073 kk=1;
0074 while exist([stringXBase,num2str(kk)],'var')
0075     rc = eval([stringRBase,num2str(kk)]);
0076     tc = eval([stringTBase,num2str(kk)]);
0077     x  = eval([stringXBase,num2str(kk)]);
0078     BoardCorners(kk).n_sq_x = eval([stringnsqxBase,num2str(kk)]);
0079     BoardCorners(kk).n_sq_y = eval([stringnsqyBase, num2str(kk)]);
0080     BoardCorners(kk).corners=(rc * x + tc * ones(1,size(x,2)))./1000; % in m
0081     kk=kk+1;
0082 end
0083 
0084 % fix sizes to noscans
0085 if size(cameraPlanes,2)<noscans
0086     for cntr=size(cameraPlanes,2)+1:noscans
0087         cameraPlanes(:,cntr)=NaN;
0088         BoardCorners(cntr).n_sq_x=NaN;
0089         BoardCorners(cntr).n_sq_y=NaN;
0090         BoardCorners(cntr).corners=NaN;
0091     end
0092 elseif size(cameraPlanes,2)>noscans
0093     % trim
0094     cameraPlanes=cameraPlanes(:,1:noscans);
0095     BoardCorners=BoardCorners(1:1:noscans);
0096 end