Untitled

Hlines = zeros(4,3);
Llines = zeros(3,3);
Mlines = zeros(6,3);
Circumference = zeros(3,3);
curve = zeros(3,3);



img = imread("Look-outCat.jpg");
imgOriginal = img;

%Controlliamo che l'immagine sia in RGB, quindi la trasformiamo in scala di
%grigi
if size (img,3) == 3 
    grayimage = rgb2gray(img);
end

%miglioriamo il contrasto 
img = imadjust(grayimage);

%rileviamo i bordi tramite l'algoritmo di canny
img = edge(img,"canny",0.05);


%identifico le linee rette tramite la trasformata di hough
[H,teta,rho] = hough(img);
numpeaks = 50; 
p = houghpeaks(H,numpeaks,"Threshold",ceil(0.1*max(H(:))));
lines = houghlines(img,teta,rho,p,"FillGap",5,"MinLength",7);
figure;
imshow(img);
hold on;

for k = 1:length(lines)
   xy = [lines(k).point1; lines(k).point2];
   angle = abs(atan2d(xy(2,2)-xy(1,2),xy(2,1)-xy(1,1)));
   if angle <0
       angle = angle +180;
   end
   if (angle < 30) || (angle > 120)
        plot(xy(:,1), xy(:,2), 'LineWidth', 2, 'Color', 'blue');
   elseif (angle>=75 && angle <=105)
        plot(xy(:,1), xy(:,2), 'LineWidth', 2, 'Color', 'green');
   else
        plot(xy(:,1), xy(:,2), 'LineWidth', 2, 'Color', 'red');
   end
end
figure;
imshow(imgOriginal);
hold on;

Hlines(1,:) = cross([321 592 1],[384 236 1]);
Hlines(2,:) = cross([801 654 1],[778 317 1]);
Hlines(3,:) = cross([1075 341 1],[1093 417 1]);
Hlines(4,:) = cross([1316 384 1],[1419 665 1]);

Llines(1,:) = cross([402 249 1],[673 282 1]);
Llines(2,:) = cross([470 712 1],[1345 698 1]);
Llines(3,:) = cross([454 775 1],[1166 749 1]);

%[x, y]=getpts;
%scatter(x,y,100,'filled');
Mlines(1,:) = cross([402 250 1],[464 456 1]);
Mlines(2,:) = cross([776 296 1],[720 470 1]);
Mlines(3,:) = cross([969 461 1],[1070 348 1]);
Mlines(4,:) = cross([1165 482 1],[1300 388 1]);
Mlines(5,:) = cross([305 716 1],[420 778 1]);
Mlines(6,:) = cross([1441 697 1],[1175 753 1]);


%Vanishing point finded by using l1 and l3
V1 = cross(Llines(1,:),Llines(3,:));
V1 = V1/V1(3);
plot(V1(1),V1(2),'.b','MarkerSize',30);
text(V1(1),V1(2), 'v1', 'FontSize', 18, 'Color', 'w');

%Vanishing point finded by using m1 and m4
V2 = cross(Mlines(1,:),Mlines(4,:));
V2 = V2/V2(3);
plot(V2(1),V2(2),'.b','MarkerSize',30);
text(V2(1),V2(2), 'v2', 'FontSize', 18, 'Color', 'w');

%Find the vanishing line 
vanishingLine = cross(V1,V2);
plot([V1(1),V2(1)],[V1(2),V2(2)],'LineWidth', 2, 'Color', 'blue')

vanishingLine = vanishingLine/norm(vanishingLine(1:2));
HR = [1 0 0; 0 1 0; vanishingLine];
%Trovo i 4 angoli alla base del mobile (matrice 4x3)
corners = zeros(4,3);
%angolo in basso a sinistra
corners(1,:) = cross(Mlines(5,:),Llines(3,:));
%angolo in basso a destra
corners(2,:) = cross(Mlines(6,:),Llines(3,:)); 
%angolo in alto a destra 
corners(3,:) = cross(Mlines(6,:),Llines(2,:));
%angolo in alto a sinistra
corners(4,:) = cross(Mlines(5,:),Llines(2,:));

