[1163] | 1 | %-------------------------------------------------------------------------- |
---|
| 2 | % 'civ': function adapted from PIVlab http://pivlab.blogspot.com/ |
---|
| 3 | % function [xtable ytable utable vtable typevector] = civ (image1,image2,ibx,iby step, subpixfinder, mask, roi) |
---|
| 4 | % |
---|
| 5 | % OUTPUT: |
---|
| 6 | % xtable: set of x coordinates |
---|
| 7 | % ytable: set of y coordinates |
---|
| 8 | % utable: set of u displacements (along x) |
---|
| 9 | % vtable: set of v displacements (along y) |
---|
| 10 | % ctable: max image correlation for each vector |
---|
| 11 | % typevector: set of flags, =1 for good, =0 for NaN vectors |
---|
| 12 | % |
---|
| 13 | %INPUT: |
---|
| 14 | % par_civ: structure of input parameters, with fields: |
---|
| 15 | % .ImageA: first image for correlation (matrix) |
---|
| 16 | % .ImageB: second image for correlation(matrix) |
---|
| 17 | % .CorrBoxSize: 1,2 vector giving the size of the correlation box in x and y |
---|
| 18 | % .SearchBoxSize: 1,2 vector giving the size of the search box in x and y |
---|
| 19 | % .SearchBoxShift: 1,2 vector or 2 column matrix (for civ2) giving the shift of the search box in x and y |
---|
| 20 | % .CorrSmooth: =1 or 2 determines the choice of the sub-pixel determination of the correlation max |
---|
| 21 | % .ImageWidth: nb of pixels of the image in x |
---|
| 22 | % .Dx, Dy: mesh for the PIV calculation |
---|
| 23 | % .Grid: grid giving the PIV calculation points (alternative to .Dx .Dy): centres of the correlation boxes in Image A |
---|
| 24 | % .Mask: name of a mask file or mask image matrix itself |
---|
| 25 | % .MinIma: thresholds for image luminosity |
---|
| 26 | % .MaxIma |
---|
| 27 | % .CheckDeformation=1 for subpixel interpolation and image deformation (linear transform) |
---|
| 28 | % .DUDX: matrix of deformation obtained from patch at each grid point |
---|
| 29 | % .DUDY |
---|
| 30 | % .DVDX: |
---|
| 31 | % .DVDY |
---|
| 32 | |
---|
| 33 | function [xtable,ytable,utable,vtable,ctable,FF,result_conv,errormsg] = civ (par_civ) |
---|
| 34 | |
---|
| 35 | %% check input images |
---|
| 36 | par_civ.ImageA=sum(double(par_civ.ImageA),3);%sum over rgb component for color images |
---|
| 37 | par_civ.ImageB=sum(double(par_civ.ImageB),3); |
---|
| 38 | [npy_ima,npx_ima]=size(par_civ.ImageA); |
---|
| 39 | if ~isequal(size(par_civ.ImageB),[npy_ima npx_ima]) |
---|
| 40 | errormsg='image pair with unequal size'; |
---|
| 41 | return |
---|
| 42 | end |
---|
| 43 | |
---|
| 44 | %% prepare measurement grid if not given as input |
---|
| 45 | if ~isfield(par_civ,'Grid')% grid points defining central positions of the sub-images in image A |
---|
| 46 | nbinterv_x=floor((npx_ima-1)/par_civ.Dx); |
---|
| 47 | gridlength_x=nbinterv_x*par_civ.Dx; |
---|
| 48 | minix=ceil((npx_ima-gridlength_x)/2); |
---|
| 49 | nbinterv_y=floor((npy_ima-1)/par_civ.Dy); |
---|
| 50 | gridlength_y=nbinterv_y*par_civ.Dy; |
---|
| 51 | miniy=ceil((npy_ima-gridlength_y)/2); |
---|
| 52 | [GridX,GridY]=meshgrid(minix:par_civ.Dx:npx_ima-1,miniy:par_civ.Dy:npy_ima-1); |
---|
| 53 | par_civ.Grid(:,1)=reshape(GridX,[],1); |
---|
| 54 | par_civ.Grid(:,2)=reshape(GridY,[],1);% increases with array index |
---|
| 55 | end |
---|
| 56 | nbvec=size(par_civ.Grid,1); |
---|
| 57 | |
---|
| 58 | |
---|
| 59 | %% prepare correlation and search boxes |
---|
| 60 | CorrBoxSizeX=par_civ.CorrBoxSize(:,1); |
---|
| 61 | CorrBoxSizeY=par_civ.CorrBoxSize(:,2); |
---|
| 62 | if size(par_civ.CorrBoxSize,1)==1 |
---|
| 63 | CorrBoxSizeX=par_civ.CorrBoxSize(1)*ones(nbvec,1); |
---|
| 64 | CorrBoxSizeY=par_civ.CorrBoxSize(2)*ones(nbvec,1); |
---|
| 65 | end |
---|
| 66 | |
---|
| 67 | shiftx=par_civ.SearchBoxShift(:,1);%use the input shift estimate, rounded to the next integer value |
---|
| 68 | shifty=-par_civ.SearchBoxShift(:,2);% sign minus because image j index increases when y decreases |
---|
| 69 | if numel(shiftx)==1% case of a unique shift for the whole field( civ1) |
---|
| 70 | shiftx=shiftx*ones(nbvec,1); |
---|
| 71 | shifty=shifty*ones(nbvec,1); |
---|
| 72 | end |
---|
| 73 | |
---|
| 74 | %% shift the grid by half the expected displacement to get the velocity closer to the initial grid |
---|
| 75 | par_civ.Grid(:,1)=par_civ.Grid(:,1)-shiftx/2; |
---|
| 76 | par_civ.Grid(:,2)=par_civ.Grid(:,2)+shifty/2; |
---|
| 77 | |
---|
| 78 | %% Array initialisation and default output if par_civ.CorrSmooth=0 (just the grid calculated, no civ computation) |
---|
| 79 | xtable=round(par_civ.Grid(:,1)+0.5)-0.5; |
---|
| 80 | ytable=round(npy_ima-par_civ.Grid(:,2)+0.5)-0.5;% y index corresponding to the position in image coordinates |
---|
| 81 | shiftx=round(shiftx); |
---|
| 82 | shifty=round(shifty); |
---|
| 83 | utable=shiftx;%zeros(nbvec,1); |
---|
| 84 | vtable=shifty;%zeros(nbvec,1); |
---|
| 85 | ctable=zeros(nbvec,1); |
---|
| 86 | FF=zeros(nbvec,1); |
---|
| 87 | result_conv=[]; |
---|
| 88 | errormsg=''; |
---|
| 89 | |
---|
| 90 | %% prepare mask |
---|
| 91 | if isfield(par_civ,'Mask') && ~isempty(par_civ.Mask) |
---|
| 92 | if strcmp(par_civ.Mask,'all') |
---|
| 93 | return % get the grid only, no civ calculation |
---|
| 94 | elseif ischar(par_civ.Mask) |
---|
| 95 | par_civ.Mask=imread(par_civ.Mask);% read the mask if not allready done |
---|
| 96 | end |
---|
| 97 | end |
---|
| 98 | check_MinIma=isfield(par_civ,'MinIma');% test for image luminosity threshold |
---|
| 99 | check_MaxIma=isfield(par_civ,'MaxIma') && ~isempty(par_civ.MaxIma); |
---|
| 100 | |
---|
| 101 | %% Apply mask |
---|
| 102 | % Convention for mask, IDEAS NOT IMPLEMENTED |
---|
| 103 | % mask >200 : velocity calculated |
---|
| 104 | % 200 >=mask>150;velocity not calculated, interpolation allowed (bad spots) |
---|
| 105 | % 150>=mask >100: velocity not calculated, nor interpolated |
---|
| 106 | % 100>=mask> 20: velocity not calculated, impermeable (no flux through mask boundaries) |
---|
| 107 | % 20>=mask: velocity=0 |
---|
| 108 | checkmask=0; |
---|
| 109 | MinA=min(min(par_civ.ImageA)); |
---|
| 110 | if isfield(par_civ,'Mask') && ~isempty(par_civ.Mask) |
---|
| 111 | checkmask=1; |
---|
| 112 | if ~isequal(size(par_civ.Mask),[npy_ima npx_ima]) |
---|
| 113 | errormsg='mask must be an image with the same size as the images'; |
---|
| 114 | return |
---|
| 115 | end |
---|
| 116 | check_undefined=(par_civ.Mask<200 & par_civ.Mask>=20 ); |
---|
| 117 | end |
---|
| 118 | |
---|
| 119 | %% compute image correlations: MAINLOOP on velocity vectors |
---|
| 120 | sum_square=1;% default |
---|
| 121 | mesh=1;% default |
---|
| 122 | CheckDeformation=isfield(par_civ,'CheckDeformation')&& par_civ.CheckDeformation==1; |
---|
| 123 | if CheckDeformation |
---|
| 124 | mesh=0.25;%mesh in pixels for subpixel image interpolation (x 4 in each direction) |
---|
| 125 | par_civ.CorrSmooth=2;% use SUBPIX2DGAUSS (take into account more points near the max) |
---|
| 126 | end |
---|
[1169] | 127 | |
---|
[1163] | 128 | if par_civ.CorrSmooth~=0 % par_civ.CorrSmooth=0 implies no civ computation (just input image and grid points given) |
---|
| 129 | for ivec=1:nbvec |
---|
| 130 | iref=round(par_civ.Grid(ivec,1)+0.5);% xindex on the image A for the middle of the correlation box |
---|
| 131 | jref=round(npy_ima-par_civ.Grid(ivec,2)+0.5);% j index for the middle of the correlation box in the image A |
---|
| 132 | FF(ivec)=0; |
---|
| 133 | ibx2=floor(CorrBoxSizeX(ivec)/2); |
---|
[1169] | 134 | iby2=floor(CorrBoxSizeY(ivec)/2); |
---|
| 135 | isx2=ibx2+ceil(par_civ.SearchRange(1)); |
---|
| 136 | isy2=iby2+ceil(par_civ.SearchRange(2)); |
---|
[1163] | 137 | subrange1_x=iref-ibx2:iref+ibx2;% x indices defining the first subimage |
---|
| 138 | subrange1_y=jref-iby2:jref+iby2;% y indices defining the first subimage |
---|
| 139 | subrange2_x=iref+shiftx(ivec)-isx2:iref+shiftx(ivec)+isx2;%x indices defining the second subimage |
---|
| 140 | subrange2_y=jref+shifty(ivec)-isy2:jref+shifty(ivec)+isy2;%y indices defining the second subimage |
---|
| 141 | image1_crop=MinA*ones(numel(subrange1_y),numel(subrange1_x));% default value=min of image A |
---|
| 142 | image2_crop=MinA*ones(numel(subrange2_y),numel(subrange2_x));% default value=min of image A |
---|
| 143 | check1_x=subrange1_x>=1 & subrange1_x<=npx_ima;% check which points in the subimage 1 are contained in the initial image 1 |
---|
| 144 | check1_y=subrange1_y>=1 & subrange1_y<=npy_ima; |
---|
| 145 | check2_x=subrange2_x>=1 & subrange2_x<=npx_ima;% check which points in the subimage 2 are contained in the initial image 2 |
---|
| 146 | check2_y=subrange2_y>=1 & subrange2_y<=npy_ima; |
---|
| 147 | image1_crop(check1_y,check1_x)=par_civ.ImageA(subrange1_y(check1_y),subrange1_x(check1_x));%extract a subimage (correlation box) from image A |
---|
| 148 | image2_crop(check2_y,check2_x)=par_civ.ImageB(subrange2_y(check2_y),subrange2_x(check2_x));%extract a larger subimage (search box) from image B |
---|
| 149 | if checkmask |
---|
| 150 | mask1_crop=ones(numel(subrange1_y),numel(subrange1_x));% default value=1 for mask |
---|
| 151 | mask2_crop=ones(numel(subrange2_y),numel(subrange2_x));% default value=1 for mask |
---|
| 152 | mask1_crop(check1_y,check1_x)=check_undefined(subrange1_y(check1_y),subrange1_x(check1_x));%extract a mask subimage (correlation box) from image A |
---|
| 153 | mask2_crop(check2_y,check2_x)=check_undefined(subrange2_y(check2_y),subrange2_x(check2_x));%extract a mask subimage (search box) from image B |
---|
| 154 | sizemask=sum(sum(mask1_crop))/(numel(subrange1_y)*numel(subrange1_x));%size of the masked part relative to the correlation sub-image |
---|
| 155 | if sizemask > 1/2% eliminate point if more than half of the correlation box is masked |
---|
| 156 | FF(ivec)=1; % |
---|
| 157 | utable(ivec)=NaN; |
---|
| 158 | vtable(ivec)=NaN; |
---|
| 159 | else |
---|
| 160 | image1_crop=image1_crop.*~mask1_crop;% put to zero the masked pixels (mask1_crop='true'=1) |
---|
| 161 | image2_crop=image2_crop.*~mask2_crop; |
---|
| 162 | image1_mean=mean(mean(image1_crop))/(1-sizemask); |
---|
| 163 | image2_mean=mean(mean(image2_crop))/(1-sizemask); |
---|
| 164 | end |
---|
| 165 | else |
---|
| 166 | image1_mean=mean(mean(image1_crop)); |
---|
| 167 | image2_mean=mean(mean(image2_crop)); |
---|
| 168 | end |
---|
| 169 | %threshold on image minimum |
---|
| 170 | if FF(ivec)==0 |
---|
| 171 | if check_MinIma && (image1_mean < par_civ.MinIma || image2_mean < par_civ.MinIma) |
---|
| 172 | FF(ivec)=1; |
---|
| 173 | %threshold on image maximum |
---|
| 174 | elseif check_MaxIma && (image1_mean > par_civ.MaxIma || image2_mean > par_civ.MaxIma) |
---|
| 175 | FF(ivec)=1; |
---|
| 176 | end |
---|
| 177 | if FF(ivec)==1 |
---|
| 178 | utable(ivec)=NaN; |
---|
| 179 | vtable(ivec)=NaN; |
---|
| 180 | else |
---|
| 181 | %mask |
---|
| 182 | if checkmask |
---|
| 183 | image1_crop=(image1_crop-image1_mean).*~mask1_crop;%substract the mean, put to zero the masked parts |
---|
| 184 | image2_crop=(image2_crop-image2_mean).*~mask2_crop; |
---|
| 185 | else |
---|
| 186 | image1_crop=(image1_crop-image1_mean); |
---|
| 187 | image2_crop=(image2_crop-image2_mean); |
---|
| 188 | end |
---|
| 189 | %deformation |
---|
| 190 | if CheckDeformation |
---|
| 191 | xi=(1:mesh:size(image1_crop,2)); |
---|
| 192 | yi=(1:mesh:size(image1_crop,1))'; |
---|
| 193 | [XI,YI]=meshgrid(xi-ceil(size(image1_crop,2)/2),yi-ceil(size(image1_crop,1)/2)); |
---|
| 194 | XIant=XI-par_civ.DUDX(ivec)*XI+par_civ.DUDY(ivec)*YI+ceil(size(image1_crop,2)/2); |
---|
| 195 | YIant=YI+par_civ.DVDX(ivec)*XI-par_civ.DVDY(ivec)*YI+ceil(size(image1_crop,1)/2); |
---|
| 196 | image1_crop=interp2(image1_crop,XIant,YIant); |
---|
| 197 | image1_crop(isnan(image1_crop))=0; |
---|
| 198 | xi=(1:mesh:size(image2_crop,2)); |
---|
| 199 | yi=(1:mesh:size(image2_crop,1))'; |
---|
| 200 | image2_crop=interp2(image2_crop,xi,yi,'*spline'); |
---|
| 201 | image2_crop(isnan(image2_crop))=0; |
---|
| 202 | end |
---|
| 203 | sum_square=sum(sum(image1_crop.*image1_crop)); |
---|
| 204 | %reference: Oliver Pust, PIV: Direct Cross-Correlation |
---|
| 205 | %%%%%% correlation calculation |
---|
| 206 | result_conv= conv2(image2_crop,flip(flip(image1_crop,2),1),'valid'); |
---|
| 207 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
| 208 | corrmax= max(max(result_conv)); |
---|
[1169] | 209 | |
---|
[1163] | 210 | %result_conv=(result_conv/corrmax); %normalize, peak=always 255 |
---|
| 211 | %Find the correlation max, at 255 |
---|
| 212 | [y,x] = find(result_conv==corrmax,1); |
---|
| 213 | subimage2_crop=image2_crop(y:y+2*iby2/mesh,x:x+2*ibx2/mesh);%subimage of image 2 corresponding to the optimum displacement of first image |
---|
| 214 | sum_square=sum_square*sum(sum(subimage2_crop.*subimage2_crop));% product of variances of image 1 and 2 |
---|
| 215 | sum_square=sqrt(sum_square);% srt of the variance product to normalise correlation |
---|
| 216 | if ~isempty(y) && ~isempty(x) |
---|
| 217 | try |
---|
| 218 | if par_civ.CorrSmooth==1 |
---|
| 219 | [vector,FF(ivec)] = SUBPIXGAUSS (result_conv,x,y); |
---|
| 220 | elseif par_civ.CorrSmooth==2 |
---|
| 221 | [vector,FF(ivec)] = SUBPIX2DGAUSS (result_conv,x,y); |
---|
| 222 | else |
---|
| 223 | [vector,FF(ivec)] = quadr_fit(result_conv,x,y); |
---|
| 224 | end |
---|
| 225 | utable(ivec)=vector(1)*mesh+shiftx(ivec); |
---|
| 226 | vtable(ivec)=-(vector(2)*mesh+shifty(ivec)); |
---|
| 227 | xtable(ivec)=iref+utable(ivec)/2-0.5;% convec flow (velocity taken at the point middle from imgae 1 and 2) |
---|
| 228 | ytable(ivec)=jref+vtable(ivec)/2-0.5;% and position of pixel 1=0.5 (convention for image coordinates=0 at the edge) |
---|
| 229 | iref=round(xtable(ivec)+0.5);% nearest image index for the middle of the vector |
---|
| 230 | jref=round(ytable(ivec)+0.5); |
---|
| 231 | % eliminate vectors located in the mask |
---|
| 232 | if checkmask && (iref<1 || jref<1 ||iref>npx_ima || jref>npy_ima ||( par_civ.Mask(jref,iref)<200 && par_civ.Mask(jref,iref)>=100)) |
---|
| 233 | utable(ivec)=0; |
---|
| 234 | vtable(ivec)=0; |
---|
| 235 | FF(ivec)=1; |
---|
| 236 | end |
---|
| 237 | ctable(ivec)=corrmax/sum_square;% correlation value |
---|
| 238 | catch ME |
---|
| 239 | FF(ivec)=1; |
---|
| 240 | disp(ME.message) |
---|
| 241 | end |
---|
| 242 | else |
---|
| 243 | FF(ivec)=1; |
---|
| 244 | end |
---|
| 245 | end |
---|
| 246 | end |
---|
| 247 | end |
---|
| 248 | end |
---|
| 249 | ytable=npy_ima-ytable+1;%reverse from j index to image coordinate y |
---|
[1168] | 250 | result_conv=result_conv/sum_square;% keep the last correlation matrix for output |
---|
[1163] | 251 | |
---|
| 252 | |
---|
| 253 | |
---|
| 254 | %------------------------------------------------------------------------ |
---|
| 255 | % --- Find the maximum of the correlation function with subpixel resolution |
---|
| 256 | % make a fit with a gaussian curve from the three correlation values across the max, along each direction. |
---|
| 257 | % OUPUT: |
---|
| 258 | % vector = optimum displacement vector with subpixel correction |
---|
| 259 | % FF =flag: =0 OK |
---|
| 260 | % =1 , max too close to the edge of the search box (1 pixel margin) |
---|
| 261 | % INPUT: |
---|
| 262 | % result_conv: 2D correlation fct |
---|
| 263 | % x,y: position of the maximum correlation at integer values |
---|
| 264 | |
---|
| 265 | function [vector,FF] = SUBPIXGAUSS (result_conv,x,y) |
---|
| 266 | %------------------------------------------------------------------------ |
---|
| 267 | % vector=[0 0]; %default |
---|
| 268 | FF=true;% error flag for vector truncated by the limited search box |
---|
| 269 | [npy,npx]=size(result_conv); |
---|
| 270 | |
---|
| 271 | peaky = y; peakx=x; |
---|
| 272 | if y < npy && y > 1 && x < npx-1 && x > 1 |
---|
[1169] | 273 | FF=false; % no error by the limited search box |
---|
[1163] | 274 | max_conv=result_conv(y,x);% max correlation |
---|
| 275 | %peak2noise= max(4,max_conv/std(reshape(result_conv,1,[])));% ratio of max conv to standard deviation of correlations (estiamtion of noise level), set to value 4 if it is too low |
---|
| 276 | peak2noise=100;% TODO: make this threshold more precise, depending on the image noise |
---|
[1169] | 277 | result_conv=result_conv*peak2noise/max_conv;% renormalise the correlation with respect to the noise |
---|
[1163] | 278 | result_conv(result_conv<1)=1; %set to 1 correlation values smaller than 1 (=0 by discretisation, to avoid divergence in the log) |
---|
| 279 | |
---|
| 280 | f0 = log(result_conv(y,x)); |
---|
| 281 | f1 = log(result_conv(y-1,x)); |
---|
| 282 | f2 = log(result_conv(y+1,x)); |
---|
| 283 | peaky = peaky+ (f1-f2)/(2*f1-4*f0+2*f2); |
---|
| 284 | f1 = log(result_conv(y,x-1)); |
---|
| 285 | f2 = log(result_conv(y,x+1)); |
---|
| 286 | peakx = peakx+ (f1-f2)/(2*f1-4*f0+2*f2); |
---|
| 287 | end |
---|
| 288 | |
---|
| 289 | vector=[peakx-floor(npx/2)-1 peaky-floor(npy/2)-1]; |
---|
| 290 | |
---|
| 291 | %------------------------------------------------------------------------ |
---|
| 292 | % --- Find the maximum of the correlation function after interpolation |
---|
| 293 | function [vector,FF] = SUBPIX2DGAUSS (result_conv,x,y) |
---|
| 294 | %------------------------------------------------------------------------ |
---|
| 295 | % vector=[0 0]; %default |
---|
| 296 | FF=true; |
---|
| 297 | peaky=y; |
---|
| 298 | peakx=x; |
---|
| 299 | [npy,npx]=size(result_conv); |
---|
| 300 | if (x < npx) && (y < npy) && (x > 1) && (y > 1) |
---|
| 301 | FF=false; |
---|
| 302 | max_conv=result_conv(y,x);% max correlation |
---|
| 303 | peak2noise= max(4,max_conv/std(reshape(result_conv,1,[])));% ratio of max conv to standard deviation of correlations (estiamtion of noise level), set to value 4 if it is too low |
---|
| 304 | result_conv=result_conv*peak2noise/max_conv;% renormalise the correlation with respect to the noise |
---|
| 305 | result_conv(result_conv<1)=1; %set to 1 correlation values smaller than 1 (=0 by discretisation, to avoid divergence in the log) |
---|
| 306 | for i=-1:1 |
---|
| 307 | for j=-1:1 |
---|
| 308 | %following 15 lines based on H. Nobach and M. Honkanen (2005) |
---|
| 309 | % Two-dimensional Gaussian regression for sub-pixel displacement |
---|
| 310 | % estimation in particle image velocimetry or particle position |
---|
| 311 | % estimation in particle tracking velocimetry |
---|
| 312 | % Experiments in Fluids (2005) 38: 511-515 |
---|
| 313 | c10(j+2,i+2)=i*log(result_conv(y+j, x+i)); |
---|
| 314 | c01(j+2,i+2)=j*log(result_conv(y+j, x+i)); |
---|
| 315 | c11(j+2,i+2)=i*j*log(result_conv(y+j, x+i)); |
---|
| 316 | c20(j+2,i+2)=(3*i^2-2)*log(result_conv(y+j, x+i)); |
---|
| 317 | c02(j+2,i+2)=(3*j^2-2)*log(result_conv(y+j, x+i)); |
---|
| 318 | end |
---|
| 319 | end |
---|
| 320 | c10=(1/6)*sum(sum(c10)); |
---|
| 321 | c01=(1/6)*sum(sum(c01)); |
---|
| 322 | c11=(1/4)*sum(sum(c11)); |
---|
| 323 | c20=(1/6)*sum(sum(c20)); |
---|
| 324 | c02=(1/6)*sum(sum(c02)); |
---|
| 325 | deltax=(c11*c01-2*c10*c02)/(4*c20*c02-c11*c11); |
---|
| 326 | deltay=(c11*c10-2*c01*c20)/(4*c20*c02-c11*c11); |
---|
| 327 | if abs(deltax)<1 |
---|
| 328 | peakx=x+deltax; |
---|
| 329 | end |
---|
| 330 | if abs(deltay)<1 |
---|
| 331 | peaky=y+deltay; |
---|
| 332 | end |
---|
| 333 | end |
---|
| 334 | vector=[peakx-floor(npx/2)-1 peaky-floor(npy/2)-1]; |
---|
| 335 | |
---|
| 336 | %------------------------------------------------------------------------ |
---|
| 337 | % --- Find the maximum of the correlation function after quadratic interpolation |
---|
| 338 | function [vector,F] = quadr_fit(result_conv,x,y) |
---|
| 339 | [npy,npx]=size(result_conv); |
---|
| 340 | if x<4 || y<4 || npx-x<4 ||npy-y <4 |
---|
| 341 | F=1; |
---|
| 342 | vector=[x y]; |
---|
| 343 | else |
---|
| 344 | F=0; |
---|
| 345 | x_ind=x-4:x+4; |
---|
| 346 | y_ind=y-4:y+4; |
---|
| 347 | x_vec=0.25*(x_ind-x); |
---|
| 348 | y_vec=0.25*(y_ind-y); |
---|
| 349 | [X,Y]=meshgrid(x_vec,y_vec); |
---|
| 350 | coord=[reshape(X,[],1) reshape(Y,[],1)]; |
---|
| 351 | result_conv=reshape(result_conv(y_ind,x_ind),[],1); |
---|
| 352 | |
---|
| 353 | |
---|
| 354 | % n=numel(X); |
---|
| 355 | % x=[X Y]; |
---|
| 356 | % X=X-0.5; |
---|
| 357 | % Y=Y+0.5; |
---|
| 358 | % y = (X.*X+2*Y.*Y+X.*Y+6) + 0.1*rand(n,1); |
---|
| 359 | p = polyfitn(coord,result_conv,2); |
---|
| 360 | A(1,1)=2*p.Coefficients(1); |
---|
| 361 | A(1,2)=p.Coefficients(2); |
---|
| 362 | A(2,1)=p.Coefficients(2); |
---|
| 363 | A(2,2)=2*p.Coefficients(4); |
---|
| 364 | vector=[x y]'-A\[p.Coefficients(3) p.Coefficients(5)]'; |
---|
| 365 | vector=vector'-[floor(npx/2) floor(npy/2)]-1 ; |
---|
| 366 | % zg = polyvaln(p,coord); |
---|
| 367 | % figure |
---|
| 368 | % surf(x_vec,y_vec,reshape(zg,9,9)) |
---|
| 369 | % hold on |
---|
| 370 | % plot3(X,Y,reshape(result_conv,9,9),'o') |
---|
| 371 | % hold off |
---|
| 372 | end |
---|
| 373 | |
---|
| 374 | |
---|