%%Manually Segmentation Result
clear;
load data.mat;

X = Xe(1:3, :, 1)';
plist = [18 19 23 72; 22 45 48 64; 37 55 67 70]';
ons = ones(4, 1);
lambda = zeros(3,3);

for i = 1:3
    lambda(:,i) = X(plist(:,i),:)\ons;
end

d = abs(X*lambda - 1);
s = sqrt(sum(lambda.^2));

for i = 1:3
    d(:,i) = d(:,i)/s(i);
end

[C,I] = min(d, [], 2);

for i = 1:72
    if(C(i)>0.1)
        I(i) = 4;
    end
end
error(1) = sum(C);
for i = 1:3
    lambda(:,i) = X(I==i,:)\ones(size(X(I==i,:),1),1);
end

d = abs(X*lambda - 1);
s = sqrt(sum(lambda.^2));

for i = 1:3
    d(:,i) = d(:,i)/s(i);
end

[C,I] = min(d, [], 2);

for i = 1:72
    if(C(i)>0.1)
        I(i) = 4;
    end
end
error(2) = sum(C);
%%
path(path, '../oldhouse2');
% with the affine tracker and undone radial distortion
seq_name = 'oldhouse2/A2000';
image_type = 'bmp';
load oldhouse2/A2000_result_ST;  % tracks from 110 to 200

% specify index of starting frame fs, end frame fe
% and the subsampling factor ft
fs = 0; fe = 88; ft = 12;
findex = [fs:ft:fe]; offset = 1;
indf = 1:size(findex,2);

% find features tracked in all the frames
ind = find(goodfeat ~= 0);
j = 0;
for i = (1+offset-1):ft:(fe-fs+1)
    j = j+1;
    xim(1,:,j) = xttfirst(2,ind) + SaveSTB(2,ind,i);
    xim(2,:,j) = xttfirst(1,ind) + SaveSTB(1,ind,i);
    xim(3,:,j) = 1;
end;

% consider only indf frames
xim = xim(:,:,indf);
[s, n, m] = size(xim);

opt = '%03d';
imfile = sprintf('%s%s.%s',seq_name,sprintf(opt,findex(1)),image_type)
seq(1).im = (imread(imfile));

% read images
for i = 2:m
    if findex(1) < 10 opt = '%03d';
    elseif findex(1) < 100 opt = '%02d';
    else  opt = '%01d';
    end;
    imfile = sprintf('%s%s.%s',seq_name,sprintf(opt,findex(i)),image_type)
    seq(i).im = imread(imfile);
end;


[ydim,xdim,cdim] = size(seq(1).im);

for i = 1
    figure;
    imagesc(seq(i).im); colormap gray; hold on;
    axis equal; axis image; axis off;
    plot(xim(1,:,i), xim(2,:,i),'y.');
    for k = 1:n
        t = text(xim(1,k,i)+3, xim(2,k,i)+3,num2str(k));
        set(t, 'Color', 'yellow');
    end
end

for i = 1:4
    figure;
    imagesc(seq(1).im); colormap gray; hold on;
    axis equal; axis image; axis off;
    plot(xim(1,:,1), xim(2,:,1),'y.');
    for k = 1:n
        if (I(k) == i)
            t = text(xim(1,k,1)+3, xim(2,k,1)+3,num2str(k));
            set(t, 'Color', 'yellow');
        end
    end
end