function p = initialize(W, xi)

n = size(W, 2);
if size(xi, 2) ~= n
    error('Number of world and image points must be the same')
end

% Assemble the 2N by 12 homogeneous system
W1 = [W' ones(n, 1)];
Z = zeros(size(W1));
L = [W1, Z, - (xi(1, :)' * ones(1, 4)) .* W1; ...
    Z, W1, - (xi(2, :)' * ones(1, 4)) .* W1];
[~, ~, V] = svd(L);
q = V(:, end);

% Unpack the parameters from the solution q
p = parameters(q);

% Make sure R has positive determinant
if det(p.R) < 0,
    p = parameters(-q);
end

% Make sure R is orthogonal
[U, ~, V] = svd(p.R);
p.R = U * V';

p = orderfields(p, {'R', 't', 'S', 'pi'});

    function c = parameters(p)
        p = p / norm(p(9:11));
        B = [p(1:3)'; p(5:7)'; p(9:11)'];
        a = p(4:4:end);
        C = B * B';
        c.pi = C(1:2, 3);
        c.S = diag(sqrt(diag(C(1:2, 1:2)) - c.pi .^ 2));
        A = [c.S, c.pi; 0 0 1];
        c.R = A \ B;
        c.t = - B \ a;
    end
end