extractlines.m

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

%EXTRACTLINES Extract alpha,r-lines from range data.
%   L = EXTRACTLINES(SCNDATA,PARAMS,DISPLAY) extracts line segments from
%   uncertain range data given in polar coordinates. The lines, repre-
%   sented in the Hessian alpha,r-model, and their associated covariance
%   matrices are returned.
%
%   The structure SCNDATA holds the raw range readings according to the
%   output of the read function READONESTEP. PARAMS is a structure with
%   the algorithm parameters. With the flag DISPLAY being 1, the extrac-
%   tion result is displayed in a new figure.
%
%   [L,SEGS,LINES] = EXTRACTLINES(SCNDATA,PARAMS,DISPLAY) returns also
%   the matrices SEGS and LINES. Their definition is given below.
%
%   The algorithm uses a sliding window technique where a window of
%   constant size sweeps over the (ordered) set of range readings. At
%   each step the model (here: the line) is fitted to the points within
%   the window and a model fidelity measure is calculated. The sought
%   objects in the scan are found by a condition on the model fidelity.
%   
%   This produces an oversegmented range image in many cases since each
%   outlier can give rise to several small segments. Under the condition
%   of collinearity (on a significance level alpha), adjacent segments
%   are fused in a final step.
%
%   For the fit, perpendicular errors from the points onto the line are
%   minimized, which, in spite of being a non-linear regression problem
%   with points in polar coordinates, has an analytical solution.
%
%   The algorithm parameters in detail:
%      PARAMS.WINDOWSIZE : size of the sliding window in number of
%                          points (odd number, >=3)
%      PARAMS.THRESHFIDEL: threshold on model fidelity
%      PARAMS.FUSEALPHA  : significance level for segment fusion
%      PARAMS.MINLENGTH  : minimal length in [m] a segment must have
%      PARAMS.CYCLIC     : 1: range data are cyclic, 0: non-cyclic
%      PARAMS.COMPENSA   : heurististic compensation term to account
%                          for correlations between the raw data. Is 
%                          added to all alpha's after extraction, [rad]
%      PARAMS.COMPENSR   : same as above, for r values, in [m]
%
%   L is a map object according to the definition of the map class. It
%   holds the information of the infinite lines of LINES, treating the
%   line parameters alpha,r as the feature states.
%
%   SEGS is a matrix which holds information about the (finite-length)
%   segments and has the following elements:
%          1st row : identifier of segment
%          2nd row : identifier of line the segment belongs to
%          3rd row : number of contributing raw segments
%          4th row : number of contributing data points
%        5,6th row : line parameters alpha, r
%      7,8,9th row : covariance matrix elements saa, srr, sar
%         10th row : trace of covariance matrix
%      11,12th row : begin- and end-index of segment
%      13,14th row : Cartesian coordinates of segment start point
%      15,16th row : Cartesian coordinates of segment end point
%
%   LINES is a matrix which holds information about the (infinite)
%   lines and has the following elements:
%          1st row : identifier of line
%          2nd row : number of contributing segments
%          3rd row : number of contributing raw segments
%          4th row : number of contributing data points
%       5-10th row : same as above: parameters, cov. matrix, trace
%       >=11th row : identifiers of contributing segments
%
%   Reference:
%      K.O. Arras, "Feature-Based Robot Navigation in Known and Unknown
%      Environments", Ph.D. dissertation, Nr. 2765, Swiss Federal Insti-
%      tute of Technology Lausanne, Autonomous Systems Lab, June 2003.
%
%   See also FITLINEPOLAR, READONESTEP, MAP.

% This file is part of the CAS Robot Navigation Toolbox.
% Please refer to the license file for more infos.
% Copyright (c) 2004 Kai Arras, ASL-EPFL
% v.1.0, 1995, Kai Arras, IfR-ETHZ, diploma thesis
% v.2.0-v.4.0, 1997, Kai Arras, ASL-EPFL: probabilistic version
% v.4.1.1, 1.99/6.00/7.00, Kai Arras, ASL-EPFL 
% v.4.2, 05.12.02, Kai Arras, ASL-EPFL 
% v.4.3-4.4, Dec.2003, Kai Arras, CAS-KTH: minor adaptations for toolbox

function varargout = extractlines(scndata,params,displocal);

