function [y q] = fval4(v,pts) R = rotation_matrix([0 v(1) 0]); Ty = [R [0 0 -v(2)]'; [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 v1 = diff(pto(1:2,:)); v2 = diff(pto(3:4,:)); vn1 = norm(v1); vn2 = norm(v2); v3 = cross(v1,v2)/sqrt(vn1*vn2); q(1) = v3(3); v4 = pto(1,:)-pto(3,:); v3 = cross(v4,v1)/norm(v1); q(2) = abs(v3(3))-10.0; y = dot(q,q);