function [ pto Tinv pn ] = back_projection( v, imgpt ) % [PTO TINV PN] = BAC_PROJECTION(V, IMGPT) % Projects image points back to plane [a b c d] = [0 0 1 0]; % V is camera description % IMGPT are the image points (N, 2) array % PTO are the output projected points % TINV is the projection matrix % PN is the plane (as seen by camera) % % R = rotation_matrix(v); Ty = [R [0 0 -v(4)]'; [0 0 0 1]]; pw = [0 0 1 0]; pn = pw*Ty; d = -pn(4); M = [d 0 0 0; 0 d 0 0; 0 0 d 0; pn(1:3) 0]; Tn = Ty*M; Tinv = Tn'; sz = size(imgpt); N = sz(1); k = [imgpt ones(N,2)]; pth = k*Tinv; pto = pth(:,1:3)./repmat(pth(:,4),[1 3]); end