%
% Verification of Kneip's p3p code
%
%Per-Erik Forssén, 2016

% Generate three random 3D points
X=rand(3,3)+[0;0;0.5]*[1 1 1];

% Generate a random rotation
R = rand(3,3);
[E,D] = eig(R*R');
R = E*det(E); % ensure det(R)=1
% Generate a random translation
t=rand(3,1)+1.0;

% Get 3D points in global frame by applying the rotation and translation
Xw = [R t]*[X; ones(1,3)];
% Note that the mapping [R t] is from camera to world not the
% inverse as is the common convention.

% Get image points by projection and normalization.
% By "unitary" Kneip means that the points should be normalized to unit vectors.
xp = X*diag(1./sqrt(sum(X.^2,1)));

% Call p3p to find the four solutions
P = p3p(Xw,xp);

for k=1:4,
    Pk=P(:,1+(k-1)*4:4+(k-1)*4);
    if isreal(Pk),
        fprintf('solution %d:',k);
        Pk
    else
        fprintf('solution %d is complex\n',k);
    end
end

fprintf('Ground truth:');
[t R] % Solutions are packed in this way