getedgelineclstrs

 GETEDGELINECLSTRS divides the laser scans into straight lines.
 
 GETEDGELINECLSTRS divides the laser scans into straight lines by
 considering range jumps and the straight line criteria as described in
 the RADLOCC technical report.
 
 USAGE:
     clstrs=getedgelineclstrs(angleVector,rangeMatrix,distth);
 
 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.
 
     distth: is the threshold value for the straight line criteria. 
 
 OUTPUTS:
     clstrs: MxN array. Each element in clstrs is an integer indicating
     the line cluster the range to which reading belongs.

0001 function clstrs=getedgelineclstrs(angleVector,rangeMatrix,distth)
0002 % GETEDGELINECLSTRS divides the laser scans into straight lines.
0003 %
0004 % GETEDGELINECLSTRS divides the laser scans into straight lines by
0005 % considering range jumps and the straight line criteria as described in
0006 % the RADLOCC technical report.
0007 %
0008 % USAGE:
0009 %     clstrs=getedgelineclstrs(angleVector,rangeMatrix,distth);
0010 %
0011 % INPUTS:
0012 %     angleVector: 1xN vector; angleVector lists the angles for the ranges
0013 %     in rangeMatrix.
0014 %
0015 %     rangeMatrix: MxN array; Each row in rangeMatrix contains a laser scan
0016 %     with ranges at the angles specified in angleVector.
0017 %
0018 %     distth: is the threshold value for the straight line criteria.
0019 %
0020 % OUTPUTS:
0021 %     clstrs: MxN array. Each element in clstrs is an integer indicating
0022 %     the line cluster the range to which reading belongs.
0023 
0024 fprintf('Progress: ');
0025 
0026 if ~exist('distth','var')
0027     distth=0.02; %m
0028 end
0029 
0030 % pec threshold
0031 % objth=1; % metre
0032 
0033 % jump threshold
0034 jumpth=0.1; % metre (10cm)
0035 
0036 noscans=size(rangeMatrix,1);
0037 nopts=length(angleVector);
0038 
0039 mainlncombvec=combnk(1:nopts,2);
0040 
0041 % get xs and ys
0042 
0043 [x,y]=pol2cart(repmat(angleVector,[noscans,1]),rangeMatrix);
0044 
0045 
0046 % set splitpoints according to jumps
0047 % all points within the object threshold are included but large jumps should divide the scan
0048 
0049 splitpoints=cell(noscans,1);
0050 spointdone=cell(noscans,1);
0051 
0052 for cntr1=1:noscans
0053     range=rangeMatrix(cntr1,:);
0054 
0055     rangeedge=laseredge(range);
0056     
0057     jumppts=find(abs(rangeedge)>jumpth);
0058 
0059     splitpoints{cntr1}=[1,jumppts,nopts+1];
0060     spointdone{cntr1}=[0,zeros(size(jumppts)),1];
0061 end
0062 
0063 % loop over all scans
0064 % objects are now divided according to straight lines
0065 
0066 progmsg=[];
0067 
0068 for cntr1=1:noscans
0069     fprintf(repmat('\b', 1, length(progmsg)));
0070     progmsg=sprintf('%%%i',round(cntr1/noscans*100));
0071     fprintf('%s',progmsg);
0072     while ~isempty(find(spointdone{cntr1}==0,1))
0073         % loop over all clusters
0074         for cntr2=1:length(splitpoints{cntr1})-1
0075             if ~spointdone{cntr1}(cntr2)
0076                 indcs=splitpoints{cntr1}(cntr2):splitpoints{cntr1}(cntr2+1)-1;
0077 
0078                 if length(indcs)>2 % check if worth splitting first
0079 
0080                     % loop over all line combinations
0081                     mxlen=0;
0082                     lncombvec=getcombs(indcs(1),indcs(end),mainlncombvec);
0083                     for cntr3=1:size(lncombvec,1)
0084                         ind1=lncombvec(cntr3,1);
0085                         ind2=lncombvec(cntr3,2);
0086 
0087                         % one line should be quicker
0088 
0089                         dist = abs((x(cntr1,ind2)-x(cntr1,ind1))*(y(cntr1,ind1)-y(cntr1,ind1:ind2))-(x(cntr1,ind1)-x(cntr1,ind1:ind2))*(y(cntr1,ind2)-y(cntr1,ind1)))...
0090                             /norm([x(cntr1,ind2)-x(cntr1,ind1),y(cntr1,ind2)-y(cntr1,ind1)]);
0091 
0092                         if isempty(find(dist>distth,1))
0093                             if length(dist)>mxlen
0094                                 mxlen=length(dist);
0095                                 mxind1=ind1;
0096                                 mxind2=ind2;
0097                             end
0098                         end
0099                     end
0100 
0101                     % register results
0102                     if mxind1==indcs(1) && mxind2==indcs(end)
0103                         spointdone{cntr1}(cntr2)=1;
0104                     elseif mxind1==indcs(1)
0105                         spointdone{cntr1}(cntr2)=1;
0106                         spointdone{cntr1}=[spointdone{cntr1}(1:cntr2),0,spointdone{cntr1}(cntr2+1:end)];
0107                         splitpoints{cntr1}=[splitpoints{cntr1}(1:cntr2),mxind2+1,splitpoints{cntr1}(cntr2+1:end)];
0108                     elseif mxind2==indcs(end)
0109                         spointdone{cntr1}=[spointdone{cntr1}(1:cntr2),0,spointdone{cntr1}(cntr2+1:end)];
0110                         splitpoints{cntr1}=[splitpoints{cntr1}(1:cntr2),mxind1,splitpoints{cntr1}(cntr2+1:end)];
0111                     else
0112                         spointdone{cntr1}=[spointdone{cntr1}(1:cntr2),0,0,spointdone{cntr1}(cntr2+1:end)];
0113                         splitpoints{cntr1}=[splitpoints{cntr1}(1:cntr2),mxind1,mxind2+1,splitpoints{cntr1}(cntr2+1:end)];
0114                     end
0115                     break; % break under all conditions since spointdone exists
0116                 else
0117                     spointdone{cntr1}(cntr2)=1;
0118                 end
0119             end
0120         end
0121     end
0122     for cntr2=1:length(splitpoints{cntr1})-1
0123         clstrs(cntr1,splitpoints{cntr1}(cntr2):splitpoints{cntr1}(cntr2+1)-1)=cntr2;
0124     end
0125 end
0126 
0127 fprintf('\n');