function ComputeCameraUsingVanishingPoint

im = imread('undistorted.png');

f = 1224;
K = [f 0 size(im,2)/2;
     0 f size(im,1)/2;
     0 0 1];

m1 = [2563;25;1];
m2 = [2439;545;1];
m3 = [571;25;1];
m4 = [723;498;1];

l1 = GetLineFromTwoPoints(m1,m2);
l2 = GetLineFromTwoPoints(m3,m4);

v1 = GetPointFromTwoLines(l1,l2);
v1 = v1/v1(3);

r3 = inv(K)*v1/norm(inv(K)*v1)
pitch = atan(-r3(1)/norm(r3(2:3)))
roll = atan(r3(2)/r3(3))

%% Rendering


h = figure(2)
clf;

% hold on
% plot3([0 r3(1)], [0 r3(2)], [0 r3(3)], 'k-', 'LineWidth', 2);

X = GeneratePlanePoint(r3, r3*5, 0.5);
r3 = -r3;
r1 = [1; 0; 0];
r1 = r1-(r1'*r3)*r3;
r1 = r1/norm(r1);
r2 = Vec2Skew(r3)*r1;
R = [r1'; r2'; r3'];
t = -r3*5;
H = [R t; 0 0 0 1];
% H = inv(H);

t1 = -R * r3*5+t;
plot3(t1(1)+[0 1], t1(2)+[0 0], t1(3)+[0 0], 'r-', 'LineWidth', 2);
hold on
plot3(t1(1)+[0 0], t1(2)+[0 1], t1(3)+[0 0], 'g-', 'LineWidth', 2);
hold on
plot3(t1(1)+[0 0], t1(2)+[0 0], t1(3)+[0 1], 'b-', 'LineWidth', 2);
hold on

p1 = [0; 0; 0; 1];
q1 = H * p1;
p2 = [1; 0; 0; 1];
q2 = H * p2;
plot3([q1(1) q2(1)], [q1(2) q2(2)], [q1(3) q2(3)], 'r-', 'LineWidth', 2);
hold on

p1 = [0; 0; 0; 1];
q1 = H * p1;
p2 = [0; 1; 0; 1];
q2 = H * p2;
plot3([q1(1) q2(1)], [q1(2) q2(2)], [q1(3) q2(3)], 'g-', 'LineWidth', 2);
hold on

p1 = [0; 0; 0; 1];
q1 = H * p1;
p2 = [0; 0; 1; 1];
q2 = H * p2;
plot3([q1(1) q2(1)], [q1(2) q2(2)], [q1(3) q2(3)], 'b-', 'LineWidth', 2);
hold on

for i = 1 : size(X,1)
    p1 = [X(i,1:3)'; 1];
    q1 = H * p1;
    p2 = [X(i,4:6)'; 1];
    q2 = H * p2;
    hold on
    plot3([q1(1) q2(1)], [q1(2) q2(2)], [q1(3) q2(3)], '-', 'Color', [0.5 0.5 0.5]);
end

% DrawPlane(r3, r3*5, 0.5);
axis equal
grid on
axis off
az = -180:180;
for i = 1 : length(az)
    view(az(i), -10);
    drawnow;
%     print(h, sprintf('vanishing%07d.png', i), '-dpng');
end
% return;

figure(1)
clf;
plot3([0 r3(1)], [0 r3(2)], [0 r3(3)], 'b-');
xlabel('X');
ylabel('Y');
zlabel('Z');
xlim([-1 1]);
ylim([-1 1]);
zlim([-1 1]);
axis equal
grid on

x1 = GetPointFromTwoLines(l1,l2);

figure(1);
clf;
hold on
d0 = m1(1);
d1 = x1(1);
y1 = -(l1(1)*d0+l1(3))/l1(2);
y2 = -(l1(1)*d1+l1(3))/l1(2);
plot([d0 d1], [y1 y2], 'b-');

hold on
d0 = m3(1);
d1 = x1(1);
y1 = -(l2(1)*d0+l2(3))/l2(2);
y2 = -(l2(1)*d1+l2(3))/l2(2);
plot([d0 d1], [y1 y2], 'b-');

imshow(im);
hold on
d0 = m1(1);
d1 = x1(1);
y1 = -(l1(1)*d0+l1(3))/l1(2);
y2 = -(l1(1)*d1+l1(3))/l1(2);
plot([d0 d1], [y1 y2], 'b-', 'LineWidth', 2);

hold on
d0 = m3(1);
d1 = x1(1);
y1 = -(l2(1)*d0+l2(3))/l2(2);
y2 = -(l2(1)*d1+l2(3))/l2(2);
plot([d0 d1], [y1 y2], 'b-', 'LineWidth', 2);

