QBuildLaserRangeAngle

   USAGE:
       [rangeMatrix, angleVector] = QBuildLaserRangeAngle( laserLogData )
       Loads a generic range bearing ascii log line (logged with Q library)
       and produces a very generic range and angle format for the data

   INPUT:
       laserLogData - variable containing laser range data in Q generic rb format
           size is R rows by C columns, R is the number of scans
           format:
           <timestamp> <initial angle> <angle increment> <final angle> <range type int>
           <nscans> <range1> <range2> ... <range N>
           all in the row

   OUTPUT:
       rangeMatrix - variable containing laser range data in rows
           size is R rows by C columns, R is the number of scans and C is the points per scan
               eg a typical SICK laser scan might have [39,181] if there were
               39 scans and the mode was 180 degrees, 1 degree resolution
           NOTE: The ranges are in METRES

       angleVector - variable containing a single row of angles in radians corresponding
           to all of the laserRangeData matrix
           If laserRangeData is R by C, laserAngleVector must be 1 by C
           NOTE:   The angles are in RADIANS
                   0 angle points forwards out of the laser, positive angles
                   are clockwise from top down

 Written by James Underwood 10/07/06

0001 function   [rangeMatrix, angleVector, laserDivisor] = QBuildLaserRangeAngle( laserLogData )
0002 %   USAGE:
0003 %       [rangeMatrix, angleVector] = QBuildLaserRangeAngle( laserLogData )
0004 %       Loads a generic range bearing ascii log line (logged with Q library)
0005 %       and produces a very generic range and angle format for the data
0006 %
0007 %   INPUT:
0008 %       laserLogData - variable containing laser range data in Q generic rb format
0009 %           size is R rows by C columns, R is the number of scans
0010 %           format:
0011 %           <timestamp> <initial angle> <angle increment> <final angle> <range type int>
0012 %           <nscans> <range1> <range2> ... <range N>
0013 %           all in the row
0014 %
0015 %   OUTPUT:
0016 %       rangeMatrix - variable containing laser range data in rows
0017 %           size is R rows by C columns, R is the number of scans and C is the points per scan
0018 %               eg a typical SICK laser scan might have [39,181] if there were
0019 %               39 scans and the mode was 180 degrees, 1 degree resolution
0020 %           NOTE: The ranges are in METRES
0021 %
0022 %       angleVector - variable containing a single row of angles in radians corresponding
0023 %           to all of the laserRangeData matrix
0024 %           If laserRangeData is R by C, laserAngleVector must be 1 by C
0025 %           NOTE:   The angles are in RADIANS
0026 %                   0 angle points forwards out of the laser, positive angles
0027 %                   are clockwise from top down
0028 %
0029 % Written by James Underwood 10/07/06
0030 %
0031 
0032 laserUnitsType = laserLogData(1,5);
0033 if laserUnitsType == 0
0034     error( 'LaserUnitsType 0 is invalid' );
0035 elseif laserUnitsType == 1 % mm
0036     laserDivisor = 1000;
0037 elseif laserUnitsType == 2 % cm
0038     laserDivisor = 100;
0039 elseif laserUnitsType == 3 % m
0040     laserDivisor = 1;
0041 elseif laserUnitsType == 4 % km
0042     laserDivisor = 1e-3;
0043 else
0044     error( 'LaserUnitsType not supported - supported values are [1-mm, 2-cm, 3-m, 4-km]' )
0045 end
0046 
0047 %angleVector = linspace( laserLogData(1,2), laserLogData(1,2)+laserLogData(1,3)*(laserLogData(1,5)-1), laserLogData(1,5) );
0048 angleVector = linspace( laserLogData(1,2), laserLogData(1,4), laserLogData(1,6) );
0049 rangeMatrix = laserLogData(:,7:end) ./ laserDivisor;