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 |
|
---|