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