for i=1:4
    corners(i,:) = corners(i,:)/corners(i,3);
    plot(corners(i,1),corners(i,2),'.b','MarkerSize',30);
    text(corners(i,1),corners(i,2),sprintf("A%d",i), 'FontSize', 18, 'Color', 'w');
end

rectifiedPoints = zeros(size(corners));
for i=1:4
    rectifiedPoint = HR * corners(i,:)';
    rectifiedPoints(i,:) = (rectifiedPoint/rectifiedPoint(3))';
end

%calcolo la profondità m
widthDown = norm(rectifiedPoints(2,1:2)-rectifiedPoints(1,1:2));
widthUp = norm(rectifiedPoints(3,1:2)-rectifiedPoints(4,1:2));
depthLeft = norm(rectifiedPoints(4,1:2)-rectifiedPoints(1,1:2));
depthRight = norm(rectifiedPoints(3,1:2)-rectifiedPoints(2,1:2));
width = (widthUp+widthDown)/2;
depth = (depthRight+depthLeft)/2;
m = depth/width;
disp(m);

%Trovo la matrice K
V3 = cross(Hlines(1,:),Hlines(2,:))+cross(Hlines(1,:),Hlines(3,:))+cross(Hlines(1,:),Hlines(4,:))+cross(Hlines(2,:),Hlines(3,:))+cross(Hlines(2,:),Hlines(4,:))+cross(Hlines(3,:),Hlines(4,:));
V3 = V3/5;
V3 = V3/V3(3);
plot(V3(1),V3(2),'.b','MarkerSize',30);
text(V3(1),V3(2), 'v3', 'FontSize', 18, 'Color', 'w');

imgCenter = [size(imgOriginal,2)/2;size(imgOriginal,1)/2];
scale = max(size(imgOriginal));
V1(1:2) = (V1(1:2)-imgCenter')/scale;
V2(1:2) = (V2(1:2)-imgCenter')/scale;
V3(1:2) = (V3(1:2)-imgCenter')/scale;
V1_n = V1/sqrt(V1(1)^2+V1(2)^2+V1(3)^2);
V2_n = V2/sqrt(V2(1)^2+V2(2)^2+V2(3)^2);
V3_n = V3/sqrt(V3(1)^2+V3(2)^2+V3(3)^2);

A = zeros(3,6);

A(1,:) = [V1_n(1)*V2_n(1), V1_n(1)*V2_n(2)+V1_n(2)*V2_n(1), V1_n(2)*V2_n(2), ...
          V1_n(1)*V2_n(3)+V1_n(3)*V2_n(1), V1_n(2)*V2_n(3)+V1_n(3)*V2_n(2), V1_n(3)*V2_n(3)];

A(2,:) = [V1_n(1)*V3_n(1), V1_n(1)*V3_n(2)+V1_n(2)*V3_n(1), V1_n(2)*V3_n(2), ...
          V1_n(1)*V3_n(3)+V1_n(3)*V3_n(1), V1_n(2)*V3_n(3)+V1_n(3)*V3_n(2), V1_n(3)*V3_n(3)];

A(3,:) = [V2_n(1)*V3_n(1), V2_n(1)*V3_n(2)+V2_n(2)*V3_n(1), V2_n(2)*V3_n(2), ...
          V2_n(1)*V3_n(3)+V2_n(3)*V3_n(1), V2_n(2)*V3_n(3)+V2_n(3)*V3_n(2), V2_n(3)*V3_n(3)];


[~,~,V] = svd(A);
omega_vec = V(:,end);

omega = [omega_vec(1) omega_vec(2) omega_vec(4);
         omega_vec(2) omega_vec(3) omega_vec(5);
         omega_vec(4) omega_vec(5) omega_vec(6)];
ep = 1e-10;
omega = omega + ep * eye(3);
prova = cond(omega);
disp(prova);

[U,D,~] = svd(omega);
K = inv(U*sqrt(D));
K = K/K(3,3);
disp(K);