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

figure(2);
imshow(imgOriginal);
hold on; 

%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

%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)];
Circumference = Circumference./Circumference(3,3);


%plot circonferenza

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
[height, width, ~] = size(imgOriginal);
conic_eq = @(x, y) Circumference(1,1)*x.^2 + 2*Circumference(1,2)*x.*y + Circumference(2,2)*y.^2 + 2*Circumference(1,3)*x + 2*Circumference(2,3)*y + Circumference(3,3);
x_min = 1;
x_max = width;
y_min = 1;
y_max = height;
fimplicit(conic_eq, [x_min, x_max, y_min, y_max], 'r', 'LineWidth', 2);                


%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(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(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);
vanishingLine = vanishingLine./vanishingLine(3);
plot([V1(1),V2(1)],[V1(2),V2(2)],'LineWidth', 2, 'Color', 'blue');

HR = [1 0 0; 0 1 0; vanishingLine];
HR = HR./HR(3,3);

syms 'x';
syms 'y';

% every conic provide a 2nd degree equation
eq1 = Circumference(1,1)*x^2 + 2*Circumference(1,2)*x*y + Circumference(2,2)*y^2 + 2*Circumference(3,1)*x + 2*Circumference(3,2)*y + Circumference(3,3);
eq2 = vanishingLine(1)*x + vanishingLine(2)*y+ vanishingLine(3);
% solve a system with a line and the conic: this gives a 2th degree equation
eqns = [eq1 == 0, eq2 == 0];
S12 = solve(eqns, [x,y], 'IgnoreAnalyticConstraints',true,'Maxdegree',4);
% hence you get 2 pairs of complex conjugate solution
s1 = [double(S12.x(1)); double(S12.y(1)); 1];
s2 = [double(S12.x(2)); double(S12.y(2)); 1];
II = s1;
JJ = s2;
imDCCP = II*JJ' + JJ*II';
%[V,D] = eigs(imDCCP);
%disp("matrice autovalori");
%disp(D);
%valori negativi 
%procediamo per via geometrica
Q = inv(HR)'*Circumference*inv(HR);
Q = Q./Q(3,3);
par_geometrici = AtoG([Q(1,1),2*Q(1,2),Q(2,2),2*Q(1,3),2*Q(2,3),Q(3,3)]);
center = par_geometrici(1:2);
axes = par_geometrici(3:4);
angle = par_geometrici(5);
alpha = angle;
a = axes(1);
b = axes(2);
U = [cos(alpha), -sin(alpha); sin(alpha), cos(alpha)];
S = diag([1, a/b]);
K = U*S*U';
A = [K zeros(2,1); zeros(1,2),1];
T = A*HR;
T = T./T(3,3);
tform = projective2d(T');
J = imwarp(imgOriginal,tform);
hold off;
figure(3);
axis equal;
imshow(J);
hold on;
title('Metric rectification.')


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

%test sull'angolo:
disp("TEST SUGLI ANGOLI DELLA BASE DEL PARALLELEPIPEDO RETTIFICATA");
lati = zeros(4,3);
lati(1,:) = cross(rectifiedPoints(1,:),rectifiedPoints(2,:));
lati(2,:) = cross(rectifiedPoints(2,:),rectifiedPoints(3,:));
lati(3,:) = cross(rectifiedPoints(3,:),rectifiedPoints(4,:));
lati(4,:) = cross(rectifiedPoints(4,:),rectifiedPoints(1,:));
disp("Angolo tra L3 e M6:");
disp(rad2deg(acos((lati(1,1)*lati(2,1)+(lati(1,2)*lati(2,2)))/sqrt((lati(1,1)^2+lati(1,2)^2)*(lati(2,1)^2+lati(2,2)^2)))));
disp("angolo tra M6 e L2:");
disp(rad2deg(acos((lati(2,1)*lati(3,1)+(lati(2,2)*lati(3,2)))/sqrt((lati(2,1)^2+lati(2,2)^2)*(lati(3,1)^2+lati(3,2)^2)))));
disp("Angolo tra L2 e M5:");
disp(rad2deg(acos((lati(3,1)*lati(4,1)+(lati(3,2)*lati(4,2)))/sqrt((lati(3,1)^2+lati(3,2)^2)*(lati(4,1)^2+lati(4,2)^2)))));
disp("Angolo tra M5 e L3:");
disp(rad2deg(acos((lati(1,1)*lati(4,1)+(lati(1,2)*lati(4,2)))/sqrt((lati(1,1)^2+lati(1,2)^2)*(lati(4,1)^2+lati(4,2)^2)))));

%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("Profondità calcolata:");
disp(m);


%Trovo la matrice di calibrazione K
V3 = cross(Hlines(1,:),Hlines(4,:));
V3 = V3./V3(3);
Hinv = inv(T);
Hinv = Hinv./Hinv(3,3);
h1 = Hinv(:,1)';
h2 = Hinv(:,2)';
h3 = Hinv(:,3)';


syms 'u0'; 
syms 'v0'; 
syms 'Fy'; 
syms 'a'; 
W = [a^2,0,-u0*a^2;0,1,-v0;-u0*a^2,-v0,Fy^2+(a^2)*(u0^2)+v0^2];
eq1 = h1 * W * h2';
eq2 = (h1 * W * h1') - (h2 * W * h2');
eq3 = V3 * W * h1';
eq4 = V3 * W * h2';
eqns = [eq1 == 0, eq2 == 0, eq3 == 0, eq4 == 0];
S1234 = solve(eqns, [u0,v0,Fy,a]);
K = [double(S1234.a(4)*S1234.Fy(4)),0,double(S1234.u0(4));0,double(S1234.Fy(4)),double(S1234.v0(4));0,0,1];
disp("Matrice K:");
disp(K);

%trovo l'altezza del parallelepipedo
vanishingLine2 = cross(V1,V3);
vanishingLine2 = vanishingLine2./vanishingLine2(3);

n = (K' * vanishingLine2')';
n = n./norm(n);
randomVector = [1,1,1];
r1 = cross(n,randomVector);
r1 = r1./norm(r1);
r2 = cross(n,r1);
r2 = r2./norm(r2);
R = [r1',r2',n'];
T2 = inv(K*R);
T2 = T2./T2(3,3);

%Trovo i 4 spigoli frontali del mobile (matrice 4x3)
corners2 = zeros(4,3);
%angolo in basso a sinistra
corners2(1,:) = cross(Hlines(1,:),Llines(2,:));
%angolo in basso a destra
corners2(2,:) = cross(Hlines(4,:),Llines(2,:)); 
%angolo in alto a destra 
corners2(3,:) = cross(Hlines(4,:),Llines(1,:));
%angolo in alto a sinistra
corners2(4,:) = cross(Hlines(1,:),Llines(1,:));

hold off;
figure(4);
hold on;
axis equal;
rectifiedPoints2 = zeros(size(corners2));
for i=1:4
    rectifiedPoint = T2 * corners2(i,:)';
    rectifiedPoints2(i,:) = (rectifiedPoint./rectifiedPoint(3))';
    plot(rectifiedPoints2(i,1),rectifiedPoints2(i,2),'.b','MarkerSize',30);
    text(rectifiedPoints2(i,1),rectifiedPoints2(i,2),sprintf("A%d",i), 'FontSize', 18, 'Color', 'w');
end

%test sull'angolo:
disp("TEST SUGLI ANGOLI DEL PIANO FRONTALE RETTIFICATO DEL PARALLELEPIPEDO");
lati2 = zeros(4,3);
lati2(1,:) = cross(rectifiedPoints2(1,:),rectifiedPoints2(2,:));
lati2(2,:) = cross(rectifiedPoints2(2,:),rectifiedPoints2(3,:));
lati2(3,:) = cross(rectifiedPoints2(3,:),rectifiedPoints2(4,:));
lati2(4,:) = cross(rectifiedPoints2(4,:),rectifiedPoints2(1,:));
disp("Angolo tra L2 e H4:");
disp(rad2deg(acos((lati2(1,1)*lati2(2,1)+(lati2(1,2)*lati2(2,2)))/sqrt((lati2(1,1)^2+lati2(1,2)^2)*(lati2(2,1)^2+lati2(2,2)^2)))));
disp("angolo tra H4 e L1:");
disp(rad2deg(acos((lati2(2,1)*lati2(3,1)+(lati2(2,2)*lati2(3,2)))/sqrt((lati2(2,1)^2+lati2(2,2)^2)*(lati2(3,1)^2+lati2(3,2)^2)))));
disp("Angolo tra L1 e H1:");
disp(rad2deg(acos((lati2(3,1)*lati2(4,1)+(lati2(3,2)*lati2(4,2)))/sqrt((lati2(3,1)^2+lati2(3,2)^2)*(lati2(4,1)^2+lati2(4,2)^2)))));
disp("Angolo tra L2 e H1:");
disp(rad2deg(acos((lati2(1,1)*lati2(4,1)+(lati2(1,2)*lati2(4,2)))/sqrt((lati2(1,1)^2+lati2(1,2)^2)*(lati2(4,1)^2+lati2(4,2)^2)))));

%trovo l'altezza: 
widthDown2 = norm(rectifiedPoints2(2,1:2)-rectifiedPoints2(1,1:2));
widthUp2 = norm(rectifiedPoints2(3,1:2)-rectifiedPoints2(4,1:2));
heightLeft2 = norm(rectifiedPoints2(4,1:2)-rectifiedPoints2(1,1:2));
heightRight2 = norm(rectifiedPoints2(3,1:2)-rectifiedPoints2(2,1:2));
width2 = (widthUp2+widthDown2)/2;
height2 = (heightRight2+heightLeft2)/2;
m2 = height2/width2;
disp("Altezza calcolata:");
disp(m2);

%trovo le coordinate di dodici punti della curva sconosciuta S
xx2 = zeros(1,length(xx));
yy2 = zeros(1,length(yy));
curvePointsRectified = zeros(length(xx),3);
for i=1:length(xx)
    curvePoint = [xx(i),yy(i),1];
    curvePointsRectified(i,:) = (T * curvePoint')';
    curvePointsRectified(i,:) = curvePointsRectified(i,:)./curvePointsRectified(i,3);
end
hold off;
figure(5);
hold on;
axis equal;
plot(curvePointsRectified(:,1), curvePointsRectified(:,2), 'r-', 'LineWidth', 1.5, 'DisplayName', 'Spline 2D');

%trovo la posizione della cam rispetto al vertice frontale in passo a sinistra
%caratterizzo un'omografia con sistema di riferimento planare sul vertice in basso a sinistra: 
corners2(1,:) = corners2(1,:)./corners2(1,3);
corners2(2,:) = corners2(2,:)./corners2(2,3);
corners2(3,:) = corners2(3,:)./corners2(3,3);
corners2(4,:) = corners2(4,:)./corners2(4,3);
image_points = [corners2(1,1:2);corners2(2,1:2);corners2(3,1:2);corners2(4,1:2)];
real_points = [0,0; 1,0; 1,0.395; 0,0.395];

%omografia dalla scena all'immagine
Hscenetoimage = fitgeotrans(real_points,image_points,'projective');
%estraggo la soluzione
Hscenetoimage = Hscenetoimage.T.';

h1 = Hscenetoimage(:,1);
h2 = Hscenetoimage(:,2);
h3 = Hscenetoimage(:,3);

% normalization factor
lambda = 1 / norm(K \ h1);
% r1 = K^-1 * h1 normalized
r1 = (K \ h1) * lambda; 
r2 = (K \ h2) * lambda; 
r3 = cross(r1,r2); 

% due to noise in the data R may be not a true rotation matrix.
% approximate it through svd, obtaining a orthogonal matrix
R = [r1, r2, r3];
[U, ~, V] = svd(R);
R = U * V';
T = (K \ (lambda * h3));
cameraRotation = R.';
cameraPosition = -R.'*T;

%calcolo la scala
corners_rectified1 = inv(Hscenetoimage) * corners2(1,:)';
corners_rectified2 = inv(Hscenetoimage) * corners2(2,:)';
corners_rectified1 = corners_rectified1./corners_rectified1(3);
corners_rectified2 = corners_rectified2./corners_rectified2(3);
hold off;
figure(6);
hold on;
plot(corners_rectified1(1),corners_rectified1(2),'.b','MarkerSize',30);
text(corners_rectified1(1),corners_rectified1(2),'X', 'FontSize', 18, 'Color', 'w');
plot(corners_rectified2(1),corners_rectified2(2),'.b','MarkerSize',30);
text(corners_rectified2(1),corners_rectified2(2),'X', 'FontSize', 18, 'Color', 'w');


distance = sqrt((corners_rectified2(1)-corners_rectified1(1))^2+(corners_rectified2(2)-corners_rectified1(2))^2);
scale_factor = 1/distance;
disp(cameraPosition);
cameraPosition = cameraPosition*scale_factor;
disp(cameraPosition);
disp(norm(cameraPosition));
disp(cameraRotation);
hold off;
figure(7);
hold on;
plotCamera('Location', cameraPosition, 'Orientation', cameraRotation.', 'Size', 0.05);
pcshow([real_points, zeros(size(real_points,1), 1)],'red','VerticalAxisDir', 'up', 'MarkerSize', 20);
real_points_3D = [real_points, zeros(size(real_points,1),1)];
all_points = [real_points_3D; real_points_3D(1,:)];
plot3(all_points(:,1),all_points(:,2),all_points(:,3),'-o','LineWidth',2,'Color','r');
xlabel('X')
ylabel('Y')
zlabel('Z')

%'LineWidth', 2, 'Color', 'blue');

%tform = fitgeotrans(image_points, real_points, 'projective');
%Hnew = tform.T.';
%B = inv(K) * inv(Hnew);
%R1 = (B(:,1))';
%R2 = (B(:,2))';
%O1 = (B(:,3))';
%real_distance = 1;
%calculated_distance = norm(image_points(2,:) - image_points(1,:));
%scale_factor = real_distance / calculated_distance;
%O1 = O1 * scale_factor;
%disp(norm(O1));
%R3 = cross(R1,R2);
%R1 = R1./norm(R1);
%R2 = R2./norm(R2);
%R3 = R3./norm(R3);

























function par_geometrici = AtoG(coeff)
    % coeff: [A, 2B, C, 2D, 2E, F]
    % Restituisce: [x0, y0, a, b, theta]
    
    % Estrai i coefficienti dalla conica
    A = coeff(1);
    B = coeff(2) / 2; 
    C = coeff(3);
    D = coeff(4) / 2;
    E = coeff(5) / 2;
    F = coeff(6);
    
    % Calcolo del discriminante per determinare il tipo di conica
    delta = A*C - B^2;
    if delta == 0
        error('La matrice rappresenta una parabola o una degenerazione.');
    end
    
    % Calcolo del centro della conica
    x0 = (C*D - B*E) / delta;
    y0 = (A*E - B*D) / delta;
    
    % Matrice associata alla parte quadratica
    Q = [A, B; B, C];
    
    % Calcolo degli autovalori e autovettori di Q
    [V, Lambda] = eig(Q); % Lambda: matrice diagonale degli autovalori
    lambda1 = Lambda(1,1); % Primo autovalore
    lambda2 = Lambda(2,2); % Secondo autovalore
    
    % Assi principali della conica
    a = sqrt(abs(F / (delta * lambda1)));
    b = sqrt(abs(F / (delta * lambda2)));
    
    % Calcolo dell'angolo di rotazione
    theta = atan2(V(2,1), V(1,1)); % Angolo di rotazione
    
    % Output: [x0, y0, a, b, theta]
    par_geometrici = [x0, y0, a, b, theta];
end