perimeter_new/ros2vec.m

   1 function vec=ros2vec(ros)
   2 % from the ros values in 8 directions make vector representation
   3 s=1/sqrt(2);
   4 r=squeeze(ros)
   5 Y=[-s 0 s
   6    -1 0 1
   7    -s 0 s]
   8 X=Y'
   9 xx=X.*r
  10 yy=Y.*r
  11 ii=[3 3 2 1 1 1 2 3];
  12 jj=[2 3 3 3 2 1 1 1];
  13 for k=1:8
  14     i=ii(k);
  15     j=jj(k);
  16     plot([xx(i,j)],[yy(i,j)],'*k')
  17     rr(k)=r(i,j);
  18     hold on
  19 end
  20 tt=linspace(0,9/4,10)*pi;
  21 rr(9:10)=rr(1:2);
  22 pp = csape(tt,rr,'periodic');
  23 fpolarplot([0,0],@(theta)ppval(pp,theta),500,'-r');
  24
  25 % find the vector xs,yz of largest spread
  26 % X(i,j)*xs + Y(i,j)*ys = r(i,j)-omni
  27 %A = [X(:),Y(:)]; A(5,:)=[];
  28 %b = r(:)-omni; b(5)=[];
  29 %v= A\b;
  30 %f = @(theta)2*omni+v(1)*cos(theta)+v(2)*sin(theta);
  31
  32 hold off
  33 end
  34
  35 function H=fpolarplot(center,f,np,LineSpec)
  36 % H=radplot(center,f,np,style)
  37 %   center
  38 %   function handle
  39 %   np
  40 % adapted from https://www.mathworks.com/matlabcentral/fileexchange/2876-draw-a-circle
  41 theta=linspace(0,2*pi,np);
  42 rho=f(theta);
  43 [x,y]=pol2cart(theta,rho);
  44 x=x+center(1);
  45 y=y+center(2);
  46 H=plot(x,y,LineSpec);
  47 axis square;
  48 end