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);

Published with MATLAB® 7.10