subpixcrnr

 SUBPIXCRNR gets the subpixel position of the chessboard corners.
 
 SUBPIXCRNR Finds the sub-pixel corners on the image I with initial guess xt
 xt and xc are 2xN matrices. The first component is the x coordinate
 (horizontal) and the second component is the y coordinate (vertical).
 
 USAGE:
     [xc] = subpixcrnr(xt,I);
 
 Based on Harris corner finder method.
 
 Finds corners to a precision below .1 pixel!
 Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
 Sept 1998 - UPDATED to handle diverged points: we keep the original points
 good is a binary vector indicating wether a feature point has been properly
 found.
 
 Add a zero zone of size wx2,wy2
 July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
 resolution and larger number of iterations.
 
 California Institute of Technology
 (c) Jean-Yves Bouguet -- Oct. 14th, 1997

0001 function [xc,good,bad,type] = subpixcrnr(xt,I,wintx,winty,wx2,wy2)
0002 % SUBPIXCRNR gets the subpixel position of the chessboard corners.
0003 %
0004 % SUBPIXCRNR Finds the sub-pixel corners on the image I with initial guess xt
0005 % xt and xc are 2xN matrices. The first component is the x coordinate
0006 % (horizontal) and the second component is the y coordinate (vertical).
0007 %
0008 % USAGE:
0009 %     [xc] = subpixcrnr(xt,I);
0010 %
0011 % Based on Harris corner finder method.
0012 %
0013 % Finds corners to a precision below .1 pixel!
0014 % Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
0015 % Sept 1998 - UPDATED to handle diverged points: we keep the original points
0016 % good is a binary vector indicating wether a feature point has been properly
0017 % found.
0018 %
0019 % Add a zero zone of size wx2,wy2
0020 % July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
0021 % resolution and larger number of iterations.
0022 %
0023 % California Institute of Technology
0024 % (c) Jean-Yves Bouguet -- Oct. 14th, 1997
0025 
0026 % double precision
0027 % I=double(I);
0028 
0029 line_feat = 1; % set to 1 to allow for extraction of line features.
0030 
0031 xt = xt';
0032 % xt = fliplr(xt);
0033 
0034 
0035 if nargin < 4,
0036    winty = 5;
0037    if nargin < 3,
0038       wintx = 5;
0039    end;
0040 end;
0041 
0042 
0043 if nargin < 6,
0044    wx2 = -1;
0045    wy2 = -1;
0046 end;
0047 
0048 
0049 %mask = ones(2*wintx+1,2*winty+1);
0050 mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);
0051 
0052 
0053 % another mask:
0054 [X,Y] = meshgrid(-winty:winty,-wintx:wintx);
0055 mask2 = X.^2 + Y.^2;
0056 mask2(wintx+1,winty+1) = 1;
0057 mask2 = 1./mask2;
0058 %mask - mask2;
0059 
0060 
0061 if (wx2>0) & (wy2>0),
0062    if ((wintx - wx2)>=2)&((winty - wy2)>=2),
0063       mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
0064    end;
0065 end;
0066 
0067 offx = [-wintx:wintx]'*ones(1,2*winty+1);
0068 offy = ones(2*wintx+1,1)*[-winty:winty];
0069 
0070 resolution = 0.005;
0071 
0072 MaxIter = 10;
0073 
0074 [nx,ny] = size(I);
0075 N = size(xt,1);
0076 
0077 xc = xt; % first guess... they don't move !!!
0078 
0079 type = zeros(1,N);
0080 
0081 
0082 for i=1:N,
0083 
0084     v_extra = resolution + 1;         % just larger than resolution
0085 
0086     compt = 0;                 % no iteration yet
0087 
0088     while (norm(v_extra) > resolution) & (compt<MaxIter),
0089 
0090         cIx = xc(i,1);             %
0091         cIy = xc(i,2);             % Coords. of the point
0092         crIx = round(cIx);         % on the initial image
0093         crIy = round(cIy);         %
0094         itIx = cIx - crIx;         % Coefficients
0095         itIy = cIy - crIy;         % to compute
0096         if itIx > 0,             % the sub pixel
0097             vIx = [itIx 1-itIx 0]';     % accuracy.
0098         else
0099             vIx = [0 1+itIx -itIx]';
0100         end;
0101         if itIy > 0,
0102             vIy = [itIy 1-itIy 0];
0103         else
0104             vIy = [0 1+itIy -itIy];
0105         end;
0106 
0107 
0108         % What if the sub image is not in?
0109 
0110         if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
0111         elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
0112         else
0113             xmin = crIx-wintx-2; xmax = crIx+wintx+2;
0114         end;
0115 
0116         if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
0117         elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
0118         else
0119             ymin = crIy-winty-2; ymax = crIy+winty+2;
0120         end;
0121 
0122 
0123         SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
0124         SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
0125         SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
0126         [gy,gx] = gradient(SI);         % The gradient image
0127         gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
0128         gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients
0129 
0130         px = cIx + offx;
0131         py = cIy + offy;
0132 
0133         gxx = gx .* gx .* mask;
0134         gyy = gy .* gy .* mask;
0135         gxy = gx .* gy .* mask;
0136 
0137 
0138         bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];
0139 
0140         a = sum(sum(gxx));
0141         b = sum(sum(gxy));
0142         c = sum(sum(gyy));
0143 
0144         dt = a*c - b^2;
0145 
0146         xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
0147 
0148 
0149         %keyboard;
0150 
0151         if line_feat,
0152 
0153             G = [a b;b c];
0154             [U,S,V]  = svd(G);
0155 
0156             %keyboard;
0157 
0158             % If non-invertible, then project the point onto the edge orthogonal:
0159 
0160             if (S(1,1)/S(2,2) > 50),
0161                 % projection operation:
0162                 xc2 = xc2 + sum((xc(i,:)-xc2).*(V(:,2)'))*V(:,2)';
0163                 type(i) = 1;
0164             end;
0165 
0166         end;
0167 
0168 
0169         %keyboard;
0170 
0171         %      G = [a b;b c];
0172         %      [U,S,V]  = svd(G);
0173 
0174 
0175         %      if S(1,1)/S(2,2) > 150,
0176         %     bb2 = U'*bb;
0177         %     xc2 = (V*[bb2(1)/S(1,1) ;0])';
0178         %      else
0179         %     xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
0180         %      end;
0181 
0182 
0183         %if (abs(a)> 50*abs(c)),
0184         %     xc2 = [(c*bb(1)-b*bb(2))/dt xc(i,2)];
0185         %      elseif (abs(c)> 50*abs(a))
0186         %     xc2 = [xc(i,1) (a*bb(2)-b*bb(1))/dt];
0187         %      else
0188         %     xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
0189         %      end;
0190 
0191         %keyboard;
0192 
0193         if (isnan(xc2(1)) || isnan(xc2(2))),
0194             xc2 = [0 0];
0195         end;
0196         
0197         v_extra = xc(i,:) - xc2;
0198 
0199         xc(i,:) = xc2;
0200 
0201         %      keyboard;
0202 
0203         compt = compt + 1;
0204 
0205     end
0206 end;
0207 
0208 
0209 % check for points that diverge:
0210 
0211 delta_x = xc(:,1) - xt(:,1);
0212 delta_y = xc(:,2) - xt(:,2);
0213 
0214 %keyboard;
0215 
0216 
0217 bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
0218 good = ~bad;
0219 in_bad = find(bad);
0220 
0221 % For the diverged points, keep the original guesses:
0222 
0223 xc(in_bad,:) = xt(in_bad,:);
0224 
0225 % xc = fliplr(xc);
0226 xc = xc';
0227 
0228 bad = bad';
0229 good = good';