function E = viprectification_lsqnl(m,Xleft,Xright,w,h,lambda)
%Generates error function for performing quasi-Euclidean rectification
%  E = VIPRECTIFICATION_LSQNL(M,Xleft,Xright,W,H,LAMBDA) computes the Sampson
%  error between 2-D points Xleft and Xright based on the model M using the method
%  of quasi-Euclidean calibration, and so requires the input image size
%  width and height, W and H. LAMBDA is a normalization factor. At 0 it has
%  no effect and the output transform is unbiased; at higher values the
%  derived transform should more closely resemble an affine transform (with
%  [0 0 1] in its bottom row). The default value is 1, which offers
%  moderate normalization.

    % Normalization between Sampson errors and homography affinity
    % constraints.
    if nargin < 6 || isempty(lambda)
        lambda = 1;
    end
    a = (3^m(1))*(w+h)';
    K = [a 0 w/2; 0 a h/2; 0 0 1];
    Rr = viprectification_rotmat(m(2:4), 'YZX');
    Rl = viprectification_rotmat(m(5:6), 'YZ');
    F = inv(K)'*(Rr'*[0 0  0
                      0 0 -1
                      0 1  0]*Rl)/K;
    Hr = (K*Rr)/K;
    Hl = (K*Rl)/K;

    % Compute Sampson error
    symmat3 = [0 -1 0
               1  0 0
               0  0 0];
    E = [( sum(Xright.*(F*Xleft),1).^2 ./ ( sum((symmat3*F*Xleft).^2,1) ...
        + sum((Xright'*F*symmat3).^2,2)' ) )';
        lambda * norm(eye(2)-Hl(1:2,1:2));
        lambda * norm(eye(2)-Hr(1:2,1:2));
        lambda * 0.001 * norm(Hl(1:2,3));
        lambda * 0.001 * norm(Hr(1:2,3))];
end