if ((nargout==1) | (nargout==3)) & ~isempty(scndata) & ...
    isstruct(params) & ((displocal==0) | (displocal==1)),
  
  % First: {S} --> {R} here
  xS = scndata.steps.data2;
  yS = scndata.steps.data3;
  sint = sin(scndata.params.xs(3));
  cost = cos(scndata.params.xs(3));
  xR = xS*cost - yS*sint + scndata.params.xs(1);
  yR = xS*sint + yS*cost + scndata.params.xs(2);
  [phi,rho] = cart2pol(xR,yR);
  stdrho = ones(length(xR),1)*scndata.params.stdrho;
  scan = [phi, rho, stdrho];
  l = size(scan,1);
  
  % ------------------------------------------------------------- %
  %%%%%%%%%%%%%%%%%%%%%%%%   Constants   %%%%%%%%%%%%%%%%%%%%%%%%%%
  % ------------------------------------------------------------- %
  % Constants of algorithm
  winsize     = params.windowsize;  % window size in points
  threshfidel = params.threshfidel; % model fidelity threshold
  minlength   = params.minlength;   % minimal segment length in [m]
  cyc         = params.cyclic;      % cyclic data or not
  saacompens  = params.compensa;    % heurist. compens. for raw data corr., [rad]
  srrcompens  = params.compensr;    % heurist. compens. for raw data corr., [m]
  mode        = 1;                  % multi-segment mode by default
  fuselevel   = chi2invtable(params.fusealpha,2); % signific. level for segment fusion
  
  % Constants for graphic output
  MUE = 50;									 	      % blows up infinite (alpha,r)-lines in figures
  TXTSHIFT = 0.1;						        % shifts text away from the denoted object in [m]
  
  % ---------------------------------------------------------- %
  %%%%%%%%%%% Slide Window And Fit Lines --> alpharC %%%%%%%%%%%
  % ---------------------------------------------------------- %
  halfWin = (winsize-1)/2;
  ibeg = 1 + ~cyc*halfWin;
  iend = l - ~cyc*halfWin;
  windowvec = ibeg:iend;
  for imid = windowvec,	
    for j = imid-halfWin:imid+halfWin,
      k = mod(j-1,l) + 1;
      windowpts(j-imid+halfWin+1,:) = scan(k,:); 
    end;
    [p,C] = fitlinepolar(windowpts);
    ifillin = mod(imid-1,l) + 1;
    alpharC(1,ifillin) = p(1,1);			 % alpha
    alpharC(2,ifillin) = p(2,1);		   % r
    alpharC(3,ifillin) = C(1,1);		   % sigma aa
    alpharC(4,ifillin) = C(2,2);			 % sigma rr
    alpharC(5,ifillin) = C(1,2);	     % sigma ar
  end;
  
  % --------------------------------------------------------------------------- %
  %%%%%%%% Calculate Model Fidelity (=Compactness) Measure -> compactness %%%%%%%
  % --------------------------------------------------------------------------- %
  ibeg = 1 + ~cyc*(1+halfWin);   % taking always 3 adjacent points in model space
  iend = l - ~cyc*(1+halfWin);	 % keep track of shrinking valid range at array ends if cyc=1
  compactvec  = ibeg:iend;
  windowlines = zeros(5,3);
  compactness = zeros(1,l);
  for imid = compactvec,
    for j = imid-1:imid+1,
      k = mod(j-1,l) + 1;
      windowlines(:,j-imid+2) =  alpharC(:,k);
    end;
    compactness(mod(imid-1,l)+1) = calccompactness(windowlines);
  end;
  
  % ------------------------------------------------------------------- %
  %%%%%%%%% Apply Model Fidelity Threshold --> rawSegs(1/2/3,:) %%%%%%%%%
  % ------------------------------------------------------------------- %
  nSegBlowup  = halfWin;
  rSegIndices = findregions(compactness(compactvec),threshfidel,'<',cyc);
  nRawSegs    = size(rSegIndices,1);
  
  if rSegIndices(1,1) >= 0,		% check case rSegIndices = -1, see help findregions
    trwseg = cputime;
    % Test here for wraparound segment kernels
    if cyc & (nRawSegs > 1),														        % ingle segm. requires no cyclic treatment
      o1 = (rSegIndices(1,3) < rSegIndices(1,2));							  % does first segment wrap around?
      o2 = (rSegIndices(nRawSegs,3) < rSegIndices(nRawSegs,2));	% does last segment wrap around?
      if (o1 & o2) | (rSegIndices(1,2)-rSegIndices(nRawSegs,3) + ~(o1 | o2)*l <= 1),
        rSegIndices(1,2) = rSegIndices(nRawSegs,2);             % ibeg of last one is ibeg of new cyclic segm.
        rSegIndices = rSegIndices(1:nRawSegs-1,:);						  % delete last (=first) raw segment
        nRawSegs = nRawSegs - 1;
      end;
    end;
    % Blow up segments. Note: if cyc=0 add (1+halfWin) to all indices since they are valid...
    for i = 1:nRawSegs,																	        % ...for the *shrinked* cmpct-vector
      rSegIndices(i,2) = mod2(rSegIndices(i,2)-nSegBlowup+~cyc*(1+halfWin),l);
      rSegIndices(i,3) = mod2(rSegIndices(i,3)+nSegBlowup+~cyc*(1+halfWin),l);
    end;
    rawSegs = rSegIndices';
    
    % --------------------------------------------------------------------------------- %
    %%%%%%%%% Line Fit To Points In Homogenous Regions -> rawSegs(4/5/6/7/8/9,:) %%%%%%%%
    % --------------------------------------------------------------------------------- %
    for i = 1:nRawSegs,
      segPoints = 0;
      ibeg = rawSegs(2,i);
      iend = rawSegs(3,i);
      if iend < ibeg,                   % discontinuity lies within the segment
        segPoints = [scan(ibeg:l,:); scan(1:iend, :)];
      else															% normal case: discontinuity lies not within the segment
        segPoints = scan(ibeg:iend, :);
      end;
      [p,C] = fitlinepolar(segPoints);
      rawSegs(4,i) = length(segPoints);	% number of raw segment points
      rawSegs(5,i) = p(1,1);						% alpha
      rawSegs(6,i) = p(2,1);						% r
      rawSegs(7,i) = C(1,1);						% sigma_aa
      rawSegs(8,i) = C(2,2);						% sigma_rr
      rawSegs(9,i) = C(2,1);						% sigma_ar
      xybeg = [scan(ibeg,2)*cos(scan(ibeg,1)), scan(ibeg,2)*sin(scan(ibeg,1))];
      xyend = [scan(iend,2)*cos(scan(iend,1)), scan(iend,2)*sin(scan(iend,1))];
      Pe1 = calcendpoint(xybeg,rawSegs(5:6,i));
      Pe2 = calcendpoint(xyend,rawSegs(5:6,i));
      rawSegs(10:13,i) = [Pe1, Pe2]';	  % Cartesian endpoint coordinates of raw segment
    end;
    
    % --------------------------------------------------------------------------- %
    %%%%%%%%  Agglomerative Hierarchical Clustering --> D,M,rawSegs,nLines %%%%%%%%
    % --------------------------------------------------------------------------- %
    nLines = nRawSegs;
    if nRawSegs > 1,
      % Fill in distance matrix and set diagonal elements to zero
      for i=1:nRawSegs-1,
        for j=i+1:nRawSegs,
          D(i,j) = mahalanobisar(rawSegs(5:9,i),rawSegs(5:9,j));
        end;
      end;
      for i=1:nRawSegs, D(i,i) = 0; end;
      M = zeros(nRawSegs);		  % M holds the markers, whether a min in D is valid or not
      
      ahcmode  = 2;             % mode 2: coll,neigh,over; mode 1: coll,neigh; mode 0: coll
      terminate = 0; 
      while ~terminate,
        % Find minimum
        dmin = Inf;
        for i = 1:nRawSegs-1,   % finds always first (lowest index) instance of segment in question
          for j = i+1:nRawSegs,	
            if ~((D(i,j) < 0) | ((ahcmode==1)&(M(i,j)==1)) | ((ahcmode==2)&(M(i,j)==2))),
              if D(i,j) < dmin,
                dmin = D(i,j);
                minrow = i;     % row in D which holds the current minimal distance
                mincol = j;     % colon in D which holds the current minimal distance
              end;
            end;
          end;
        end;
        
        if dmin < fuselevel,
          if isneighbour(rawSegs, minrow, mincol, ahcmode-1, cyc) | (ahcmode==0),
            % Fusion of identified segments in 'rawSegs'
            nLines = nLines - 1; 
            rejectClustNr = rawSegs(1,mincol);
            for i = 1:nRawSegs,
              if rawSegs(1,i) == rejectClustNr,
                rawSegs(1,i) = rawSegs(1,minrow); % mark all segments by overwriting their old ID
              end;