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

Published with MATLAB® 7.9