source: branches/FACT++_part_filenames/drive/FilterLed.cc

Last change on this file was 18621, checked in by tbretz, 8 years ago
Removed references to root, replaced with basic types.
File size: 11.7 KB
Line 
1#include "FilterLed.h"
2
3#include <memory.h> // memset
4#include <math.h>
5#include <iostream> // cout
6
7#include "Led.h"
8#include "Ring.h"
9
10#include "MGImage.h"
11
12using namespace std;
13
14class ClusterFinder
15{
16private:
17 uint8_t *fImg;
18
19 uint32_t fW;
20 uint32_t fH;
21
22 int32_t fX0;
23 int32_t fX1;
24
25 int32_t fY0;
26 int32_t fY1;
27
28 uint32_t fLimitingSize;
29
30 uint32_t fCount;
31 float fSumX;
32 float fSumY;
33
34 float FindCluster(int32_t x, int32_t y)
35 {
36 // if edge is touched stop finding cluster
37 if (x<fX0 || x>=fX1 || y<fY0 || y>=fY1)
38 return -1;
39
40 if (fCount>fLimitingSize)
41 return -2;
42
43 // get the value
44 float val = fImg[y*fW+x];
45
46 // if its empty we have found the border of the cluster
47 if (val==0)
48 return 0;
49
50 // mark the point as processed
51 fImg[y*fW+x] = 0;
52
53 fSumX += x*val; // sumx
54 fSumY += y*val; // sumy
55 fCount++;
56
57 float rc[4];
58 rc[0] = FindCluster(x+1, y );
59 rc[1] = FindCluster(x, y+1);
60 rc[2] = FindCluster(x-1, y );
61 rc[3] = FindCluster(x, y-1);
62
63 for (int i=0; i<4; i++)
64 {
65 if (rc[i]<0) // check if edge is touched
66 return rc[i];
67
68 val += rc[i];
69 }
70
71 return val;
72 }
73
74public:
75 ClusterFinder(uint8_t *img, uint32_t w, uint32_t h) : fImg(0), fLimitingSize(999)
76 {
77 fW = w;
78 fH = h;
79
80 fX0 = 0;
81 fY0 = 0;
82 fX1 = fW;
83 fY1 = fH;
84
85 fImg = new uint8_t[fW*fH];
86
87 memcpy(fImg, img, fW*fH);
88 }
89
90 ~ClusterFinder()
91 {
92 delete [] fImg;
93 }
94 Double_t GetSumX() const { return fSumX; }
95 Double_t GetSumY() const { return fSumY; }
96
97 uint32_t GetCount() const { return fCount; }
98
99 void SetLimitingSize(uint32_t lim) { fLimitingSize=lim; }
100
101 float FindClusterAt(int32_t x, int32_t y)
102 {
103 fCount = 0;
104 fSumX = 0;
105 fSumY = 0;
106
107 return FindCluster(x, y);
108 }
109
110 void SetRange(int32_t x0=0, int32_t y0=0, int32_t x1=0, int32_t y1=0)
111 {
112 fX0 = x0;
113 fY0 = y0;
114 fX1 = x1==0?fW:x1;
115 fY1 = y1==0?fH:y1;
116 }
117
118 void FindCluster(vector<Led> &leds, int32_t x0=0, int32_t y0=0, int32_t x1=0, int32_t y1=0)
119 {
120 fX0 = x0;
121 fY0 = y0;
122 fX1 = x1==0?fW:x1;
123 fY1 = y1==0?fH:y1;
124
125 for (int32_t x=fX0; x<fX1; x++)
126 for (int32_t y=fY0; y<fY1; y++)
127 {
128 const uint8_t &b = fImg[y*fW+x];
129 if (b==0)
130 continue;
131
132 const float mag = FindClusterAt(x, y);
133 if (fCount>999)
134 {
135 cout << "ERROR - Spot with Size>999 detected..." << endl;
136 return;
137 }
138
139 if (mag>0 && fCount>4)
140 leds.push_back(Led(fSumX/mag, fSumY/mag, 0, mag));
141 }
142 //leds.Compress();
143 }
144};
145
146
147void FilterLed::DrawBox(const int x1, const int y1,
148 const int x2, const int y2,
149 const int col) const
150{
151 MGImage::DrawBox(fImg, 768, 576, x1, y1, x2, y2, col);
152}
153
154void FilterLed::MarkPoint(float px, float py, float mag) const
155{
156 const int x = (int)(px+.5);
157 const int y = (int)(py+.5);
158 const int m = (int)(mag);
159
160 DrawBox(x-8, y, x-5, y, m);
161 DrawBox(x, y+5, x, y+8, m);
162 DrawBox(x+5, y, x+8, y, m);
163 DrawBox(x, y-8, x, y-5, m);
164}
165
166void FilterLed::MarkPoint(const Led &led) const
167{
168 /*
169 int32_t M = (int)(log(led.GetMag())*20);
170
171 cout << led.GetMag() << endl;
172
173 if (M>0xff)
174 M=0xff;
175 if (M<0xc0)
176 M=0xc0;
177 */
178
179 const int x = (int)(led.GetX()+.5);
180 const int y = (int)(led.GetY()+.5);
181
182 MarkPoint(x, y, 0xff);
183}
184
185void FilterLed::DrawCircle(float cx, float cy, float r, uint8_t col) const
186{
187 MGImage::DrawCircle(fImg, 768, 576, cx, cy, r, col);
188}
189
190void FilterLed::DrawHexagon(float cx, float cy, float r, uint8_t col) const
191{
192 MGImage::DrawHexagon(fImg, 768, 576, cx, cy, r, col);
193}
194
195void FilterLed::DrawCircle(const Ring &l, uint8_t col) const
196{
197 DrawCircle(l.GetX(), l.GetY(), l.GetR(), col);
198}
199
200void FilterLed::DrawCircle(const Ring &l, double r, uint8_t col) const
201{
202 DrawCircle(l.GetX(), l.GetY(), r, col);
203}
204
205void FilterLed::DrawHexagon(const Ring &l, double r, uint8_t col) const
206{
207 DrawHexagon(l.GetX(), l.GetY(), r, col);
208}
209
210void FilterLed::GetMinMax(const int offset, uint8_t *min, uint8_t *max) const
211{
212 *min = fImg[0];
213 *max = fImg[0];
214
215 uint8_t *s = (uint8_t*)fImg;
216 const uint8_t *e0 = s+fW*fH;
217
218 //
219 // calculate mean value (speed optimized)
220 //
221 while (s<e0)
222 {
223 const uint8_t *e = s+fH-offset;
224 s += offset;
225
226 while (s<e)
227 {
228 if (*s>*max)
229 {
230 *max = *s;
231 if (*max-*min==255)
232 return;
233 }
234 if (*s<*min)
235 {
236 *min = *s;
237 if (*max-*min==255)
238 return;
239 }
240 s++;
241 }
242 s+=offset;
243 }
244}
245
246int FilterLed::GetMeanPosition(const int x, const int y,
247 const int boxx, const int boxy,
248 float &mx, float &my, unsigned int &sum) const
249{
250 unsigned int sumx=0;
251 unsigned int sumy=0;
252
253 sum=0;
254 for (int dx=x-boxx; dx<x+boxx+1; dx++)
255 for (int dy=y-boxy; dy<y+boxy+1; dy++)
256 {
257 const uint8_t &m = fImg[dy*fW+dx];
258
259 sumx += m*dx;
260 sumy += m*dy;
261 sum += m;
262 }
263
264 mx = (float)sumx/sum;
265 my = (float)sumy/sum;
266
267 return (int)my*fW + (int)mx;
268}
269
270int FilterLed::GetMeanPosition(const int x, const int y, const int boxx, const int boxy) const
271{
272 float mx, my;
273 unsigned int sum;
274 return GetMeanPosition(x, y, boxx, boxy, mx, my, sum);
275}
276
277int FilterLed::GetMeanPositionBox(const int x, const int y,
278 const int boxx, const int boxy,
279 float &mx, float &my, unsigned int &sum) const
280{
281 //-------------------------------
282 // Improved algorithm:
283 // 1. Look for the largest five-pixel-cross signal inside the box
284 int x0 = max(x-boxx+1, 0);
285 int y0 = max(y-boxy+1, 0);
286
287 int x1 = min(x+boxx+1-1, fW);
288 int y1 = min(y+boxy+1-1, fH);
289
290 int maxx=0;
291 int maxy=0;
292
293 unsigned int max =0;
294 for (int dx=x0; dx<x1; dx++)
295 {
296 for (int dy=y0; dy<y1; dy++)
297 {
298 const unsigned int sumloc =
299 fImg[(dy+0)*fW + (dx-1)] +
300 fImg[(dy+0)*fW + (dx+1)] +
301 fImg[(dy+1)*fW + dx] +
302 fImg[(dy+0)*fW + dx] +
303 fImg[(dy-1)*fW + dx];
304
305 if(sumloc<=max)
306 continue;
307
308 maxx=dx;
309 maxy=dy;
310 max =sumloc;
311 }
312 }
313
314 // 2. Calculate mean position inside a circle around
315 // the highst cross-signal with radius of 6 pixels.
316 ClusterFinder find(fImg, fW, fH);
317 find.SetLimitingSize(9999);
318 find.SetRange(x0, y0, x1, y1);
319
320 const float mag = find.FindClusterAt(maxx, maxy);
321
322 mx = find.GetSumX()/mag;
323 my = find.GetSumY()/mag;
324
325 sum = (int)(mag+0.5);
326
327 return (int)my*fW + (int)mx;
328}
329
330int FilterLed::GetMeanPositionBox(const int x, const int y,
331 const int boxx, const int boxy) const
332{
333 float mx, my;
334 unsigned int sum;
335 return GetMeanPositionBox(x, y, boxx, boxy, mx, my, sum);
336}
337
338void FilterLed::Execute(vector<Led> &leds, int xc, int yc) const
339{
340 double bright;
341 Execute(leds, xc, yc, bright);
342}
343
344void FilterLed::Execute(vector<Led> &leds, int xc, int yc, double &bright) const
345{
346 const int x0 = max(xc-fBoxX, 0);
347 const int y0 = max(yc-fBoxY, 0);
348 const int x1 = min(xc+fBoxX, fW);
349 const int y1 = min(yc+fBoxY, fH);
350
351 const int wx = x1-x0;
352 const int hy = y1-y0;
353
354 double sum = 0;
355 double sq = 0;
356
357 for (int x=x0; x<x1; x++)
358 for (int y=y0; y<y1; y++)
359 {
360 uint8_t &b = fImg[y*fW+x];
361
362 // Skip saturating pixels
363 if (b>0xf0)
364 continue;
365
366 sum += b;
367 sq += b*b;
368 }
369
370 sum /= wx*hy;
371 sq /= wx*hy;
372
373 bright=sum;
374
375
376 // 254 because b<=max and not b<max
377 const double sdev = sqrt(sq-sum*sum);
378 const uint8_t max = sum+fCut*sdev>254 ? 254 : (uint8_t)(sum+fCut*sdev);
379
380 //
381 // clean image from noise
382 // (FIXME: A lookup table could accelerate things...
383 //
384 for (int x=x0; x<x1; x++)
385 for (int y=y0; y<y1; y++)
386 {
387 uint8_t &b = fImg[y*fW+x];
388 if (b<=max)
389 b = 0;
390 }
391
392 ClusterFinder find(fImg, fW, fH);
393 find.FindCluster(leds, x0, y0, x1, y1);
394}
395
396void FilterLed::FindStar(vector<Led> &leds, int xc, int yc, bool box) const
397{
398 // fBox: radius of the inner (signal) box
399 // Radius of the outer box is fBox*sqrt(2)
400
401 //
402 // Define inner box in which to search the signal
403 //
404 const int x0 = max(xc-fBoxX, 0);
405 const int y0 = max(yc-fBoxY, 0);
406 const int x1 = min(xc+fBoxX, fW);
407 const int y1 = min(yc+fBoxY, fH);
408
409 //
410 // Define outer box (excluding inner box) having almost
411 // the same number of pixels in which the background
412 // is calculated
413 //
414 const double sqrt2 = sqrt(2.);
415
416 const int xa = max(xc-(int)nearbyint(fBoxX*sqrt2), 0);
417 const int ya = max(yc-(int)nearbyint(fBoxY*sqrt2), 0);
418 const int xb = min(xc+(int)nearbyint(fBoxX*sqrt2), fW);
419 const int yb = min(yc+(int)nearbyint(fBoxY*sqrt2), fH);
420
421 //
422 // Calculate average and sdev for a square
423 // excluding the inner part were we expect
424 // the signal to be.
425 //
426 double sum = 0;
427 double sq = 0;
428
429 int n=0;
430 for (int x=xa; x<xb; x++)
431 for (int y=ya; y<yb; y++)
432 {
433 if (x>=x0 && x<x1 && y>=y0 && y<y1)
434 continue;
435
436 uint8_t &b = fImg[y*fW+x];
437
438 sum += b;
439 sq += b*b;
440 n++;
441 }
442
443 sum /= n;
444 sq /= n;
445
446 // 254 because b<=max and not b<max
447 const double sdev = sqrt(sq-sum*sum);
448 const uint8_t max = sum+fCut*sdev>254 ? 254 : (uint8_t)(sum+fCut*sdev);
449
450 //
451 // clean image from noise
452 // (FIXME: A lookup table could accelerate things...
453 //
454 n=0;
455 for (int x=x0; x<x1; x++)
456 for (int y=y0; y<y1; y++)
457 {
458 uint8_t &b = fImg[y*fW+x];
459 if (b<=max)
460 b = 0;
461 else
462 n++;
463 }
464
465 //
466 // Mark the background region
467 //
468 for (int x=xa; x<xb; x+=2)
469 {
470 fImg[ya*fW+x]=0xf0;
471 fImg[yb*fW+x]=0xf0;
472 }
473 for (int y=ya; y<yb; y+=2)
474 {
475 fImg[y*fW+xa]=0xf0;
476 fImg[y*fW+xb]=0xf0;
477 }
478
479 //
480 // Check if any pixel found...
481 //
482 if (n<5)
483 return;
484
485 //
486 // Get the mean position of the star
487 //
488 float mx, my;
489 unsigned int mag;
490 int pos = box ? GetMeanPositionBox(xc, yc, fBoxX-1, fBoxY-1, mx, my, mag)
491 : GetMeanPosition(xc, yc, fBoxX-1, fBoxY-1, mx, my, mag);
492
493 if (pos<0 || pos>=fW*fH || fImg[pos]<sum+fCut*sdev)
494 return;
495
496 // cout << "Mean=" << sum << " SDev=" << sdev << " : ";
497 // cout << "Sum/n = " << sum << "/" << n << " = " << (n==0?0:mag/n) << endl;
498
499 leds.push_back(Led(mx, my, 0, -2.5*log10((float)mag)+13.7));
500}
501
502void FilterLed::Stretch() const
503{
504 uint8_t min, max;
505 GetMinMax(25, &min, &max);
506
507 if (min==max || max-min>230) // 255/230=1.1
508 return;
509
510 const float scale = 255./(max-min);
511
512 uint8_t *b = fImg;
513 const uint8_t *e = fImg+fW*fH;
514
515 while (b<e)
516 {
517 if (*b<min)
518 {
519 *b++=0;
520 continue;
521 }
522 if (*b>max)
523 {
524 *b++=255;
525 continue;
526 }
527 *b = (uint8_t)((*b-min)*scale);
528 b++;
529 }
530}
Note: See TracBrowser for help on using the repository browser.