findlaserboardpoints

 FINDLASERBOARDPOINTS is the main automatic boar detection function.

 FINDLASERBOARDPOINTS is used to automatically detect and extract laser
 points belonging to the calibration plane.
 
 The measures relied on by the function are the frequency of occurence,
 the intial estimate and the matching of lengths.
 
 INPUTS:
     angleVector: 1xN vector; angleVector lists the angles for the ranges
     in rangeMatrix.
 
     rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan
     with ranges at the angles specified in angleVector.
 
     laserDivisor: laserDivisor is a scalar representing the resolution
     mode at which the SICK laser was operating.
 
     clstrs: MxN array. clstrs should be the same size as rangeMatrix.
     Each element in clstrs is an integer indicating the line cluster the
     range to which reading belongs.
 
     tiest: Initial estimate of translation vector. If tiest is not passed
     as input or passed as empty, the intial estimate is not used for
     board selection.
 
     riset: Initial estimate of rotation matrix.
 
     Nci: 3xM array containg the normal vector of the calibration plane in
     the camera coordinate frame.
 
     BoardCorners: 1xM cell array with each cell containing 3 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.
 
     thresholds: structure of one of the following forms:
         1. {fthlo,lenth} initial estimate is not used
         2. {fthlo,iesthlo,lenth} initial estimate is used
         3. {fthlo,fthhi,iesthlo,iesthhi,lenth,} used when lines near the
         borders of the classifier need to be user verified
     
     manselen: optional flag indicating wether the program should prompt the user
     for manual selection, default is 0.
 
     debug: flag for debugging
 
     manclstrs: for debugging
 
 OUTPUTS:
     clstrs: MxN array. Each element in clstrs is an integer indicating
     the line cluster the range to which reading belongs.
 
     selclstrs: Mx1 vector with the selected cluster of each scan (0=none)

