function [R,T,K] = calibration2Dto3D(X,x) n = size(X,2); % number of points % Build the matrix of coefficients from the given points M = [X', ones(n,1), zeros(n,4), -repmat(x(1,:)', [1,4]).*[X', ones(n,1)];... zeros(n,4), X', ones(n,1), -repmat(x(2,:)', [1,4]).*[X', ones(n,1)]]; % Singular Value Decomposition [~,~,v] = svd(M); % Turn the vector back into a 3x4 matrix P = reshape(v(:,12), [4,3])'; % Express this matrix as R*K using QR Factorization [R, K] = qr(P(:,1:3)); % Recover T T = inv(K)*P(:,4); end