circfit_DEMO.m

%% circfit demo
% very simple demonstation of the circfit function.
%parameters:
r=6;            %radius
m=[2;1];        %center
arc=1;          %arc fraction, 1 for full circle
numpoints=50;   %number of points
sigma2=.5;      %variance of noise


x=linspace(0,arc*2*pi,numpoints);
data=r*[cos(x);sin(x)]+repmat(m,1,size(x,2));
data=data+sqrt(sigma2)*randn(size(data));
w=ones(size(x));
[r_est,m_est]=circfit(data,w);

%output
r
r_est
m
m_est

figure;
x=linspace(0,2*pi,100);
plot(r*cos(x)+m(1,1),r*sin(x)+m(2,1),'g')
hold on
plot(m(1),m(2),'gx')
scatter(data(1,:),data(2,:),'kx')
plot(r_est*cos(x)+m_est(1,1),r_est*sin(x)+m_est(2,1),'b')
plot(m_est(1),m_est(2),'bx')
axis equal
title('Simple circle fitting')
legend('circle','center','noisy point measurements','estimated circle','estimated center')
hold off

%% curvature demo
%Simple application of circfit function to calculate the (causal) estimate
%of the (signed) curvature of positional data. The value of the curvature
%will be stored in 'kappa'. For the first 'windowlength' positions there
%will be no curvature since we need a number of smaples to estimate the
%curvature. 
%For the demonstation an eliptical path is considered. The window/weight
%should be fine-tuned for other applications.

numpoints=100;                  %number of sample points
xpos = @(t)(1.5*cos(2*pi*t));   %function for x-coordinates
ypos = @(t)(sin(2*pi*t));       %function for y-coordinates
windowlength=3;                 %how many samples to consider

t=linspace(0,1,numpoints);
pos_data=[xpos(t);ypos(t)];


%calc weights
w=exp(-(linspace(-5,0,n)).^2/2);
w=w/sum(w);

cmap = colormap(jet(180));
cmap = [cmap; flipud(cmap)];
figure
plot(pos_data(1,:),pos_data(2,:))
hold on
for i = 1:size(t,2)-n
    [r,m]=circfit(pos_data(:,i:i+n-1),w);
    M(:,i)=m;
    R(:,i)=r;
    
    a=pos_data(:,i+n-1)-pos_data(:,i+n-2);
    b=m-pos_data(:,i+n-1);
    axb=a(1)*b(2)-a(2)*b(1);
    kappa(:,i)=sign(axb)/r;
    color = cmap(round(180+180/pi*angle(b(1)+1i*b(2))),:);

    clear a b axb
    line([pos_data(1,i+n-1),m(1,1)],[pos_data(2,i+n-1),m(2,1)],'Color',color)
end
hold off
axis off
axis equal
title('Demonstation of curvature estimation')