[37] | 1 | function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM)
|
---|
| 2 | % GETPOINT ... extracts position of an LED from an image
|
---|
| 3 | % only one or none LED is expected
|
---|
| 4 | %
|
---|
| 5 | % function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM)
|
---|
| 6 | %
|
---|
| 7 | % imname ... name of the image (full path should be specified)
|
---|
| 8 | % showfig .. show figures (1->on/0->off)
|
---|
| 9 | % imconfig . config.imgs, see CONFIGDATA
|
---|
| 10 | % avIM ... average image of the camera, see IM2POINTS
|
---|
| 11 | % stdIM ... image of standard deviations, see IM2POINTS
|
---|
| 12 | %
|
---|
| 13 | % pos ...... 2x1 vector containing (x,y)'-coordinates of the point
|
---|
| 14 | % if error then 0 is returned
|
---|
| 15 | % err ...... boolean, indicates an error (ambiguous blobs, point too
|
---|
| 16 | % eccentric, etc.)
|
---|
| 17 |
|
---|
| 18 | % $Author: svoboda $
|
---|
| 19 | % $Revision: 2.6 $
|
---|
| 20 | % $Id: getpoint.m,v 2.6 2005/05/20 12:05:40 svoboda Exp $
|
---|
| 21 | % $State: Exp $
|
---|
| 22 |
|
---|
| 23 |
|
---|
| 24 | err = 0;
|
---|
| 25 |
|
---|
| 26 | foo = ver('images');
|
---|
| 27 | IPT_VER = str2num(foo.Version(1));
|
---|
| 28 | if IPT_VER >= 5
|
---|
| 29 | region_properties_function = 'regionprops';
|
---|
| 30 | else
|
---|
| 31 | region_properties_function = 'imfeature';
|
---|
| 32 | end
|
---|
| 33 |
|
---|
| 34 | SHOW_WARN = 0; % show warnings?
|
---|
| 35 | BLK_PROC = 0; % blkproc may be faster for bigger LEDs, rather not
|
---|
| 36 | SUB_PIX = 1/imconfig.subpix; % required sub-pixel precision 3 -> 1/3 pixel
|
---|
| 37 |
|
---|
| 38 | TEST_AREA = 0; % check the size of the thresholded blob? (mostly not necessary)
|
---|
| 39 | TEST_ECC = 0; % perform the eccentricity check? (mostly not necessary)
|
---|
| 40 | ECC_THR = 0.99; % eccentricity threshold (for validity check)
|
---|
| 41 | % this threshold is not usable in the current implementation
|
---|
| 42 |
|
---|
| 43 | LEDSIZE = imconfig.LEDsize; % avg diameter of a LED in pixels
|
---|
| 44 |
|
---|
| 45 | im.name = imname;
|
---|
| 46 |
|
---|
| 47 | % threshold for the LED detection, set to a default value if not specified
|
---|
| 48 | try imconfig.LEDthr; catch imconfig.LEDthr = 70; end
|
---|
| 49 |
|
---|
| 50 | %%%
|
---|
| 51 | % set figure handles
|
---|
| 52 | fig.im4thr = 1; % image used for thresholding
|
---|
| 53 | fig.imOrig = 2; % original image
|
---|
| 54 | fig.blob = 3; % ouput of bwlabel
|
---|
| 55 | fig.subI = 4; % subimage (local neighbourhood of est. LED pos.)
|
---|
| 56 |
|
---|
| 57 | im.info = imfinfo(im.name);
|
---|
| 58 | im.orig = imread(im.name);
|
---|
| 59 |
|
---|
| 60 | if strcmp(im.info.ColorType,'grayscale');
|
---|
| 61 | im.I = im.orig;
|
---|
| 62 | else
|
---|
| 63 | [im.r,im.c] = size(im.orig(:,:,1));
|
---|
| 64 | im.R = im.orig(:,:,1); % Red component
|
---|
| 65 | im.G = im.orig(:,:,2); % Green component
|
---|
| 66 | end
|
---|
| 67 |
|
---|
| 68 | % find possible location of the point by thresholding
|
---|
| 69 | if strcmp(imconfig.LEDcolor,'green')
|
---|
| 70 | im.thr = uint8(abs(double(im.G(:,:))-double(avIM(:,:,2)))); % on which image the thresholding will be done
|
---|
| 71 | im.std = stdIM(:,:,2); % use green component
|
---|
| 72 | im.fit = im.G; % image for fitting of the PSF
|
---|
| 73 | elseif strcmp(imconfig.LEDcolor,'red')
|
---|
| 74 | im.thr = uint8(abs(double(im.R(:,:))-double(avIM(:,:,1)))); % on which image the thresholding will be done
|
---|
| 75 | im.std = stdIM(:,:,1); % use red component
|
---|
| 76 | im.fit = im.R; % image for fitting of the PSF
|
---|
| 77 | elseif strcmp(im.info.ColorType,'grayscale')
|
---|
| 78 | im.thr = uint8(abs(double(im.I)-double(avIM)));
|
---|
| 79 | im.std = stdIM;
|
---|
| 80 | im.fit = im.I;
|
---|
| 81 | elseif ~strcmp(im.info.ColorType,'grayscale') & strcmp(imconfig.LEDcolor,'intensity')
|
---|
| 82 | try im.I; catch im.I = uint8(round(mean(double(im.orig),3))); end
|
---|
| 83 | im.thr = uint8(round(abs(double(im.I)-double(mean(avIM,3)))));
|
---|
| 84 | try im.std = stdIM; catch im.std = uint8(round(mean(double(stdIM),3))); end
|
---|
| 85 | try im.fit = im.I; catch im.fit = im.I; end
|
---|
| 86 | else
|
---|
| 87 | error('getpoint: no valid color of the laser pointer, see CONFIGDATA');
|
---|
| 88 | end
|
---|
| 89 |
|
---|
| 90 | % show figures if required, may be useful when debugging
|
---|
| 91 | if showfig
|
---|
| 92 | figure(fig.imOrig),
|
---|
| 93 | clf
|
---|
| 94 | imshow(im.orig)
|
---|
| 95 | title(strcat(im.name, ' original'))
|
---|
| 96 | hold on
|
---|
| 97 | figure(fig.im4thr),
|
---|
| 98 | clf
|
---|
| 99 | imshow(im.thr);
|
---|
| 100 | title(strcat(im.name, ' image to be thresholded'))
|
---|
| 101 | drawnow
|
---|
| 102 | hold on
|
---|
| 103 | end
|
---|
| 104 |
|
---|
| 105 | [maxint,idx] = max(im.thr(:));
|
---|
| 106 | leds.thr = double(maxint)*0.99; %4/5;
|
---|
| 107 |
|
---|
| 108 | if ( (im.thr(idx) < 5*double(im.std(idx))) | ( im.thr(idx)< imconfig.LEDthr ))
|
---|
| 109 | if SHOW_WARN
|
---|
| 110 | warning('Perhaps no LED in the image, detected maximum of image difference is too low')
|
---|
| 111 | disp(sprintf('Max: %d, Thr4Max1: %d, Thr4Max2: %d',double(im.thr(idx)), 10*double(im.std(idx)), imconfig.LEDthr))
|
---|
| 112 | end
|
---|
| 113 | err=1;
|
---|
| 114 | pos=0;
|
---|
| 115 | return;
|
---|
| 116 | else
|
---|
| 117 | [L,num]=bwlabel(im.thr>leds.thr);
|
---|
| 118 | end
|
---|
| 119 |
|
---|
| 120 | if num>1 % sum(diff(any(im.thr>leds.thr,2))>0)>1
|
---|
| 121 | if SHOW_WARN
|
---|
| 122 | warning('More than one blob detected')
|
---|
| 123 | figure(99), imshow(im.thr>leds.thr), title('thresholded image');
|
---|
| 124 | disp('press a key to continue')
|
---|
| 125 | pause
|
---|
| 126 | end
|
---|
| 127 | err=1;
|
---|
| 128 | pos=0;
|
---|
| 129 | return;
|
---|
| 130 | else
|
---|
| 131 | im.stats = eval([region_properties_function,'(L,',char(39),'Centroid',char(39),')']);
|
---|
| 132 | rawpos = round([im.stats.Centroid(2),im.stats.Centroid(1)]);
|
---|
| 133 | % rawpos = zeros(1,2);
|
---|
| 134 | % [rawpos(1),rawpos(2)] = ind2sub(size(im.thr),idx);
|
---|
| 135 | end
|
---|
| 136 |
|
---|
| 137 |
|
---|
| 138 | leds.size = round(LEDSIZE/1.2);
|
---|
| 139 | % (2*leds.size+1)x(2*leds.size+1) is the area of interest around each detected LED
|
---|
| 140 | % check if the LED lies in the allowed position (not very close to the image border
|
---|
| 141 | % it is because of the implementation not because of principle
|
---|
| 142 | if rawpos(1)-leds.size < 1 | rawpos(1)+leds.size > size(im.thr,1) | ...
|
---|
| 143 | rawpos(2)-leds.size < 1 | rawpos(2)+leds.size > size(im.thr,2)
|
---|
| 144 | if SHOW_WARN
|
---|
| 145 | warning('LED position lies outside allowed boundary');
|
---|
| 146 | end
|
---|
| 147 | err = 1;
|
---|
| 148 | pos = 0;
|
---|
| 149 | return;
|
---|
| 150 | end
|
---|
| 151 |
|
---|
| 152 | leds.rows = (rawpos(1)-leds.size):(rawpos(1)+leds.size);
|
---|
| 153 | leds.cols = (rawpos(2)-leds.size):(rawpos(2)+leds.size);
|
---|
| 154 | % [L,num] = bwlabel(im.thr(leds.rows,leds.cols)>leds.thr);
|
---|
| 155 |
|
---|
| 156 | % perform checks of the thresholded blob if required
|
---|
| 157 | if TEST_ECC
|
---|
| 158 | im.stats = eval([region_properties_function,'(L,',char(39),'Eccentricity',char(39),')']);
|
---|
| 159 | if (im.stats.Eccentricity > ECC_THR)
|
---|
| 160 | if SHOW_WARN
|
---|
| 161 | warning(sprintf('eccentricity treshold %2.4f exceeded by %2.4f', ECC_THR, im.stats.Eccentricity));
|
---|
| 162 | end
|
---|
| 163 | err = 1; pos = 0; return;
|
---|
| 164 | end
|
---|
| 165 | end
|
---|
| 166 | if TEST_AREA
|
---|
| 167 | im.stats = eval([region_properties_function,'(L,',char(39),'Area',char(39),')']);
|
---|
| 168 | if im.stats.Area > size(leds.rows,2)*size(leds.cols,2)/1.5
|
---|
| 169 | if SHOW_WARN
|
---|
| 170 | warning('Detected LED to too big')
|
---|
| 171 | end
|
---|
| 172 | err=1;
|
---|
| 173 | pos=0;
|
---|
| 174 | return;
|
---|
| 175 | end
|
---|
| 176 | end
|
---|
| 177 |
|
---|
| 178 |
|
---|
| 179 | % Crop the sub-image of interest around found LED
|
---|
| 180 | IM = im.fit(leds.rows,leds.cols);
|
---|
| 181 |
|
---|
| 182 | % visual check of LED position
|
---|
| 183 | if showfig
|
---|
| 184 | figure(fig.subI), clf
|
---|
| 185 | imshow(im.thr>leds.thr)
|
---|
| 186 | hold on
|
---|
| 187 | plot(rawpos(2),rawpos(1),'g+','EraseMode','Back');
|
---|
| 188 | drawnow
|
---|
| 189 | % pause
|
---|
| 190 | end
|
---|
| 191 |
|
---|
| 192 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 193 | % interpolate local neighborhood and find the maxima
|
---|
| 194 | % by correlation with the gaussian (see declaration of Gsize)
|
---|
| 195 | leds.scale = SUB_PIX; % the area of interest will be inreased by leds.scale using bilinear interpolation
|
---|
| 196 | % leds.size should be comparable to the leds.scale. If leds.size is assumed too small
|
---|
| 197 | % then the correlation based detection does not work
|
---|
| 198 | % properly
|
---|
| 199 | finepos = getfinepos(IM,rawpos,leds,LEDSIZE,BLK_PROC,showfig,SHOW_WARN);
|
---|
| 200 |
|
---|
| 201 |
|
---|
| 202 |
|
---|
| 203 | %%% plot information about detected LED
|
---|
| 204 | if showfig
|
---|
| 205 | figure(fig.im4thr)
|
---|
| 206 | plot(finepos(2),finepos(1),'r+','EraseMode','Back');
|
---|
| 207 | figure(fig.imOrig)
|
---|
| 208 | plot(finepos(2),finepos(1),'r+','EraseMode','Back','MarkerSize',25,'LineWidth',3);
|
---|
| 209 | drawnow
|
---|
| 210 | end
|
---|
| 211 |
|
---|
| 212 | if showfig
|
---|
| 213 | pause
|
---|
| 214 | end
|
---|
| 215 |
|
---|
| 216 | pos = [finepos(2); finepos(1)];
|
---|
| 217 |
|
---|
| 218 | return % end of the getpoint function
|
---|
| 219 |
|
---|
| 220 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 221 | %%%
|
---|
| 222 | % internal function for finding the fine position
|
---|
| 223 | function finepos = getfinepos(IM,rawpos,leds,LEDSIZE,BLK_PROC,showfig,SHOW_WARN)
|
---|
| 224 |
|
---|
| 225 | Gsize = round(leds.scale*LEDSIZE/2); % (2*Gsize+1)x(2*Gsize+1) is the dimension of the Gaussian mask that models PSF
|
---|
| 226 |
|
---|
| 227 | % t1 = cputime;
|
---|
| 228 | IM2 = imresize(IM,leds.scale,'bicubic'); % zoom in
|
---|
| 229 | % disp(sprintf('elapsed for resize: %f',cputime-t1'))
|
---|
| 230 |
|
---|
| 231 | % Correlation mask that approximates point spread function (PSF) of a LED
|
---|
| 232 | Gaussian = fspecial('Gaussian',2*Gsize+1,leds.scale*LEDSIZE/3);
|
---|
| 233 |
|
---|
| 234 | % activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2));
|
---|
| 235 | % activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2));
|
---|
| 236 |
|
---|
| 237 | %%%
|
---|
| 238 | % generally it evaluates only 5x5 region aroung the rough position
|
---|
| 239 | % it is scaled accordingly. The 5x5 grid is a compromise between the speed
|
---|
| 240 | % and the robustness against bad rough position
|
---|
| 241 | sc = 2; % 2 is for 5x5 neigh, 1 is for 3x3, 3 is for 9x9 and so on
|
---|
| 242 | activerows = ceil(size(IM2,1)/2)-sc*round(leds.scale):ceil(size(IM2,1)/2)+sc*round(leds.scale);
|
---|
| 243 | activecols = ceil(size(IM2,2)/2)-sc*round(leds.scale):ceil(size(IM2,2)/2)+sc*round(leds.scale);
|
---|
| 244 | im2activerows = ceil(size(IM2,1)/2)-floor(size(Gaussian,1)/2)-sc*round(leds.scale):ceil(size(IM2,1)/2)+floor(size(Gaussian,1)/2)+sc*round(leds.scale);
|
---|
| 245 | im2activecols = ceil(size(IM2,2)/2)-floor(size(Gaussian,2)/2)-sc*round(leds.scale):ceil(size(IM2,2)/2)+floor(size(Gaussian,2)/2)+sc*round(leds.scale);
|
---|
| 246 |
|
---|
| 247 | % CHECK if leds.size and leds.scale have reasonable values
|
---|
| 248 | % and correct them if not.
|
---|
| 249 | % typically, if the assumed LED size is fairly small, a complete IM2 must be taken
|
---|
| 250 | if (min([im2activerows,im2activecols])<1)
|
---|
| 251 | if SHOW_WARN
|
---|
| 252 | warning('probably incorect setting of leds.size and leds.scale variables')
|
---|
| 253 | end
|
---|
| 254 | im2activerows = 1:size(IM2,1);
|
---|
| 255 | im2activecols = 1:size(IM2,2);
|
---|
| 256 | activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2));
|
---|
| 257 | activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2));
|
---|
| 258 | end
|
---|
| 259 |
|
---|
| 260 | corrcoefmat = zeros(size(IM2));
|
---|
| 261 | % t1 = cputime;
|
---|
| 262 | if BLK_PROC % blkproc may be faster for big neighbourhoods
|
---|
| 263 | corrcoefmat(activerows,activecols) = blkproc(IM2(activerows,activecols),[1,1],[Gsize,Gsize],'corr2',Gaussian);
|
---|
| 264 | else
|
---|
| 265 | G = double(Gaussian(:));
|
---|
| 266 | Gn = G-mean(G);
|
---|
| 267 | Gn2 = sum(Gn.^2);
|
---|
| 268 | B = im2col(double(IM2(im2activerows,im2activecols)),size(Gaussian),'sliding');
|
---|
| 269 | corrcoefmat(activerows,activecols) = col2im(mycorr2(B,G,Gn,Gn2), size(Gaussian), size(IM2(im2activerows,im2activecols)),'sliding');
|
---|
| 270 | % corrcoefmat(activerows,activecols) = colfilt(double(IM2(activerows,activecols)),size(Gaussian),'sliding','mycorr2',G,Gn,Gn2);
|
---|
| 271 | end
|
---|
| 272 | % disp(sprintf('elapsed for coorrelations: %f',cputime-t1'))
|
---|
| 273 |
|
---|
| 274 | [maxcorrcoef,idxmaxcorrcoef] = max(corrcoefmat(:));
|
---|
| 275 | [rmax,cmax] = ind2sub(size(corrcoefmat),idxmaxcorrcoef);
|
---|
| 276 | finepos = rawpos+([rmax,cmax]-ceil(size(IM2)/2))./leds.scale;
|
---|
| 277 |
|
---|
| 278 | %%%
|
---|
| 279 | % plot the subimage with detected position of the maximal correlation
|
---|
| 280 | %%%
|
---|
| 281 |
|
---|
| 282 | if showfig
|
---|
| 283 | figure(5),
|
---|
| 284 | clf
|
---|
| 285 | subplot(2,2,4)
|
---|
| 286 | showimg(IM2,5);
|
---|
| 287 | title('Interpolated ROI')
|
---|
| 288 | % colormap('gray');
|
---|
| 289 | hold on
|
---|
| 290 | axis on
|
---|
| 291 | plot(cmax,rmax,'g+','EraseMode','Back','MarkerSize',15,'LineWidth',3)
|
---|
| 292 | hold off
|
---|
| 293 | subplot(2,2,3)
|
---|
| 294 | mesh(corrcoefmat)
|
---|
| 295 | title('Correlation coeffs')
|
---|
| 296 | subplot(2,2,1)
|
---|
| 297 | mesh(double(IM2(activerows,activecols)))
|
---|
| 298 | title('Active ROI')
|
---|
| 299 | subplot(2,2,2)
|
---|
| 300 | mesh(Gaussian)
|
---|
| 301 | title('PSF approx by 2D Gaussian')
|
---|
| 302 | drawnow
|
---|
| 303 | % pause
|
---|
| 304 | end
|
---|
| 305 |
|
---|
| 306 | return
|
---|
| 307 |
|
---|
| 308 |
|
---|
| 309 |
|
---|
| 310 |
|
---|
| 311 |
|
---|
| 312 |
|
---|
| 313 |
|
---|
| 314 |
|
---|
| 315 |
|
---|
| 316 |
|
---|
| 317 |
|
---|
| 318 |
|
---|
| 319 |
|
---|
| 320 |
|
---|
| 321 |
|
---|
| 322 |
|
---|