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 = 5000; 
p = houghpeaks(H,numpeaks,"Threshold",ceil(0.1*max(H(:))));
lines = houghlines(img,teta,rho,p,"FillGap",1,"MinLength",1);
figure(1);
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
hold off;

Hlines(1,:) = cross([384 236 1],[312 671 1]);
Hlines(2,:) = cross([776 301 1],[803 685 1]);
Hlines(3,:) = cross([1075 343 1],[1157 686 1]);
Hlines(4,:) = cross([1321 375 1],[1441 698 1]);

Llines(1,:) = cross([431 239 1],[1311 368 1]);
Llines(2,:) = cross([315 714 1],[1402 697 1]);
Llines(3,:) = cross([420 778 1],[1192 749 1]);

Mlines(1,:) = cross([406 257 1],[460 452 1]);
Mlines(2,:) = cross([775 297 1],[719 470 1]);
Mlines(3,:) = cross([1060 360 1],[968 461 1]);
Mlines(4,:) = cross([1144 496 1],[1307 384 1]);
Mlines(5,:) = cross([308 718 1],[416 776 1]);
Mlines(6,:) = cross([1441 698 1],[1193 749 1]);

%trovo l'immagine della circonferenza
circumferencePoints = [418,559,1;427,543,1;474,521,1;503,515,1;528,512,1;601,512,1;645,517,1;677,524,1;717,541,1;740,567,1;734,588,1];
eqC = [circumferencePoints(:,1).^2, circumferencePoints(:,1).*circumferencePoints(:,2), circumferencePoints(:,2).^2,circumferencePoints(:,1), circumferencePoints(:,2), ones(size(circumferencePoints,1),1)];
[~, ~, V] = svd(eqC);
p = V(:,end);
Circumference = [p(1), p(2)/2, p(4)/2; p(2)/2, p(3), p(5)/2; p(4)/2, p(5)/2, p(6)];
disp(Circumference);

%plot circonferenza
figure(2);
imshow(imgOriginal);
hold on; 
epsi = 1e-4;
for i=1:1200
    for j=1:1600
        if(abs([j i 1]*Circumference*[j i 1]') <= epsi)
           plot(j, i, '.b', 'MarkerSize', 6);
        end
    end
end


%trovo l'immagine della curva
curvePoints = [833,541;833,531;840,525;849,520;866,515;884,513;901 514;915,516;936,523;945,527;951,533];
x = curvePoints(:, 1);
y = curvePoints(:, 2);

N = length(x);
t = zeros(N,1);
for i = 2:N
    dx = x(i) - x(i-1);
    dy = y(i) - y(i-1);
    t(i) = t(i-1) + sqrt(dx^2 + dy^2);
end
ppx = spline(t, x);
ppy = spline(t, y);
t_fine = linspace(t(1), t(end), 200);
xx = ppval(ppx, t_fine);
yy = ppval(ppy, t_fine);

plot(xx, yy, 'r-', 'LineWidth', 1.5, 'DisplayName', 'Spline 2D');

%L direction vanishing point
V1 = cross(Llines(1,:),Llines(2,:))+cross(Llines(1,:),Llines(3,:))+cross(Llines(2,:),Llines(3,:));
V1 = V1/3;
V1 = V1/V1(3);
plot(V1(1),V1(2),'.b','MarkerSize',30);
text(V1(1),V1(2), 'v1', 'FontSize', 18, 'Color', 'w');

%M direction vanishing point
%V2 = cross(Mlines(1,:),Mlines(2,:))+cross(Mlines(1,:),Mlines(3,:))+cross(Mlines(1,:),Mlines(4,:))+cross(Mlines(1,:),Mlines(5,:))+cross(Mlines(1,:),Mlines(6,:))+cross(Mlines(2,:),Mlines(3,:))+cross(Mlines(2,:),Mlines(4,:))+cross(Mlines(2,:),Mlines(5,:))+cross(Mlines(2,:),Mlines(6,:))+cross(Mlines(3,:),Mlines(4,:))+cross(Mlines(3,:),Mlines(5,:))+cross(Mlines(3,:),Mlines(6,:))+cross(Mlines(4,:),Mlines(5,:))+cross(Mlines(4,:),Mlines(6,:))+cross(Mlines(5,:),Mlines(6,:));
%V2 = V2/15;
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')
disp(vanishingLine);

vanishingLine = vanishingLine/norm(vanishingLine);
disp(vanishingLine);

syms xr yr zr real
zr = (-xr*vanishingLine(1) - yr*vanishingLine(2)) / vanishingLine(3);
X = [xr, yr, zr];
new_eq = simplify(X * Circumference * X'); 
[sol_xr,sol_yr] = solve(new_eq == 0, [xr, yr]);
sol_zr = (-sol_xr*vanishingLine(1) - sol_yr*vanishingLine(2)) / vanishingLine(3);
disp(sol_xr);
disp(sol_yr);
disp(sol_zr);


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/6;
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);

%V1 = V1/sqrt(V1(1)^2+V1(2)^2+V1(3)^2);
%V2 = V2/sqrt(V2(1)^2+V2(2)^2+V2(3)^2);
%V3 = 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)];


A(1,:) = [V1(1)*V2(1) V1(2)*V2(2) V1(3)*V2(3) V1(1)*V2(2)+V1(2)*V2(1) V1(1)*V2(3)+V1(3)*V2(1) V1(2)*V2(3)+V1(3)*V2(2)];
A(2,:) = [V1(1)*V3(1) V1(2)*V3(2) V1(3)*V3(3) V1(1)*V3(2)+V1(2)*V3(1) V1(1)*V3(3)+V1(3)*V3(1) V1(2)*V3(3)+V1(3)*V3(2)];
A(3,:) = [V2(1)*V3(1) V2(2)*V3(2) V2(3)*V3(3) V2(1)*V3(2)+V2(2)*V3(1) V2(1)*V3(3)+V2(3)*V3(1) V2(2)*V3(3)+V2(3)*V3(2)];
[~,~,V] = svd(A);
omega_vec = V(:,end);

omega = [omega_vec(1) omega_vec(4) omega_vec(5);omega_vec(4) omega_vec(2) omega_vec(6); omega_vec(5) omega_vec(6) omega_vec(3)];
omega = omega/omega(3,3);

%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);

M = inv(omega);





L = chol(M,'lower');
K = L';
K = K / K(3,3);
disp(K);