auto_select_cb

 AUTO_SELECT_CB is the callback file for the autoselection process.

 AUTO_SELECT_CB extracts the calibration board lines from the laser scans.
 AUTO_SELECT_CB can also attempt to guess the initial estimate of the
 calibration parameters.
 
 ASSUMPTIONS:
     AUTO_SELECT_CB assumes that most of the laser scans contain the
     calibration board. It also assumes that the calibration board is
     moved around throughout the scans. If these assumptions are not
     valid, please use the Manual select method.
 
 Abdallah Kassir 1/3/2010

0001 % AUTO_SELECT_CB is the callback file for the autoselection process.
0002 %
0003 % AUTO_SELECT_CB extracts the calibration board lines from the laser scans.
0004 % AUTO_SELECT_CB can also attempt to guess the initial estimate of the
0005 % calibration parameters.
0006 %
0007 % ASSUMPTIONS:
0008 %     AUTO_SELECT_CB assumes that most of the laser scans contain the
0009 %     calibration board. It also assumes that the calibration board is
0010 %     moved around throughout the scans. If these assumptions are not
0011 %     valid, please use the Manual select method.
0012 %
0013 % Abdallah Kassir 1/3/2010
0014 
0015 
0016 if ~exist('.\Calib_Results.mat','file')
0017     disp('Calib_Results.mat is needed to proceed.');
0018     return;
0019 end
0020 if ~exist('rangeMatrix','var')
0021     disp('Range data needed, run Read data or Load.');
0022     return;
0023 end
0024 
0025 disp('Automatic board selection.');
0026 
0027 %% get line clstrs
0028 disp('Line extraction:');
0029 getclstrsrf=0;
0030 if ~exist('roughth','var')
0031     roughth=0.05; % default
0032 end
0033 roughthin=input(['Enter rough line threshold (in m) ([]=',num2str(roughth),'):']);
0034 if ~isempty(roughthin)
0035     if roughth~=roughthin
0036         getclstrsrf=1;
0037     end
0038     roughth=roughthin;
0039 end
0040 
0041 getclstrsff=0;
0042 if ~exist('fineth','var')
0043     fineth=0.02; % default
0044 end
0045 finethin=input(['Enter fine line threshold (in m) ([]=',num2str(fineth),'):']);
0046 if ~isempty(finethin)
0047     if fineth~=finethin
0048         getclstrsff=1;
0049     end
0050     fineth=finethin;
0051 end
0052 
0053 if ~exist('clstrsr','var') || getclstrsrf
0054     disp('Extracting rough lines:');
0055     clstrsr=getedgelineclstrs(angleVector,rangeMatrix,roughth);
0056 end
0057 
0058 if ~exist('clstrsf','var') || getclstrsff
0059     disp('Extracting fine lines:');
0060     clstrsf=getedgelineclstrs(angleVector,rangeMatrix,fineth);
0061 end
0062 
0063 %% stage 1 Get initial estimate
0064 disp('Initial estimate:');
0065 autoinitest=input('Do you want the program to automatically find the initial estimate? (y/n, []=y)','s');
0066 
0067 % get planes from camera calibration data
0068 [Nci,BoardCorners]=GetCameraPlanes('Calib_Results.mat',size(rangeMatrix,1));
0069 
0070 
0071 if autoinitest=='n'
0072     % Manual input of initial estimate
0073     if ~exist('deltaest','var')
0074         deltaest=[0;0;0];
0075     end
0076     deltaestin=input(['Enter initial estimate for translation vector ([]=',mat2str(deltaest',3),'):']);
0077     if ~isempty(deltaestin)
0078         deltaest=deltaestin(:);  % col vector
0079     end
0080 
0081     if ~exist('phiest','var')
0082         phiest=angvec2dcm([0;0;0]);
0083     end
0084     phiestin=input(['Enter initial estimate for rotation vector ([]=',mat2str(rad2deg(dcm2angvec(phiest))',3),'):']);
0085     if ~isempty(phiestin)
0086         % change into radians and rotation matrix
0087         phiest=angvec2dcm(deg2rad(phiestin(:))); % col vector
0088     end
0089 else
0090     % Automatic guess of initial estimate
0091     clear thresholds;
0092     % set thresholds
0093     thresholds.fthlo=0.9;
0094     thresholds.lenth=0.5;
0095     boardclstrs=findlaserboardpoints(angleVector,rangeMatrix,laserDivisor,clstrsr,[],[],Nci,BoardCorners,thresholds);
0096     [Lpts,Nc,Lptsnos] = GetCameraLaserCalibrationData(find(boardclstrs)',rangeMatrix,angleVector,clstrsr,boardclstrs,Nci);
0097     [deltaest,phiest] = getinitest(Lpts, Nc);
0098     rmserror=geterror(Lpts,Nc,deltaest,phiest);
0099     disp(['Initial estimate: delta:',mat2str(deltaest',3),', phi:',mat2str(rad2deg(dcm2angvec(phiest))',3),', rms error:',num2str(rmserror,3)]);
0100 end
0101 
0102 
0103 
0104 %% stage 2 optimise transformation
0105 disp('Optimising estimate:');
0106 boardclstrs=zeros(size(rangeMatrix,1),1);
0107 
0108 for cntr=1:10
0109     boardclstrspre=boardclstrs;
0110     clear thresholds;
0111     thresholds.fthlo=0.8;
0112     thresholds.iestthlo=0.8;
0113     thresholds.lenth=0.5;
0114 
0115     boardclstrs=findlaserboardpoints(angleVector,rangeMatrix,laserDivisor,clstrsr,deltaest,phiest,Nci,BoardCorners,thresholds);
0116     [Lpts,Nc,Lptsnos] = GetCameraLaserCalibrationData(find(boardclstrs)',rangeMatrix,angleVector,clstrsr,boardclstrs,Nci);
0117     [deltaest,phiest] = getinitest(Lpts, Nc,deltaest,phiest);
0118     rmserror=geterror(Lpts,Nc,deltaest,phiest);
0119     disp(['Initial estimate: delta:',mat2str(deltaest',3),', phi:',mat2str(rad2deg(dcm2angvec(phiest))',3),', rms error:',num2str(rmserror,3)]);
0120     
0121     if isempty(find(boardclstrs~=boardclstrspre,1))
0122         break;
0123     end
0124 end
0125 
0126 %% stage 3 get final board points
0127 disp('Final Stage:');
0128 clear thresholds;
0129 thresholds.fthlo=0.8;
0130 thresholds.fthhi=0.9;
0131 thresholds.iestthlo=0.8;
0132 thresholds.iestthhi=0.9;
0133 thresholds.lenth=0.5;
0134 disp('Automatic board detection:');
0135 % clstrs needs to be output since it adjusted by the manual selection
0136 % inside findlaserboardpoints
0137 [boardclstrs,clstrs]=findlaserboardpoints(angleVector,rangeMatrix,laserDivisor,clstrsf,deltaest,phiest,Nci,BoardCorners,thresholds,1);
0138 selectionnumbers=find(boardclstrs)'; % set selection nmubers for calib
0139 disp('Automatic board detection done.');
0140 
0141 
0142 % display results
0143 dispboardpts(angleVector,rangeMatrix,clstrs,boardclstrs,selectionnumbers);
0144