function ComputeCameraUsingTwoVanishingPoints

im = imread('undistorted.png');

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

m11 = [2145;2120;1];
m12 = [2566;1191;1];
m13 = [1804;935;1];
m14 = [1050;1320;1];

m21 = m11;
m22 = m14;
m23 = m12;
m24 = m13;

l11 = GetLineFromTwoPoints(m11,m12);
l12 = GetLineFromTwoPoints(m13,m14);

l21 = GetLineFromTwoPoints(m21,m22);
l22 = GetLineFromTwoPoints(m23,m24);

v1 = GetPointFromTwoLines(l11,l12);
v2 = GetPointFromTwoLines(l21,l22);

r1 = inv(K)*v1/norm(inv(K)*v1);
r2 = inv(K)*v2/norm(inv(K)*v2);

r3 = Vec2Skew(r1)*r2;

R = [r1 r2 r3]
det(R)
R*R'



% h = figure(2)
% clf;
% plot3([0 1], [0 0], [0 0], 'r-', 'LineWidth', 2);
% hold on
% plot3([0 0], [0 1], [0 0], 'g-', 'LineWidth', 2);
% hold on
% plot3([0 0], [0 0], [0 1], 'b-', 'LineWidth', 2);
% hold on
% plot3(r3(1)*5+[0 r1(1)], r3(2)*5+[0 r1(2)], r3(3)*5+[0 r1(3)], 'r-', 'LineWidth', 2);
% hold on
% plot3(r3(1)*5+[0 r2(1)], r3(2)*5+[0 r2(2)], r3(3)*5+[0 r2(3)], 'g-', 'LineWidth', 2);
% hold on
% plot3(r3(1)*5+[0 r3(1)], r3(2)*5+[0 r3(2)], r3(3)*5+[0 r3(3)], 'b-', 'LineWidth', 2);
% 
% 
% DrawPlane(r3, r3*5, 0.5);
% axis equal
% grid on
% axis off
% az = [-180:180];
% for i = 1 : length(az)
%     view(az(i), -0);
%     drawnow;
%     print(h, sprintf('vanishing%07d.png', i), '-dpng');
% end





h = figure(2)
clf;
X = GeneratePlanePoint(r3, -r3*5, 0.5);
% r3 = -r3;
% r1 = -r1;
% r1 = r1-(r1'*r3)*r3;
% r1 = r1/norm(r1);
% r2 = Vec2Skew(r3)*r1;
% R = [r1'; r2'; r3'];
t = -r3*5;

R = R';
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;

x = GetPointFromTwoLines(l11,l12);
figure(1);
clf;
d0 = m11(1);
d1 = v1(1);
y1 = -(l11(1)*d0+l11(3))/l11(2);
y2 = -(l11(1)*d1+l11(3))/l11(2);
plot([d0 d1], [y1 y2], 'r-');

hold on
d0 = m14(1);
d1 = v1(1);
y1 = -(l12(1)*d0+l12(3))/l12(2);
y2 = -(l12(1)*d1+l12(3))/l12(2);
plot([d0 d1], [y1 y2], 'r-');

hold on
d0 = m11(1);
d1 = v2(1);
y1 = -(l21(1)*d0+l21(3))/l21(2);
y2 = -(l21(1)*d1+l21(3))/l21(2);
plot([d0 d1], [y1 y2], 'g-');

hold on
d0 = m12(1);
d1 = v2(1);
y1 = -(l22(1)*d0+l22(3))/l22(2);
y2 = -(l22(1)*d1+l22(3))/l22(2);
plot([d0 d1], [y1 y2], 'g-');
hold on

imshow(im);
hold on
hold on
d0 = m11(1);
d1 = v1(1);
y1 = -(l11(1)*d0+l11(3))/l11(2);
y2 = -(l11(1)*d1+l11(3))/l11(2);
plot([d0 d1], [y1 y2], 'r-');

hold on
d0 = m14(1);
d1 = v1(1);
y1 = -(l12(1)*d0+l12(3))/l12(2);
y2 = -(l12(1)*d1+l12(3))/l12(2);
plot([d0 d1], [y1 y2], 'r-');

hold on
d0 = m11(1);
d1 = v2(1);
y1 = -(l21(1)*d0+l21(3))/l21(2);
y2 = -(l21(1)*d1+l21(3))/l21(2);
plot([d0 d1], [y1 y2], 'g-');

hold on
d0 = m12(1);
d1 = v2(1);
y1 = -(l22(1)*d0+l22(3))/l22(2);
y2 = -(l22(1)*d1+l22(3))/l22(2);
plot([d0 d1], [y1 y2], 'g-');