extractlines.m

用于移动机器人SLAM的仿真工具箱（CAS Robot Navigation Toolbox）。由Kai Arras编写。可迅速构建移动机器人仿真平台。

            end;
            for i = 1:nRawSegs,
              if rawSegs(1,i) == rawSegs(1,minrow),
                % First: mark all raw points which belong to the segments in question -> scan(:,5)
                % Simple, less efficient but general solution. 
                ibeg = rawSegs(2,i);
                iend = rawSegs(3,i);
                if iend < ibeg,
                  scan(ibeg:l,5) = rawSegs(1,minrow);
                  scan(1:iend,5) = rawSegs(1,minrow);
                else
                  scan(ibeg:iend, 5) = rawSegs(1,minrow);
                end;
              end;
            end;
            % Second: go through the scan and collect them -> segPoints
            segPoints = [-1 -1 -1];
            segPointCounter = 0;
            for i = 1:l,
              if scan(i,5) == rawSegs(1,minrow),
                segPointCounter = segPointCounter + 1;
                segPoints = [segPoints; scan(i,1:3)];		% accumulate points
              end;
            end;
            % Third: fit the line with the combined set of points -> new cluster center
            [p,C] = fitlinepolar(segPoints(2:segPointCounter+1,:));		% remove -1-init-vector at index 1.
            rawSegs(5:9,minrow) = [p(1,1) p(2,1) C(1,1) C(2,2) C(1,2)]';
            rawSegs(5:9,mincol) = [p(1,1) p(2,1) C(1,1) C(2,2) C(1,2)]';
            
            % Calculate new distance in minrow-th row and column
            for i = minrow+1:nRawSegs,
              if D(minrow,i) > 0,
                D(minrow,i) = mahalanobisar(rawSegs(5:9,minrow),rawSegs(5:9,i));
                M(minrow,i) = 0;   % reset isneighbourhip, v.4.2
              end;
            end;
            for i = 1:minrow-1,
              if D(i,minrow) > 0,
                D(i,minrow) = mahalanobisar(rawSegs(5:9,minrow),rawSegs(5:9,i));
                M(i,minrow) = 0;   % reset isneighbourhip, v.4.2
              end;
            end;
            % Remove mincol-th row and column by marking them with -1
            for i = mincol+1:nRawSegs,
              D(mincol,i) = -1; M(mincol,i) = 0;
            end;	% mark D and unmark M
            for i = 1:mincol-1,
              D(i,mincol) = -1; M(i,mincol) = 0;
            end;	% mark D and unmark M
          else
            M(minrow, mincol) = ahcmode; % mark minrow, mincol
          end;
        else	% dmin >= fuselevel
          if ahcmode == 2,
            if mode == 1,               % mode is changed only if line mode
              ahcmode = 1;              % is enabled...
            else
              terminate = 1;            % ... or terminate otherwise
            end;
          elseif ahcmode == 1,
            ahcmode = 0;                % mode change 2 -> 1
          else
            terminate = 1;              % terminate if there are no collinear segments anymore
          end;
        end;
      end;
    end; % if nRawSegs > 1      
    
    % ---------------------------------------------------------- %
    %%%%%%%%%% Generate Data Structures --> segs, lines %%%%%%%%%%
    % ---------------------------------------------------------- %
    j = 1;
    for i = min(rawSegs(1,:)):max(rawSegs(1,:));
      idxvec = find(rawSegs(1,:)==i);
      if ~isempty(idxvec),
        nRawSegs = sum(idxvec~=0);						% segIndices holds:
        segIndices(j,1:length(idxvec)+2) = [i nRawSegs idxvec]; % 1st column: ID of line in 'rawSegs' 
        j = j + 1;														% 2nd column: number of raw segments
      end;																		% 3:n column: indices of raw segm. in 'rawSegs'
    end;
    li = 0; si = 0;
    for i = 1:nLines, 												% for all lines
      lineID = segIndices(i,1); 							% get ID
      for j = 1:segIndices(i,2);              % for all raw segments constituting the line lineID
        ibeg = rawSegs(2,segIndices(i,2+j));  % get their begin- and end-indices which,
        iend = rawSegs(3,segIndices(i,2+j));  % in general, overlap the adjacent segments
        if iend < ibeg, 										  % discontinuity lies within segment
          scan(ibeg:l,5) = lineID; 						% mark raw points
          scan(1:iend,5) = lineID;
        else  															  % normal case: discontinuity lies not within segment
          scan(ibeg:iend,5) = lineID;         % mark raw points
        end;
      end;
      nLinePoints = (sum(scan(:,5) == lineID));
      jSegIndices = findregions(scan(:,5),lineID,'==',cyc);		% find joint segments
      nJointSegs  = size(jSegIndices, 1);
      
      lmax = -Inf; imax = 0;
      for j = 1:nJointSegs,
        ibeg = jSegIndices(j,2); iend = jSegIndices(j,3);
        [pBeg(1),pBeg(2)] = pol2cart(scan(ibeg,1),scan(ibeg,2));
        [pEnd(1),pEnd(2)] = pol2cart(scan(iend,1),scan(iend,2));
        Pe1 = calcendpoint(pBeg,rawSegs(5:6,segIndices(i,3))); % Cartesian projection
        Pe2 = calcendpoint(pEnd,rawSegs(5:6,segIndices(i,3)));
        len = sqrt(sum((Pe1-Pe2).^2));
        if len > lmax,
          lmax = len; imax = j;
        end;
      end;
      
      if lmax >= minlength,
        li = li + 1;
        lines(1,li) = lineID;                     % line ID
        lines(2,li) = nJointSegs;                 % number of contributing joint segments
        lines(3,li) = segIndices(i,2);				    % number of contributing raw segments
        lines(4,li) = nLinePoints;                % number of contributing raw data points
        lines(5:6,li) = rawSegs(5:6,segIndices(i,3));           % alpha,r
        lines(7,li) = rawSegs(7,segIndices(i,3)) + saacompens;  % saa
        lines(8,li) = rawSegs(8,segIndices(i,3)) + srrcompens;  % srr
        lines(9,li) = rawSegs(9,segIndices(i,3));               % sar
        lines(10,li) = sum(rawSegs(7:8,segIndices(i,3))); % trace of covariance matrix
        for j = 1:nJointSegs,
          si = si + 1;
          lines(10+j,li) = si; 				  	        % IDs of contributing joint segments
          segs(1,si) = si; 					              % ID of joint segment
          segs(2,si) = lineID; 					          % ID of line the segment belongs to
          segs(3,si) = segIndices(i,2);	          % number of contributing raw segments
          nPoints = mod2(jSegIndices(j,3)-jSegIndices(j,2)+1, l);
          segs(4,si) = nPoints;					          % number of contributing raw data points
          segs(5:10, si) = lines(5:10,li);				% copy alpha,r and cov-matrix
          segs(11:12,si) = jSegIndices(j,2:3)';   % begin, end index of joint segment
          ibeg = jSegIndices(j,2);
          iend = jSegIndices(j,3);
          [pBeg(1),pBeg(2)] = pol2cart(scan(ibeg,1), scan(ibeg,2));
          [pEnd(1),pEnd(2)] = pol2cart(scan(iend,1), scan(iend,2));
          Pe1 = calcendpoint(pBeg,lines(5:6,li)); % Cartesian projection
          Pe2 = calcendpoint(pEnd,lines(5:6,li));
          segs(13:16,si) = [Pe1, Pe2]';	          % Cartesian coordinates of endpoints
        end;
      end;
    end;
    nJointSegs = si;
    nLines = li;
    
    % Resort 'segs' such that it is sorted acc. to real succession of segments in scan
    if nLines > 0
      [dummy,jSegsisort] = sortrows(segs',11);
      for i = 1:nJointSegs,
        tmp(:,i) = segs(:,jSegsisort(i));
      end;
      segs = tmp;
      
      % ---------------------------------------------------------- %
      %%%%%%%%%%%  Generate Final Data Structures --> L  %%%%%%%%%%%
      % ---------------------------------------------------------- %
      
      % Put everything into the new map object L
      created = 0; iline = 1;
      for i = 1:nLines;
        % create map if not yet done
        if ~created,
          L = map('local map',0);
          created = 1;
        end;
        % get alpha-r values
        id    = lines(1,i);
        alpha = lines(5,i);
        r     = lines(6,i);
        saa   = lines(7,i);
        srr   = lines(8,i);
        sar   = lines(9,i);
        % create line feature
        l = arlinefeature(id,[alpha;r],[saa sar; sar srr]);
        % add it to local map
        L = addentity(L,l);
      end;
      if created,
        X = get(L,'x');
        nL = length(X);
      else
        L = []; nL = 0;
      end;
      
      if nL == 0, str1 = 'no'; str2 = 'lines';
      elseif nL == 1, str1 = '1'; str2 = 'line';
      else str1 = int2str(nL); str2 = 'lines';
      end; disp([' ',str1,' ',str2,' extracted']);
      
      % Assign output
      if nargout == 1;
        varargout{1} = L;
      else
        varargout{1} = L;
        varargout{2} = segs;
        varargout{3} = lines;
      end;  
      
      
      % -------------------------------------- %
      %%%%%%%%%%  Plot final lines  %%%%%%%%%%%%
      % -------------------------------------- %
      
      if displocal,
        figure(2); clf; hold on; set(gca,'Box','on'); axis equal;
        xlabel('x [m]'); ylabel('y [m]'); title('Local map: extracted lines');
        plot(scan(:,2).*cos(scan(:,1)),scan(:,2).*sin(scan(:,1)),...
          '.','Color',[.6 .6 .6],'MarkerSize',6);
        drawreference(zeros(3,1),'R',1.0,'k');
          axisvec = axis;
          for i = 1:nLines,
            x1 = lines(6,i)*cos(lines(5,i)) - MUE*sin(lines(5,i));
            y1 = lines(6,i)*sin(lines(5,i)) + MUE*cos(lines(5,i));
            x2 = lines(6,i)*cos(lines(5,i)) + MUE*sin(lines(5,i));
            y2 = lines(6,i)*sin(lines(5,i)) - MUE*cos(lines(5,i));
            plot([x1 x2],[y1 y2],'LineWidth',1,'Color',[.4 .4 .4],'LineStyle','-.');
            xtext = lines(6,i)*cos(lines(5,i)) + TXTSHIFT;
            ytext = lines(6,i)*sin(lines(5,i)) + TXTSHIFT;
            text(xtext,ytext,int2str(lines(1,i)));
          end;
          si = 0;
          for i = 1:nLines,                 % calc endpoints and plot final (joint) segments
            for j = 1:lines(2,i);           % for all joint segment of line i
              si = si + 1;
              Pe1 = segs(13:14,si)';
              Pe2 = segs(15:16,si)';
              plot([Pe1(1) Pe2(1)],[Pe1(2) Pe2(2)],'LineWidth',2.7,'Color',[.9 .5 .0]);
              plot(Pe1(1),Pe1(2),'kx','LineWidth',2,'MarkerSize',9);
              plot(Pe2(1),Pe2(2),'kx','LineWidth',2,'MarkerSize',9);
            end;
          end;
          % restore and enlarge axis vector slightly
          axis(1.1*axisvec);
        end;
        
      else
        disp('extractlines: All found segments too short');
        if nargout == 3, varargout{2} = []; varargout{3} = []; end; varargout{1} = [];
      end;
    else % if nRawSegs > 0...
      disp('extractlines: No lines found');
      if nargout == 3, varargout{2} = []; varargout{3} = []; end; varargout{1} = [];
    end;
  else	
    disp('extractlines: Wrong input. Check your arguments');
    if nargout == 3, varargout{2} = []; varargout{3} = []; end; varargout{1} = [];
  end;