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;

if size (img,3) == 3 
    grayimage = rgb2gray(img);
end

img = imadjust(grayimage);

img = edge(img,"canny",0.05);

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

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

V1_candidates = zeros(3,3);
combinations = nchoosek(1:3, 2);  
for i = 1:size(combinations, 1)
    V_temp = cross(Llines(combinations(i,1),:), Llines(combinations(i,2),:));
    V1_candidates(i,:) = V_temp/V_temp(3);
end
V1 = mean(V1_candidates, 1);
V1 = V1/V1(3);

V2_candidates = zeros(4,3);
combinations = [1 4; 2 5; 3 6; 1 6];  
for i = 1:size(combinations, 1)
    V_temp = cross(Mlines(combinations(i,1),:), Mlines(combinations(i,2),:));
    V2_candidates(i,:) = V_temp/V_temp(3);
end
V2 = mean(V2_candidates, 1);
V2 = V2/V2(3);

V3_candidates = zeros(3,3);
combinations = nchoosek(1:4, 2);  
for i = 1:3
    V_temp = cross(Hlines(combinations(i,1),:), Hlines(combinations(i,2),:));
    V3_candidates(i,:) = V_temp/V_temp(3);
end
V3 = mean(V3_candidates, 1);
V3 = V3/V3(3);

plot(V1(1),V1(2),'.b','MarkerSize',30);
text(V1(1),V1(2), 'v1', 'FontSize', 18, 'Color', 'w');
plot(V2(1),V2(2),'.b','MarkerSize',30);
text(V2(1),V2(2), 'v2', 'FontSize', 18, 'Color', 'w');

vanishingLine = cross(V1,V2);
plot([V1(1),V2(1)],[V1(2),V2(2)],'LineWidth', 2, 'Color', 'blue');

img_size = max(size(imgOriginal));
img_center = [size(imgOriginal,2)/2; size(imgOriginal,1)/2];

V1(1:2) = (V1(1:2) - img_center')/img_size;
V2(1:2) = (V2(1:2) - img_center')/img_size;
V3(1:2) = (V3(1:2) - img_center')/img_size;

V1_n = V1/sqrt(V1(1)^2 + V1(2)^2);
V2_n = V2/sqrt(V2(1)^2 + V2(2)^2);
V3_n = V3/sqrt(V3(1)^2 + V3(2)^2);

angle12 = acosd(dot(V1_n,V2_n));
angle13 = acosd(dot(V1_n,V3_n));
angle23 = acosd(dot(V2_n,V3_n));

disp(['Angolo tra V1 e V2: ' num2str(angle12)]);
disp(['Angolo tra V1 e V3: ' num2str(angle13)]);
disp(['Angolo tra V2 e V3: ' num2str(angle23)]);

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

epsilon = 1e-10;
omega = omega + epsilon * eye(3);

omega = (omega + omega')/2;

cond_number = cond(omega);
disp(['Condition number of omega: ' num2str(cond_number)]);

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

disp('Parametri della camera:');
disp(['fx = ' num2str(K(1,1))]);
disp(['fy = ' num2str(K(2,2))]);
disp(['u0 = ' num2str(K(1,3))]);
disp(['v0 = ' num2str(K(2,3))]);

disp('Matrice K finale:');
disp(K);