function XYproj = AdjustProjectedPointsOnConic(XY,XYproj0,A,kmax)
% Adjust ("polish") the projected points on the conic
% by a few Newton's iterations (kmax is maximum number of iterations)
% (this step ensures maximal accuracy)
% Nikolai Chernov, July 2012
XYproj = XYproj0;
if (kmax <= 0), return, end;
n = size(XY,1);
for i=1:n % loop over the given points
u = XY(i,1); v = XY(i,2); % coordinates of the original point
x = XYproj0(i,1); % coordinates of its projection...
y = XYproj0(i,2); % found previously and being adjusted here
Qbest = realmax;
for k=1:(kmax+1)
Fx = A(1)*x + A(2)*y + A(4);
Fy = A(2)*x + A(3)*y + A(5);
% Fx and Fy are the derivatives of F (see below)
% with respect to x and y
F = ((Fx + A(4))*x + (Fy + A(5))*y + A(6))/2;
% ideally, F=0
% (only true if the projected point lies ON the conic)
G = (x-u)*Fy - (y-v)*Fx;
% ideally, G=0
% (only true if the projection is orthogonal to the conic)
%fprintf(1,' %d %.5f %.5f %.5f %.5f\n',k,x,y,FG);
Q = F^2 + G^2; % Q is the "quality" of the adjustment
if (Q >= Qbest), break, end; % no improvement, quit
xbest = x;
ybest = y;
Qbest = Q;
if (k > kmax), break, end;
Gx = A(2)*(x-u) - A(1)*(y-v) + Fy;
Gy = A(3)*(x-u) - A(2)*(y-v) - Fx;
% Gx and Gy are the derivatives of G
% with respect to x and y
% Next we compute the Newton step
det = Fx*Gy - Fy*Gx; % determinant
dx = (F*Gy - G*Fy)/det;
dy = (G*Fx - F*Gx)/det; % Cramer's rule
if ((~isfinite(dx))||(~isfinite(dy))), break, end;
x = x - dx;
y = y - dy; % adjust the projection
end
XYproj(i,1) = xbest;
XYproj(i,2) = ybest; % record the adjusted projection
end
end