function ComputeCameraFromHomography

im = imread('undistorted.png');

f = 1300;
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];

u = [m11(1:2)';m12(1:2)';m13(1:2)'; m14(1:2)'];
X = [0 0;1 0;1 1;0 1];
X = [X ones(4,1)]; % homogeneous coordinate

H = ComputeHomography(u, X)

denom = norm(inv(K)*H(:,1));
r1 = inv(K)*H(:,1)/denom;
r2 = inv(K)*H(:,2)/denom;
t = inv(K)*H(:,3)/denom;

r3 = Vec2Skew(r1)*r2;

R = [r1 r2 r3];

X = GeneratePlanePoint([0 0 1]', [0 0 0]', 0.1);

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

R_cam = R;
C = -R_cam'*t;
DisplayCamera(C, R_cam, 0.2);
hold on
plot3([0 0.2], [0 0], [0 0], 'r-', 'LineWidth', 2);
hold on
plot3([0 0], [0 0.2], [0 0], 'g-', 'LineWidth', 2);
hold on
plot3([0 0], [0 0], [0 0.2], 'b-', 'LineWidth', 2);
axis off
H = -H;

denom = norm(inv(K)*H(:,1));
r1 = inv(K)*H(:,1)/denom;
r2 = inv(K)*H(:,2)/denom;
t = inv(K)*H(:,3)/denom;

r3 = Vec2Skew(r1)*r2;
R = [r1 r2 r3];

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

R_cam = R;
C = -R_cam'*t;
DisplayCamera(C, R_cam, 0.2);
hold on
plot3([0 0.2], [0 0], [0 0], 'r-', 'LineWidth', 2);
hold on
plot3([0 0], [0 0.2], [0 0], 'g-', 'LineWidth', 2);
hold on
plot3([0 0], [0 0], [0 0.2], 'b-', 'LineWidth', 2);
axis off

az = [-180:180];
for i = 1 : length(az)
    subplot(1,2,1)
    view(az(i), 13);
    drawnow;
    subplot(1,2,2)
    view(az(i), 13);
    drawnow;
    print(h, sprintf('homo%07d.png', i), '-dpng');
end

return;

R_cam = R;
C = -R_cam'*t;
figure(1);
clf;
subplot(1,2,1)
DisplayCamera(C, R_cam, 0.2);
hold on
plot3([0 0.2], [0 0], [0 0], 'r-');
hold on
plot3([0 0], [0 0.2], [0 0], 'g-');
hold on
plot3([0 0], [0 0], [0 0.2], 'b-');

H = -H

denom = norm(inv(K)*H(:,1));
r1 = inv(K)*H(:,1)/denom;
r3 = inv(K)*H(:,2)/denom;
t = inv(K)*H(:,3)/denom;

r2 = Vec2Skew(r3)*r1;
R = [r1 r2 r3];
R_cam = R;
C = -R_cam'*t;
subplot(1,2,2)
DisplayCamera(C, R_cam, 0.2);
hold on
plot3([0 0.2], [0 0], [0 0], 'r-');
hold on
plot3([0 0], [0 0.2], [0 0], 'g-');
hold on
plot3([0 0], [0 0], [0 0.2], 'b-');