GetCameraLaserCalibrationData

 GETCAMERALASERCALIBRATIONDATA is an auxiliary function used to setup the data for calibration.
 
 GETCAMERALASERCALIBRATIONDATA sets up the data for calibration and optionally
 displays the the calibration points all on one figure when dflag is
 passed as 1.
 
 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.
 
     clstrs: MxN array. Each element in clstrs is an integer indicating
     the line cluster the range to which reading belongs.
 
     boardclstrs: Mx1 vector with the selected cluster of each scan (0=none)
 
     Nci: 3xM array containing the normal vector of the
     calibration plane of the corresponding laser scan.
 
     dflag: for debugging
 
 OUTPUTS:
     laserPoints: 3xN array containing all N points to be used for the
     calibration process.
     
     cameraPlanes: 3xN array containing the corresponding normal vectors for the
     points in Lpts.
     
     scannos: 3xN array containing the corresponding scan numbers of the
     points in Lpts.
 
 Abdallah Kassir 1/3/2010

0001 function [laserPoints,cameraPlanes,scannos] = GetCameraLaserCalibrationData(selectionNumbers,rangeMatrix,angleVector,clstrs,boardclstrs,Nci,dflag)
0002 % GETCAMERALASERCALIBRATIONDATA is an auxiliary function used to setup the data for calibration.
0003 %
0004 % GETCAMERALASERCALIBRATIONDATA sets up the data for calibration and optionally
0005 % displays the the calibration points all on one figure when dflag is
0006 % passed as 1.
0007 %
0008 % INPUTS:
0009 %     angleVector: 1xN vector; angleVector lists the angles for the ranges
0010 %     in rangeMatrix.
0011 %
0012 %     rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan
0013 %     with ranges at the angles specified in angleVector.
0014 %
0015 %     clstrs: MxN array. Each element in clstrs is an integer indicating
0016 %     the line cluster the range to which reading belongs.
0017 %
0018 %     boardclstrs: Mx1 vector with the selected cluster of each scan (0=none)
0019 %
0020 %     Nci: 3xM array containing the normal vector of the
0021 %     calibration plane of the corresponding laser scan.
0022 %
0023 %     dflag: for debugging
0024 %
0025 % OUTPUTS:
0026 %     laserPoints: 3xN array containing all N points to be used for the
0027 %     calibration process.
0028 %
0029 %     cameraPlanes: 3xN array containing the corresponding normal vectors for the
0030 %     points in Lpts.
0031 %
0032 %     scannos: 3xN array containing the corresponding scan numbers of the
0033 %     points in Lpts.
0034 %
0035 % Abdallah Kassir 1/3/2010
0036 
0037 if ~exist('dflag','var') || isempty(dflag)
0038     dflag=0;
0039 end
0040 cameraPlanes=[];
0041 laserPoints=[];
0042 scannos=[];
0043 validscans=[];
0044 
0045 
0046 % change to cartesian coordinates
0047 [z,x]=pol2cart(repmat(angleVector,size(rangeMatrix,1),1),rangeMatrix);
0048 
0049 %for each selection
0050 for selection = selectionNumbers
0051     
0052     %Ask user to segment laser scan corresponding to plane
0053     ptsvec=[];
0054     if boardclstrs(selection)~=0
0055         ptsvec=find(clstrs(selection,:)==boardclstrs(selection));
0056     else
0057         continue;
0058     end
0059     
0060 %     selectedRanges=laserRangeData(selection,ptsvec);
0061 %     selectedAngles=laserAngleVector(ptsvec);
0062 
0063 
0064     newLaserPoints = [x(selection,ptsvec); zeros(size(ptsvec)); z(selection,ptsvec)];
0065     %For the current paired laser scan and camera plane, the one plane
0066     %describes all laser points - here the appropriate plane is copied N
0067     %times where N is the number of laser points
0068     [newCameraPlanes] = repmat(Nci(:,selection),1,size(newLaserPoints,2));
0069     cameraPlanes=[cameraPlanes,newCameraPlanes];
0070     laserPoints=[laserPoints,newLaserPoints];
0071     scannos=[scannos,repmat(selection,1,size(newLaserPoints,2))];
0072     validscans=[validscans,selection];
0073 end
0074 
0075 if dflag
0076     %Plot the selected laser scans from 'above' - the scans should all be
0077     %planar in the appropriate orientations
0078     figure;
0079     plot(laserPoints(1,:),laserPoints(3,:),'r.',0,0,'bo');axis equal;grid on
0080     legend('Laser Points','Laser Origin');
0081     title('Laser Points on Board Planes');
0082     xlabel('x');
0083     ylabel('z');
0084     %
0085     % %Total amounts of data selected
0086     fprintf( 'Planes selected:');disp(validscans);
0087     fprintf( 'Total of %d planes selected, with a total of %d data points.\n', length(validscans),size(laserPoints,2));
0088     drawnow;
0089 end