0001 function [selclstrs,clstrs]=findlaserboardpoints(angleVector,rangeMatrix,laserDivisor,clstrs,tiest,riest,Nci,BoardCorners,thresholds,manselen,debug,manclstrs)
0002 % FINDLASERBOARDPOINTS is the main automatic boar detection function.
0003 %
0004 % FINDLASERBOARDPOINTS is used to automatically detect and extract laser
0005 % points belonging to the calibration plane.
0006 %
0007 % The measures relied on by the function are the frequency of occurence,
0008 % the intial estimate and the matching of lengths.
0009 %
0010 % INPUTS:
0011 %     angleVector: 1xN vector; angleVector lists the angles for the ranges
0012 %     in rangeMatrix.
0013 %
0014 %     rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan
0015 %     with ranges at the angles specified in angleVector.
0016 %
0017 %     laserDivisor: laserDivisor is a scalar representing the resolution
0018 %     mode at which the SICK laser was operating.
0019 %
0020 %     clstrs: MxN array. clstrs should be the same size as rangeMatrix.
0021 %     Each element in clstrs is an integer indicating the line cluster the
0022 %     range to which reading belongs.
0023 %
0024 %     tiest: Initial estimate of translation vector. If tiest is not passed
0025 %     as input or passed as empty, the intial estimate is not used for
0026 %     board selection.
0027 %
0028 %     riset: Initial estimate of rotation matrix.
0029 %
0030 %     Nci: 3xM array containg the normal vector of the calibration plane in
0031 %     the camera coordinate frame.
0032 %
0033 %     BoardCorners: 1xM cell array with each cell containing 3 elements:
0034 %         n_sq_x: number of squares of the calibration chessboard along the
0035 %         x direction.
0036 %
0037 %         n_sq_y: number of squares of the calibration chessboard along the
0038 %         y direction.
0039 %
0040 %         corners: 3x((n_sq_x+1)*(n_sq_y+1)) array with the coordinates of
0041 %         the chessboard corners in the camera frame.
0042 %
0043 %     thresholds: structure of one of the following forms:
0044 %         1. {fthlo,lenth} initial estimate is not used
0045 %         2. {fthlo,iesthlo,lenth} initial estimate is used
0046 %         3. {fthlo,fthhi,iesthlo,iesthhi,lenth,} used when lines near the
0047 %         borders of the classifier need to be user verified
0048 %
0049 %     manselen: optional flag indicating wether the program should prompt the user
0050 %     for manual selection, default is 0.
0051 %
0052 %     debug: flag for debugging
0053 %
0054 %     manclstrs: for debugging
0055 %
0056 % OUTPUTS:
0057 %     clstrs: MxN array. Each element in clstrs is an integer indicating
0058 %     the line cluster the range to which reading belongs.
0059 %
0060 %     selclstrs: Mx1 vector with the selected cluster of each scan (0=none)
0061 
0062 
0063 % Input checking
0064 if ~exist('debug','var') || isempty(debug)
0065     debug=0;
0066 end
0067 
0068 if ~exist('manclstrs','var') || isempty(manclstrs)
0069     getperf=0;
0070 else
0071     getperf=1;
0072 end
0073 
0074 if ~exist('manselen','var') || isempty(manselen)
0075     manselen=0;
0076 end
0077 
0078 % check iest
0079 if isempty(tiest)
0080     iestflag=0;
0081 else
0082     iestflag=1;
0083 end
0084 
0085 % check thresholds for his
0086 if isfield(thresholds,'fthhi')
0087     checksus=1;
0088 else
0089     checksus=0;
0090 end
0091 
0092 % get freq threshold
0093 fthlo=thresholds.fthlo;
0094 if checksus
0095     fthhi=thresholds.fthhi;
0096 end
0097 
0098 % get iest threshold
0099 if iestflag
0100     iestthlo=thresholds.iestthlo;
0101     if checksus
0102         iestthhi=thresholds.iestthhi;
0103     end
0104 end
0105 
0106 lenth=thresholds.lenth;
0107 
0108 % get nos
0109 noscans=size(rangeMatrix,1);
0110 
0111 % frequency scores
0112 fscores=getpecscores2(angleVector,rangeMatrix,laserDivisor);
0113 clstrfscores=getclstrscores(clstrs,fscores);
0114 clstrfscoresth=clstrfscores;
0115 clstrfscoresth(clstrfscoresth<fthlo)=0;
0116 
0117 
0118 
0119 % get mask for scans with no camcalib
0120 vscans=~isnan(Nci(1,:))';
0121 
0122 noccmask=zeros(size(rangeMatrix));
0123 noccmask(vscans,:)=1;
0124 noccmask=getclstrscores(clstrs,noccmask); % change into cluster information
0125 
0126 if iestflag
0127     
0128     % debug initial estimate
0129     initestdebug=0;
0130     if debug
0131         debin=input('Do you want to display the boards in the laser scans? (y/n,[]=n)','s');
0132         if debin=='y'
0133             initestdebug=1;
0134         end
0135     end
0136     
0137     % get iestscores
0138     iestscores=getinitestscore(tiest,riest,Nci,BoardCorners,angleVector,rangeMatrix,initestdebug);
0139     clstriestscores=getclstrscores(clstrs,iestscores);
0140     clstriestscoresth=clstriestscores;
0141     clstriestscoresth(clstriestscoresth<iestthlo)=0;
0142 else
0143     clstriestscores=noccmask;
0144     clstriestscoresth=clstriestscores;
0145 end
0146 
0147 % get length scores
0148 lengths=getclstrlengths(angleVector,rangeMatrix,clstrs);
0149 clstrscoresi=clstrfscoresth.*clstriestscoresth;
0150 
0151 [m,mi]=max(clstrscoresi.*logical(lengths),[],2); % do not choose zero length clusters
0152 lind=sub2ind(size(lengths),(1:noscans)',mi);
0153 lind(m==0)=[]; % remove scans with all zero scores
0154 lenvec=lengths(lind); % get lengths
0155 % choose the 99th percentile length (safer than the maximum in removing
0156 % outliers)
0157 lenvec=sort(lenvec);
0158 np=0.99*length(lenvec);
0159 lenbar=interp1(lenvec,np);
0160 
0161 lengthscores=1-abs(lengths-lenbar)./lenbar;
0162 lengthscores(lengthscores<0)=0; % clip negative scores
0163 
0164 % threshold length scores
0165 lengthscoresth=lengthscores;
0166 lengthscoresth(lengthscoresth<lenth)=0;
0167 
0168 
0169 % get final scores
0170 clstrscores=clstrfscoresth.*clstriestscoresth.*lengthscoresth;
0171 [maxscores,selclstrs]=max(clstrscores,[],2);
0172 
0173 % vscans=max(noccmask,[],2);
0174 sucscans=logical(maxscores);
0175 si=sub2ind(size(lengths),(1:noscans)',selclstrs);
0176 if checksus
0177     if iestflag
0178         susscans=(clstrfscores(si)<fthhi | clstriestscores(si)<iestthhi) & sucscans;
0179     else
0180         susscans=clstrfscores(si)<fthhi & sucscans;
0181     end
0182 end
0183 fscans=vscans&(~sucscans);
0184 
0185 % just check for rotation for SelectLaserPoints2 (guard)
0186 if ~exist('riest','var') || isempty(riest)
0187     riest=angvec2dcm([0;0;0]);
0188 end
0189 
0190 if checksus && ~isempty(find(susscans,1))
0191     disp('Please validate the selected board lines in the following scans:');
0192     for cntr=find(susscans')
0193         figure;
0194         img=GetImage(cntr);
0195         if ~isempty(img);
0196             subplot 121;
0197         end
0198         orientation=dcm2angvec(riest);
0199         orientation=orientation(3);
0200         dispclstrscore(angleVector+orientation,rangeMatrix(cntr,:),clstrs(cntr,:));
0201         title(['Scan: ',num2str(cntr)]);
0202         if ~isempty(img)
0203             subplot 122;
0204             imshow(img,[]);
0205         end
0206         fprintf('The automatically selected cluster is %d.',selclstrs(cntr));
0207         if isempty(img)
0208             fprintf('Image does not exist.');
0209         end
0210         fprintf('\n');
0211         mansel=input('If necessary, select a different one ([]=no change,0=none):');
0212         close;
0213         if ~isempty(mansel)
0214             selclstrs(cntr)=mansel;
0215             sucscans(cntr)=logical(mansel);
0216         end
0217     end
0218 end
0219 
0220 
0221 if ~isempty(find(fscans,1)) && manselen
0222     manselyn=input('\nWould you like to manually select the failed scans? (y/n,[]=n)','s');
0223     if manselyn=='y'
0224         f=figure;
0225         uisuspend(f);
0226         for cntr=find(fscans')
0227             fprintf('Select points from scan no %d.',cntr);
0228             img=GetImage(cntr);
0229             if ~isempty(img);
0230                 maximize(f);
0231                 subplot 122;
0232                 imshow(img,[]);
0233                 subplot 121;
0234             else
0235                 fprintf('Image does not exist.');
0236             end
0237             fprintf('\n');
0238             orientation=dcm2angvec(riest);
0239             orientation=orientation(3);
0240             selind = SelectLaserPoints(angleVector+orientation,rangeMatrix(cntr,:));
0241             clf;
0242             if isempty(selind)
0243                 continue;
0244             end
0245             if ~selind
0246                 break;
0247             end
0248             clstrs(cntr,:)=selind+1;
0249             selclstrs(cntr)=2;
0250             sucscans(cntr)=1;
0251         end
0252         close(f);
0253     end
0254 end
0255 
0256 selclstrs=selclstrs.*sucscans;
0257 
0258 %
0259 % Debugging
0260 %
0261 
0262 debugscans=1:noscans;
0263 
0264 if getperf
0265     figure;
0266     manclstrscores=getclstrscores(clstrs,manclstrs);
0267     % threshold
0268     manclstrscoresth=manclstrscores;
0269     manclstrscoresth(manclstrscoresth<1.5)=0;
0270     [benchscores,benchvec]=max(manclstrscoresth,[],2);
0271     benchvec=benchvec.*logical(benchscores); % remove zeros
0272     scannos=(1:noscans)';
0273     bind=sub2ind(size(manclstrscores),scannos(benchvec>0),benchvec(benchvec>0));
0274     manclstrscoresb=zeros(size(manclstrscores));
0275     manclstrscoresb(bind)=1;
0276     scatter3(clstrfscores(:),clstriestscores(:),lengthscores(:),20,manclstrscoresb(:),'filled');
0277     xlabel('Frequency');
0278     ylabel('InitEst');
0279     zlabel('Length');
0280     set(gcf,'renderer','opengl');
0281 %     trainvec=trainvec.*logical(trainscores);
0282     fprintf('\n');
0283     notruepos=nnz((benchvec==selclstrs).*vscans.*benchvec)
0284     notrueneg=nnz((benchvec==selclstrs).*vscans.*(~benchvec))
0285     falsepos=find((benchvec~=selclstrs).*vscans.*(~benchvec))
0286     falseneg=find((benchvec~=selclstrs).*vscans.*benchvec)
0287     noinvscans=nnz(~vscans)
0288     fprintf('\n');
0289     falsescans=find((benchvec~=selclstrs).*vscans);
0290     nofalsescans=nnz((benchvec~=selclstrs).*vscans)
0291     debugscans=falsescans';
0292 end
0293 
0294 if debug
0295     if ~getperf
0296         debin=input('Do you want to display all scans? (y/n,[]=n)','s');
0297     else
0298         debin='y';
0299     end
0300     if debin=='y'
0301         figure;
0302         for cntr=debugscans
0303             clf;
0304             dispclstrscore(angleVector,rangeMatrix(cntr,:),clstrs(cntr,:));
0305             title(num2str(cntr));
0306             disp(['Selected cluster is ',num2str(selclstrs(cntr)),'.']);
0307         end
0308     end
0309 end