draw.m

来自「it is a matlab file foe develop SLAM loc」· M 代码 · 共 41 行

%DRAW   Draw alpha,r-line feature.
%   DRAW(L,IDS,COLOR) draws the alpha,r-line feature L into
%   the current figure with color COLOR. Enable and disable
%   display of feature identifiers with the flag IDS being
%   1 or 0. COLOR is a [r g b]-vector or a Matlab color
%   string such as 'r' or 'g'.
% 
%   H = DRAW(...) returns a column vector of handles to the
%   line objects of the drawing.
%
%   See also ARLINEFEATURE.

% v.1.0, Dec. 2003, Kai Arras, CAS-KTH

function h = draw(l,ids,color);

% Constants
FSIZE = 0.1;           % font scale in [m]
FOFFS = 0.03;          % font offset in [m]
MUE   = 100;           % infinite-line draw coefficient

% Get values
al = l.x(1);
rl = l.x(2);

% Draw label
if ids,
  x = rl*cos(al);
  y = rl*sin(al);
  theta = al + pi/2;
  h1 = drawlabel([x y theta],num2str(get(l,'id')),FSIZE,FOFFS,color);
end;

% Draw infinite line
x1 = rl*cos(al) - MUE*sin(al);
y1 = rl*sin(al) + MUE*cos(al);
x2 = rl*cos(al) + MUE*sin(al);
y2 = rl*sin(al) - MUE*cos(al);
h2 = plot([x1 x2],[y1 y2],'Color',color);
h = cat(1,h1,h2);