getgrid

 GETGRID arranges the points that pass the chessboard filter into a grid.
 
 GETGRID returns the arranged grid of the set of candidate chessboard
 corners. 
 
 USAGE:
     crnrsgridout=getgrid(crnrs,pixs,peaklocs,ix,iy);
 
 INPUTS:
     crnrs: 2xN matrix of the N candidate corners.
 
     pixs: 2xM matrix of the M Harris corners
 
     peaklocs: edge peak locations at each point (necesary for
     arrangement)
 
     ix: the x component of the gradient image
 
     iy: the y component of the gradient image
 
 OUTPUTS:
     crnrsgridout: MxNx2 dimensional matrix contianing arranged points

0001 function crnrsgridout=getgrid(crnrs,pixs,peaklocs,ix,iy,debug)
0002 % GETGRID arranges the points that pass the chessboard filter into a grid.
0003 %
0004 % GETGRID returns the arranged grid of the set of candidate chessboard
0005 % corners.
0006 %
0007 % USAGE:
0008 %     crnrsgridout=getgrid(crnrs,pixs,peaklocs,ix,iy);
0009 %
0010 % INPUTS:
0011 %     crnrs: 2xN matrix of the N candidate corners.
0012 %
0013 %     pixs: 2xM matrix of the M Harris corners
0014 %
0015 %     peaklocs: edge peak locations at each point (necesary for
0016 %     arrangement)
0017 %
0018 %     ix: the x component of the gradient image
0019 %
0020 %     iy: the y component of the gradient image
0021 %
0022 % OUTPUTS:
0023 %     crnrsgridout: MxNx2 dimensional matrix contianing arranged points
0024 
0025 
0026 
0027 % check input
0028 if ~exist('debug','var') || isempty(debug)
0029     debug=0;
0030 end
0031 
0032 %adjust array direction
0033 if size(crnrs,1)>2
0034     crnrs=crnrs';
0035 end
0036 
0037 nocrnrs=size(crnrs,2);
0038 
0039 % get mean point
0040 centerpt=mean(crnrs,2);
0041 [currentpt,ctindx]=findnearest(centerpt,crnrs,1,0);
0042 
0043 % parameters
0044 angth=deg2rad(15);
0045 
0046 % always store first point
0047 valid=1;
0048 
0049 % inititalise matrices
0050 crnrsgrid=zeros(nocrnrs,nocrnrs);
0051 crnrsgriddone=zeros(nocrnrs,nocrnrs);
0052 
0053 % place first point in the middle of the grid
0054 xg=round(nocrnrs/2);
0055 yg=round(nocrnrs/2);
0056 
0057 % enumerate different directions
0058 right=1;
0059 top=2;
0060 left=3;
0061 bottom=4;
0062 
0063 % setup position matrix
0064 posmat=[0,top,0;left,0,right;0,bottom,0];
0065 
0066 % set while loop flag
0067 notdone=1;
0068 
0069 % set loop counter
0070 % just to ensure safe performance, prevent loop from going on forever
0071 loopcntr=0;
0072 looplimit=1e6;
0073 
0074 while notdone && loopcntr<looplimit
0075     
0076     loopcntr=loopcntr+1;
0077     % get current point coords
0078     currentpt=crnrs(:,ctindx);
0079     xc=currentpt(1);
0080     yc=currentpt(2);
0081     
0082     % get surrounding chessboard corner properties (sorted by distance)
0083     [surcrnrs,surindx]=findnearest(currentpt,crnrs,8);
0084     vecs(1,:)=surcrnrs(1,:)-xc;
0085     vecs(2,:)=surcrnrs(2,:)-yc;
0086     angles=cart2pol(vecs(1,:),vecs(2,:));
0087     angles(angles<=0)=angles(angles<=0)+2*pi();
0088     
0089     % setup the segment vectors to the corners in order to check for
0090     % validity of segment between points by summing ix and iy along segment
0091     
0092     vecsnrm=zeros(size(vecs)); % normal vectors
0093     vecslen=zeros(1,size(vecs,2)); % vector lengths
0094     for veccnr=1:size(vecs,2)
0095         vecslen(veccnr)=norm(vecs(:,veccnr));
0096         vecsnrm(:,veccnr)=vecs(:,veccnr)/vecslen(veccnr);
0097     end
0098     
0099     ixvalue=zeros(size(vecslen)); % ix values
0100     iyvalue=ixvalue; % iy values
0101     segedgevalue=iyvalue;
0102     
0103     for crnrcnr=1:size(vecs,2)
0104         for evcnr=1:round(vecslen(crnrcnr))
0105             xev=round(xc+vecsnrm(1,crnrcnr)*evcnr);
0106             yev=round(yc+vecsnrm(2,crnrcnr)*evcnr);
0107             ixvalue(crnrcnr)=ixvalue(crnrcnr)+ix(xev,yev);
0108             iyvalue(crnrcnr)=iyvalue(crnrcnr)+iy(xev,yev);
0109         end
0110         segedgevalue(crnrcnr)=norm([ixvalue(crnrcnr),iyvalue(crnrcnr)]/evcnr);
0111     end
0112     segedgemean=mean(segedgevalue);
0113     
0114     
0115     % get surrounding Harris point properties (sorted by distance)
0116     surpixs=findnearest(currentpt,pixs,8);
0117     vecspixs(1,:)=surpixs(1,:)-currentpt(1);
0118     vecspixs(2,:)=surpixs(2,:)-currentpt(2);
0119     anglespixs=cart2pol(vecspixs(1,:),vecspixs(2,:));
0120     anglespixs(anglespixs<=0)=anglespixs(anglespixs<=0)+2*pi();
0121     
0122     theta=pi()/90:pi()/90:2*pi();
0123 
0124     
0125     locs=peaklocs(:,ctindx);
0126     
0127     locs=locs';
0128     if length(locs)~=4
0129         error('There should be 4 and only 4 peaks');
0130     end
0131     lineangles=theta(locs);
0132 
0133     % get cross corners
0134 
0135     % reset crosspixs
0136     crosspixs=zeros(1,4);
0137     
0138     for pk=1:4
0139         for crnr=1:length(surindx)
0140             % check for angle proximity and segment edge projection
0141             if angprox(angles(crnr),lineangles(pk),angth) && segedgevalue(crnr)>segedgemean
0142                 for pix=1:size(surpixs,2)
0143                     % check if a Harris corner lies in between
0144                     if angprox(anglespixs(pix),lineangles(pk),angth)
0145                         if isequal(surpixs(:,pix),surcrnrs(:,crnr))
0146                             % store
0147                             crosspixs(pk)=surindx(crnr);
0148                             break;
0149                         else
0150                             break;
0151                         end
0152                     end
0153                 end
0154             end
0155         end
0156     end
0157 
0158     
0159     %Adjust cross
0160     for i=1:size(crosspixs,2)
0161         for u=xg-1:xg+1
0162             for v=yg-1:yg+1
0163                 if crosspixs(i)==crnrsgrid(u,v) && crnrsgriddone(u,v)>0 % check for valid corner, check for non zero value as well
0164                     valid=1; % a cross is valid if a match is found
0165                     k=posmat(u-xg+2,v-yg+2)-i;
0166                     crosspixs=crosspixs(mod((1:end)-k-1, end)+1 );
0167                 end
0168             end
0169         end
0170     end
0171     
0172 
0173     
0174     if valid % if connection found store
0175     
0176         %draw cross matrix
0177         cmat=zeros(3,3);
0178         cmat(2,2)=ctindx;
0179         cmat(2,3)=crosspixs(1);
0180         cmat(1,2)=crosspixs(2);
0181         cmat(2,1)=crosspixs(3);
0182         cmat(3,2)=crosspixs(4);
0183 
0184         %store changes
0185         crnrsgrid(xg-1:xg+1,yg-1:yg+1)=crnrsgrid(xg-1:xg+1,yg-1:yg+1)+cmat.*(~crnrsgrid(xg-1:xg+1,yg-1:yg+1));
0186         cmatdone=[0,1,0;1,2,1;0,1,0];
0187         cmatdone=cmatdone.*(cmatdone&cmat)-crnrsgriddone(xg-1:xg+1,yg-1:yg+1);
0188         cmatdone(cmatdone<0)=0;
0189         crnrsgriddone(xg-1:xg+1,yg-1:yg+1)=cmatdone+crnrsgriddone(xg-1:xg+1,yg-1:yg+1);
0190 %         if debug
0191 %             close all;
0192 %             figure; imshow(img);
0193 %             hold on; plot(crnrsgrid(:,:,2),crnrsgrid(:,:,1),'o');
0194 %         end
0195     % reset valid
0196     valid=0;
0197     else % ignore and reset
0198         crnrsgriddone(xg,yg)=0;
0199     end
0200     
0201     %get new point
0202     [xg,yg]=find(crnrsgriddone==1,1);
0203     
0204     % if no new point found end
0205     if isempty(xg)
0206         notdone=0;
0207     else
0208         ctindx=crnrsgrid(xg,yg);
0209     end
0210 end
0211 
0212 % store x and y coords into matrix
0213 crnrsgridout=zeros([size(crnrsgrid),2]);
0214 
0215 for x=1:size(crnrsgrid,1)
0216     for y=1:size(crnrsgrid,2)
0217         if crnrsgrid(x,y)
0218             crnrsgridout(x,y,:)=crnrs(:,crnrsgrid(x,y));
0219         end
0220     end
0221 end
0222 
0223 function prox=angprox(ang1,ang2,th)
0224 % ANGPROX checks if the two angles are within a certain threshold.
0225 %
0226 
0227 prox=abs(ang1-ang2)<th || abs(ang1-ang2)>(2*pi-th);