findregions.m

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

%FINDREGIONS Find homogeneous regions in array.
%   INDICES = FINDREGIONS(X,ID,OP,CYC) searches in the array X all
%   regions where ID has relation OP. OP is a string from the set
%   of the following relational operators:
%      '==' : equal
%      '~=' : not equal
%      '<'  : less than
%      '>'  : greater than
%      '<=' : less than or equal
%      '>=' : greater than or equal
%
%   The functions returns a rx3 matrix INDICES with elements for 
%   the region identifier, the start and the end index - each
%   region on a new row. If CYC is 1, X is interpreted to be a
%   cyclic array. If ID does not appear in X, -1 is returned, -2
%   if X is a matrix and -3 if relation OP is not valid or CYC
%   is not 0 or 1.
%
%   Example:
%      findregion([5 0 1 5 5 0 5 5],5,'==',1)
%      yields [1 7 1; 2 4 5]
%
%   See also FIND.

% v.1.0, 21.01.99, Kai Arras, ASL-EPFL
% v.1.1, Dec.2003, Kai Arras, CAS-KTH

function indices = findregions(C,id,op,cyc);

if isstr(op) & (sum(strcmp({'==' '~=' '<' '>' '<=' '>='},op))==1) & ((cyc==0)|(cyc==1))
  dim = size(C);
  if sum(dim==1) > 1,													% C is a scalar
    if C == id, indices = [1 1 1];
    else indices = -1; end;
  elseif sum(dim==1) == 1,										% C is a vector, i.e. either 1xl or lx1
    if dim(1) < dim(2),
      C = C'; l = dim(2);
    else 
      l = dim(1);
    end;
    % --- C has correct dimensions now
    iregion = 0;
    newregion = 1;
    for i = 1:l,
      if feval(op,C(i),id),										% apply relational operation 'op'
        if newregion,
          iregion = iregion + 1;
          indices(iregion,1:2) = [iregion i];
          newregion = 0;
        end;
        if i == l,														% exception: detect 1->0 for last region
          indices(iregion, 3) = l;
        end;
      else
        if ~newregion,												% detect 1 -> 0 transition
          indices(iregion, 3) = i-1;					% store end index of region
        end;
        newregion = 1;
      end;
    end;
    if iregion == 0,													% no region found at all
      indices = -1;
    else																			% "is there a circular segment? and is there
      if feval(op,C(1),id) & feval(op,C(l),id) & (iregion>1) & cyc, % more than one region (=all C(i) equal id)?"
        indices(1, 2) = indices(iregion, 2); 	% begin-idx of first one is that of last one
        iregion = iregion - 1;								% decrease region counter
        indices = indices(1:iregion,:);				% delete last (=first) region
      end;
    end;
  else
    indices = -2;															% C is a matrix not a vector
    disp('findregion: X is not a vector. Check your arguments.')
  end;
else
  indices = -3;
  disp('findregion: operation not valid or wrong value for cyclic. Check your arguments.')
end;