function [y q] = fval3(v,pts) 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; N = 4; k = [pts ones(N,2)]; pth = k*Tn'; pto = pth(:,1:3)./repmat(pth(:,4),[1 3]); % define errors q(1) = pto(2,1)-pto(1,1); q(2) = pto(4,1)-pto(3,1); q(3) = pto(3,2)-pto(1,2); q(4) = pto(4,2)-pto(2,2); q(5) = pto(3,1)-pto(1,1) - 10; y = dot(q,q);