[37] | 1 | % GETPOINT ... extracts position of an LED from an image
|
---|
| 2 | % only one or none LED is expected
|
---|
| 3 | %
|
---|
| 4 | % function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM)
|
---|
| 5 | %
|
---|
| 6 | % imname ... name of the image (full path should be specified)
|
---|
| 7 | % showfig .. show figures (1->on/0->off)
|
---|
| 8 | % imconfig . config.imgs, see CONFIGDATA
|
---|
| 9 | % avIM ... average image of the camera, see IM2POINTS
|
---|
| 10 | % stdIM ... image of standard deviations, see IM2POINTS
|
---|
| 11 | %
|
---|
| 12 | % pos ...... 2x1 vector containing (x,y)'-coordinates of the point
|
---|
| 13 | % if error then 0 is returned
|
---|
| 14 | % err ...... boolean, indicates an error (ambiguous blobs, point too
|
---|
| 15 | % eccentric, etc.)
|
---|
| 16 |
|
---|
| 17 | % $Author: svoboda $
|
---|
| 18 | % $Revision: 2.0 $
|
---|
| 19 | % $Id: getpoint.m,v 2.0 2003/06/19 12:07:10 svoboda Exp $
|
---|
| 20 | % $State: Exp $
|
---|
| 21 |
|
---|
| 22 | function [pos,err] = getpoint(imname, showfig, imconfig, avIM, stdIM,subpix)
|
---|
| 23 |
|
---|
| 24 | err = 0;
|
---|
| 25 |
|
---|
| 26 | SHOW_WARN = 0; % show warnings?
|
---|
| 27 | BLK_PROC = 0; % blkproc may be faster for bigger LEDs
|
---|
| 28 | SUB_PIX = 1/imconfig.subpix; % required sub-pixel precision 3 -> 1/3 pixel
|
---|
| 29 |
|
---|
| 30 | TEST_ECC = 0; % perform the eccentricity check?
|
---|
| 31 | ECC_THR = 0.7; % eccentricity threshold (for validity check)
|
---|
| 32 | % this threshold is not usable in the current implementation
|
---|
| 33 |
|
---|
| 34 | LEDSIZE = imconfig.LEDsize; % avg diameter of a LED in pixels
|
---|
| 35 |
|
---|
| 36 | im.name = imname;
|
---|
| 37 |
|
---|
| 38 | %%%
|
---|
| 39 | % set figure handles
|
---|
| 40 | fig.im4thr = 1; % image used for thresholding
|
---|
| 41 | fig.imOrig = 2; % original image
|
---|
| 42 | fig.blob = 3; % ouput of bwlabel
|
---|
| 43 | fig.subI = 4; % subimage (local neighbourhood of est. LED pos.)
|
---|
| 44 |
|
---|
| 45 | im.info = imfinfo(im.name);
|
---|
| 46 | im.orig = imread(im.name);
|
---|
| 47 |
|
---|
| 48 | if findstr(im.info.ColorType,'grayscale');
|
---|
| 49 | im.I = im.orig;
|
---|
| 50 | else
|
---|
| 51 | [im.r,im.c] = size(im.orig(:,:,1));
|
---|
| 52 | im.R = im.orig(:,:,1); % Red component
|
---|
| 53 | im.G = im.orig(:,:,2); % Green component
|
---|
| 54 | end
|
---|
| 55 |
|
---|
| 56 | % find possible location of the point by thresholding
|
---|
| 57 | if strcmp(imconfig.LEDcolor,'green')
|
---|
| 58 | im.thr = uint8(abs(double(im.G(:,:))-double(avIM(:,:,2)))); % on which image the thresholding will be done
|
---|
| 59 | im.std = stdIM(:,:,2); % use green component
|
---|
| 60 | im.fit = im.G; % image for fitting of the PSF
|
---|
| 61 | elseif strcmp(imconfig.LEDcolor,'red')
|
---|
| 62 | im.thr = uint8(abs(double(im.R(:,:))-double(avIM(:,:,1)))); % on which image the thresholding will be done
|
---|
| 63 | im.std = stdIM(:,:,1); % use red component
|
---|
| 64 | im.fit = im.R; % image for fitting of the PSF
|
---|
| 65 | else
|
---|
| 66 | error('getpoint: no valid color of the laser pointer, see CONFIGDATA');
|
---|
| 67 | end
|
---|
| 68 |
|
---|
| 69 | % show figures if required, may be useful when debugging
|
---|
| 70 | if showfig
|
---|
| 71 | figure(fig.imOrig),
|
---|
| 72 | clf
|
---|
| 73 | imshow(im.orig)
|
---|
| 74 | title(strcat(im.name, ' original'))
|
---|
| 75 | hold on
|
---|
| 76 | figure(fig.im4thr),
|
---|
| 77 | clf
|
---|
| 78 | imshow(im.thr);
|
---|
| 79 | title(strcat(im.name, ' image to be thresholded'))
|
---|
| 80 | drawnow
|
---|
| 81 | hold on
|
---|
| 82 | end
|
---|
| 83 |
|
---|
| 84 | % sortedInt = sort(double(im.thr(:))); % sort intensities
|
---|
| 85 | [maxint,idx] = max(im.thr(:));
|
---|
| 86 | leds.thr = double(maxint)*4/5;
|
---|
| 87 | aboveThr = sum(sum(im.thr>leds.thr));
|
---|
| 88 | % check how many pixels lie above the threshold
|
---|
| 89 | % if too many, there is probably no LED at all
|
---|
| 90 | % otherwise, take the position of the maximal intensity
|
---|
| 91 | % as the LED position
|
---|
| 92 | if aboveThr > (pi*LEDSIZE^2/2)
|
---|
| 93 | if SHOW_WARN
|
---|
| 94 | warning('Perhaps no LED in the image, detected blob is too large')
|
---|
| 95 | end
|
---|
| 96 | err=1;
|
---|
| 97 | pos=0;
|
---|
| 98 | return;
|
---|
| 99 | elseif ( (im.thr(idx) < 5*double(im.std(idx))) | ( im.thr(idx)< 70 ))
|
---|
| 100 | if SHOW_WARN
|
---|
| 101 | warning('Perhaps no LED in the image, detected maximum of image difference is too low')
|
---|
| 102 | end
|
---|
| 103 | err=1;
|
---|
| 104 | pos=0;
|
---|
| 105 | return;
|
---|
| 106 | else
|
---|
| 107 | rawpos = zeros(1,2);
|
---|
| 108 | [rawpos(1),rawpos(2)] = ind2sub(size(im.thr),idx);
|
---|
| 109 | end
|
---|
| 110 |
|
---|
| 111 | leds.size = round(LEDSIZE/1.2); % (2*leds.size+1)x(2*leds.size+1) is the area of interest around each detected LED
|
---|
| 112 | % check if the LED lies in the allowed position (not very close to the image border
|
---|
| 113 | % it is because of the implementation not because of principle
|
---|
| 114 | if rawpos(1)-leds.size < 1 | rawpos(1)+leds.size > size(im.thr,1) | ...
|
---|
| 115 | rawpos(2)-leds.size < 1 | rawpos(2)+leds.size > size(im.thr,2)
|
---|
| 116 | if SHOW_WARN
|
---|
| 117 | warning('LED position lies outside allowed boundary');
|
---|
| 118 | end
|
---|
| 119 | err = 1;
|
---|
| 120 | pos = 0;
|
---|
| 121 | return
|
---|
| 122 | end
|
---|
| 123 |
|
---|
| 124 | leds.rows = (rawpos(1)-leds.size):(rawpos(1)+leds.size);
|
---|
| 125 | leds.cols = (rawpos(2)-leds.size):(rawpos(2)+leds.size);
|
---|
| 126 | [L,num] = bwlabel(im.thr(leds.rows,leds.cols)>leds.thr);
|
---|
| 127 | %%%
|
---|
| 128 | % define subimage as local neighbour of estimated LED position
|
---|
| 129 | if TEST_ECC
|
---|
| 130 | im.stats = imfeature(L,'Centroid','Eccentricity');
|
---|
| 131 | else
|
---|
| 132 | im.stats = imfeature(L,'Centroid');
|
---|
| 133 | end
|
---|
| 134 |
|
---|
| 135 | if size(im.stats,1)>1,
|
---|
| 136 | if SHOW_WARN
|
---|
| 137 | warning('More than one blob detected')
|
---|
| 138 | end
|
---|
| 139 | err=1; pos=0;
|
---|
| 140 | return;
|
---|
| 141 | end
|
---|
| 142 |
|
---|
| 143 | if TEST_ECC
|
---|
| 144 | if (im.stats.Eccentricity > ECC_THR)
|
---|
| 145 | warning('eccentricity treshold exceeded');
|
---|
| 146 | err = 1; pos = 0;
|
---|
| 147 | end
|
---|
| 148 | end
|
---|
| 149 |
|
---|
| 150 | % Crop the sub-image of interest around found LED
|
---|
| 151 | IM = im.fit(leds.rows,leds.cols);
|
---|
| 152 |
|
---|
| 153 | % visual check of LED position
|
---|
| 154 | if showfig
|
---|
| 155 | figure(fig.subI)
|
---|
| 156 | imshow(IM)
|
---|
| 157 | plot(im.stats(1).Centroid(1),im.stats(1).Centroid(2),'g+','EraseMode','Back');
|
---|
| 158 | drawnow
|
---|
| 159 | % pause
|
---|
| 160 | end
|
---|
| 161 |
|
---|
| 162 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
| 163 | % interpolate local neighborhood and find the maxima
|
---|
| 164 | % by correlation with the gaussian (see declaration of Gsize)
|
---|
| 165 | leds.scale = SUB_PIX; % the area of interest will be inreased by leds.scale using bilinear interpolation
|
---|
| 166 | % leds.size should be comparable to the leds.scale. If leds.size is assumed too small
|
---|
| 167 | % then the correlation based detection does not work
|
---|
| 168 | % properly
|
---|
| 169 | Gsize = round(leds.scale*LEDSIZE/2); % (2*Gsize+1)x(2*Gsize+1) is the dimension of the Gaussian mask that models PSF
|
---|
| 170 |
|
---|
| 171 | %activerows = (Gsize+1):(leds.scale*(leds.size*2+1)-Gsize-1);
|
---|
| 172 | %activecols = (Gsize+1):(leds.scale*(leds.size*2+1)-Gsize-1);
|
---|
| 173 |
|
---|
| 174 | % t1 = cputime;
|
---|
| 175 | IM2 = imresize(IM,leds.scale,'bicubic'); % zoom in
|
---|
| 176 | % disp(sprintf('elapsed for resize: %f',cputime-t1'))
|
---|
| 177 |
|
---|
| 178 | % Correlation mask that approximates point spread function (PSF) of a LED
|
---|
| 179 | Gaussian = fspecial('Gaussian',2*Gsize+1,leds.scale*LEDSIZE/3);
|
---|
| 180 |
|
---|
| 181 | activerows = ceil(size(Gaussian,1)/2):(size(IM2,1)-floor(size(Gaussian,1)/2));
|
---|
| 182 | activecols = ceil(size(Gaussian,2)/2):(size(IM2,2)-floor(size(Gaussian,2)/2));
|
---|
| 183 |
|
---|
| 184 | % check if leds.size and leds.scale have reasonable values
|
---|
| 185 | if (size(activerows,2)<5 | size(activerows,2)>50)
|
---|
| 186 | error('probably incorect setting of leds.size and leds.scale variables')
|
---|
| 187 | end
|
---|
| 188 |
|
---|
| 189 | corrcoefmat = zeros(size(IM2));
|
---|
| 190 | % t1 = cputime;
|
---|
| 191 | if BLK_PROC % blkproc may be faster for big neighbourhoods
|
---|
| 192 | corrcoefmat(activerows,activecols) = blkproc(IM2(activerows,activecols),[1,1],[Gsize,Gsize],'corr2',Gaussian);
|
---|
| 193 | else
|
---|
| 194 | G = double(Gaussian(:));
|
---|
| 195 | Gn = G-mean(G);
|
---|
| 196 | Gn2 = sum(Gn.^2);
|
---|
| 197 | B = im2col(double(IM2),size(Gaussian),'sliding');
|
---|
| 198 | corrcoefmat(activerows,activecols) = col2im(mycorr2(B,G,Gn,Gn2), size(Gaussian), size(IM2),'sliding');
|
---|
| 199 | % corrcoefmat(activerows,activecols) = colfilt(double(IM2(activerows,activecols)),size(Gaussian),'sliding','mycorr2',G,Gn,Gn2);
|
---|
| 200 | end
|
---|
| 201 | % disp(sprintf('elapsed for coorrelations: %f',cputime-t1'))
|
---|
| 202 |
|
---|
| 203 | [maxcorrcoef,idxmaxcorrcoef] = max(corrcoefmat(:));
|
---|
| 204 | [rmax,cmax] = ind2sub(size(corrcoefmat),idxmaxcorrcoef);
|
---|
| 205 | finepos = rawpos+([rmax,cmax]-ceil(size(IM2)/2))./leds.scale;
|
---|
| 206 |
|
---|
| 207 | %%%
|
---|
| 208 | % plot the subimage with detected position of the maximal correlation
|
---|
| 209 | %%%
|
---|
| 210 |
|
---|
| 211 | if showfig
|
---|
| 212 | figure(5),
|
---|
| 213 | clf
|
---|
| 214 | subplot(2,2,4)
|
---|
| 215 | showimg(IM2,5);
|
---|
| 216 | % colormap('gray');
|
---|
| 217 | hold on
|
---|
| 218 | axis on
|
---|
| 219 | plot(cmax,rmax,'g+','EraseMode','Back')
|
---|
| 220 | hold off
|
---|
| 221 | subplot(2,2,3)
|
---|
| 222 | mesh(corrcoefmat)
|
---|
| 223 | subplot(2,2,1)
|
---|
| 224 | mesh(double(IM2(activerows,activecols)))
|
---|
| 225 | subplot(2,2,2)
|
---|
| 226 | mesh(Gaussian)
|
---|
| 227 | drawnow
|
---|
| 228 | % pause
|
---|
| 229 | end
|
---|
| 230 |
|
---|
| 231 | %%% plot information about detected LED
|
---|
| 232 | if showfig
|
---|
| 233 | figure(fig.im4thr)
|
---|
| 234 | plot(finepos(2),finepos(1),'r+','EraseMode','Back');
|
---|
| 235 | figure(fig.imOrig)
|
---|
| 236 | plot(finepos(2),finepos(1),'r+','EraseMode','Back','MarkerSize',10);
|
---|
| 237 | drawnow
|
---|
| 238 | end
|
---|
| 239 |
|
---|
| 240 | if showfig
|
---|
| 241 | pause
|
---|
| 242 | end
|
---|
| 243 |
|
---|
| 244 | pos = [finepos(2); finepos(1)];
|
---|
| 245 |
|
---|
| 246 |
|
---|
| 247 |
|
---|
| 248 |
|
---|
| 249 |
|
---|
| 250 |
|
---|
| 251 |
|
---|
| 252 |
|
---|
| 253 |
|
---|
| 254 |
|
---|
| 255 |
|
---|
| 256 |
|
---|
| 257 |
|
---|
| 258 |
|
---|
| 259 |
|
---|
| 260 |
|
---|
| 261 |
|
---|