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