function [imgpt invpt] = cam_plot(cam,pt,color)
%
% [IMGPT INVPT] = CAM_PLOT(CAM,PT,COLOR)
%
% PT is (N,3) of 3D values

if nargin<3
    color = 'g';
end

sz = size(pt);
N = sz(1);
obj = [pt ones(N,1)];

img = cam.T*obj'; % creates (3,N) array
img = img';
imgpt = [img(:,1)./img(:,3) img(:,2)./img(:,3)];
invpt = cam.Tinv*[imgpt ones(N,2)]';
invpt = invpt';
if nargout<2 || nargin>2
    for k=1:N
        plot3([obj(k,1) invpt(k,1)],[obj(k,2) invpt(k,2)],[obj(k,3) invpt(k,3)],color,'LineWidth',2);
    end
end

Published with MATLAB® 7.9