source: trunk/MagicSoft/Cosy/videodev/CaosFilter.cc@ 1803

Last change on this file since 1803 was 1802, checked in by tbretz, 23 years ago
*** empty log message ***
File size: 12.5 KB
Line 
1#include "CaosFilter.h"
2
3#include <memory.h> // memset
4#include <iostream.h> // cout
5#include <fstream.h>
6
7#include "Led.h"
8#include "Leds.h"
9
10ClassImp(CaosFilter);
11
12void CaosFilter::DrawBox(const int x1, const int y1,
13 const int x2, const int y2,
14 byte *buffer, const int col)
15{
16 for (int x=x1; x<x2+1; x++)
17 for (int y=y1; y<y2+1; y++)
18 buffer[y*768+x] = col;
19}
20
21void CaosFilter::MarkPoint(const int x, const int y, byte *buffer, const int col)
22{
23 DrawBox(x-8, y, x-5, y, buffer, col);
24 DrawBox(x, y+5, x, y+8, buffer, col);
25 DrawBox(x+5, y, x+8, y, buffer, col);
26 DrawBox(x, y-8, x, y-5, buffer, col);
27 return;
28}
29
30float CaosFilter::Mean(const byte *buffer, const int offset, int *min, int *max)
31{
32 double mean = 0.0;
33
34 *min = 0xff;
35 *max = 0x00;
36
37 //
38 // calculate mean value
39 //
40 for (int x=offset; x<768-offset; x++)
41 for (int y=offset; y<576-offset; y++)
42 {
43 byte val = buffer[y*768+x];
44
45 mean += val;
46
47 if (val>*max)
48 *max = val;
49
50 if (val<*min)
51 *min = val;
52 }
53
54 mean /= (768-2*offset)*(576-2*offset);
55
56 return mean;
57}
58
59int CaosFilter::GetMeanPosition(const byte *bitmap, const int x, const int y,
60 const int box, Float_t &mx, Float_t &my)
61{
62 unsigned int sumx=0;
63 unsigned int sumy=0;
64
65 unsigned int sum=0;
66
67 for (int dx=x-box; dx<x+box+1; dx++)
68 for (int dy=y-box; dy<y+box+1; dy++)
69 {
70 const byte m = bitmap[dy*768+dx]; // desc->buffer[3*(x+y*768)]; //
71
72 sumx += m*dx;
73 sumy += m*dy;
74 sum += m;
75 }
76
77 mx = (Float_t)sumx/sum;
78 my = (Float_t)sumy/sum;
79
80 return (int)my*768 + (int)mx;
81}
82
83float CaosFilter::SDev(const byte *buffer, const int offset, const double mean)
84{
85 //
86 // calculate sigma
87 //
88 double sdev=0.0;
89
90 for (int x=offset; x<768-offset; x++)
91 for (int y=offset; y<576-offset; y++)
92 {
93 const float val = mean - buffer[y*768+x];
94
95 sdev += val*val;
96 }
97
98 sdev /= (768-2*offset)*(576-2*offset)-1;
99
100 return sqrt(sdev);
101}
102
103
104int CaosFilter::GetMeanPosition(const byte *bitmap, const int x, const int y,
105 const int box)
106{
107 unsigned int sumx=0;
108 unsigned int sumy=0;
109
110 unsigned int sum=0;
111
112 for (int dx=x-box; dx<x+box+1; dx++)
113 for (int dy=y-box; dy<y+box+1; dy++)
114 {
115 const byte m = bitmap[dy*768+dx]; // desc->buffer[3*(x+y*768)]; //
116
117 sumx += m*dx;
118 sumy += m*dy;
119 sum += m;
120 }
121
122 const float px = (float)sumx/sum;
123 const float py = (float)sumy/sum;
124
125 return (int)py*768 + (int)px;
126}
127
128
129//
130// Calculation of center of rings
131//
132Double_t sqr(Double_t x) { return x*x; }
133
134void swap(int *m, int *n)
135{
136 int dummy = *m;
137 *m = *n;
138 *n = dummy;
139}
140
141bool CaosFilter::CalcCenter(Float_t *px, Float_t *py, Float_t &cx,
142 Float_t &cy, Float_t &R)
143{
144 int i=0;
145 int j=1;
146 int k=2;
147
148 Float_t h1 = py[i]-py[j];
149
150 if (h1==0)
151 {
152 swap(&j, &k);
153 h1 = py[i]-py[j];
154 if (h1==0)
155 {
156 cout << "h1==0" <<endl;
157 return kFALSE;
158 }
159 }
160
161 Float_t h2 = py[j]-py[k];
162
163 if (h2==0)
164 {
165 swap(&i, &j);
166 h2 = py[j]-py[k];
167 if (h2==0)
168 {
169 cout << "h2==0" << endl;
170 return kFALSE;
171 }
172 }
173
174 Float_t w1 = px[i]-px[j];
175 Float_t w2 = px[j]-px[k];
176
177 Float_t m1 = -w1/h1;
178 Float_t m2 = -w2/h2;
179
180 if (m2-m1==0)
181 {
182 cout << "m2-m1==0" << endl;
183 return kFALSE;
184 }
185
186 cx = ((m2*(px[j]+px[k]) +py[i]-py[k] -m1*(px[i]+px[j]))/(m2-m1)/2);
187 cy = ((m2*(py[i]+py[j]) +m1*m2*(px[k]-px[i])-m1*(py[j]+py[k]))/(m2-m1)/2);
188
189 R = sqrt(sqr(cx-px[i])+sqr(cy-py[i]));
190
191 return kTRUE;
192}
193
194//
195// Interpolation of centers of rings
196//
197void CaosFilter::InterpolCenter(int &m, Float_t *x, Float_t *y, Float_t &px,
198 Float_t &py, Float_t &pr, Float_t &sx,
199 Float_t &sy, Float_t &sr)
200{
201 int nPoints = m;
202 int mn[10]={0,0,0,1,4,10,20,35,56,84};
203 int nRings = mn[m];
204 Float_t rx[10];
205 Float_t ry[10];
206 Float_t rr[10];
207 px = 0;
208 py = 0;
209 pr = 0;
210
211 int n=0;
212 for (int i=0; i<nPoints-2; i++)
213 for (int j=i+1; j<nPoints-1; j++)
214 for (int k=j+1; k<nPoints; k++)
215 {
216 rx[n]=0;
217 ry[n]=0;
218 rr[n]=0;
219
220 Float_t xx[3] = { x[i], x[j], x[k] };
221 Float_t yy[3] = { y[i], y[j], y[k] };
222 CalcCenter(xx, yy, rx[n], ry[n], rr[n]);
223
224 px += rx[n];
225 py += ry[n];
226 pr += rr[n];
227
228 n++;
229 }
230
231 px /= n;
232 py /= n;
233 pr /= n;
234
235 //
236 // deviation of x- and y coordinate and radius
237 //
238 Float_t sumx=0;
239 Float_t sumy=0;
240 Float_t sumr=0;
241
242 for (n=0; n<nRings; n++)
243 {
244 sumx += sqr(rx[n]-px);
245 sumy += sqr(ry[n]-py);
246 sumr += sqr(rr[n]-pr);
247
248 }
249 sx=sqrt(sumx)/(nRings-1);
250 sy=sqrt(sumy)/(nRings-1);
251 sr=sqrt(sumr)/(nRings-1);
252}
253
254
255
256//
257//Calculation of rings
258//
259
260Double_t kConv = 0.502; // [pix/mm]
261
262void CaosFilter::CalcRings(int &m, Float_t *x, Float_t *y, Float_t &px,
263 Float_t &py, Float_t &pr, float *v, float *w)
264{
265
266// Float_t v[5];
267// Float_t w[5];
268 Float_t s[5];
269 Float_t sx;
270 Float_t sy;
271 Float_t sr;
272
273 for (int i=0; i<6; i++)
274 {
275 x[i] /= kConv;
276 y[i] /= kConv;
277 }
278
279 InterpolCenter(m, x, y, px, py, pr, sx, sy, sr);
280
281 //
282 // angles v and relative angle w
283 //
284 for (int j=0; j<6; j++)
285 {
286 v[j] = atan2(y[j]-py, x[j]-px)*180/TMath::Pi();
287 }
288
289 w[0]=v[0]+167.11;
290 w[1]=v[1]-94.31;
291 w[2]=v[2]+87;
292 w[3]=v[3]+56.58;
293 w[4]=v[4]-33.48;
294
295 //
296 // distances between the LEDs
297 //
298 s[0]=sqrt(sqr(x[1]-x[0]) + sqr(y[1]-y[0]));///41.6;
299 s[1]=sqrt(sqr(x[1]-x[4]) + sqr(y[1]-y[4]));///27.7;
300 s[2]=sqrt(sqr(x[3]-x[4]) + sqr(y[3]-y[4]));///39.1;
301 s[3]=sqrt(sqr(x[2]-x[3]) + sqr(y[2]-y[3]));///14.4;
302 s[4]=sqrt(sqr(x[2]-x[0]) + sqr(y[2]-y[0]));///35.2;
303
304}
305
306//
307// filter LEDs from spots
308//
309int CaosFilter::FilterLeds(float *xw, float *yw, float *xl, float *yl)
310{
311 int n=0;
312 int m=0;
313
314 for (n=0; n<10; n++)
315 {
316 if (!(xw[n]>250 && xw[n]<540 && yw[n]>140 && yw[n]<500))
317 continue;
318
319 if (n>0 && xw[n]>(xl[m-1]-3) && xw[n]<(xl[m-1]+3))
320 continue;
321
322 xl[m]=xw[n];
323 yl[m]=yw[n];
324 m++;
325 }
326 return m;
327}
328
329void CaosFilter::FilterLeds(Leds &leds)
330{
331 for (int n=0; n<leds.GetEntries(); n++)
332 {
333 const Led &led = leds(n);
334
335 const Double_t x = led.GetX()-480;
336 const Double_t y = led.GetY()-200;
337
338 const Double_t r2 = x*x+y*y;
339
340 if (r2<80*80 || r2>145*145)
341 {
342 leds.RemoveAt(n);
343 continue;
344 }
345 }
346
347 TIter Next1(&leds);
348
349 Led *led1 = NULL;
350
351 while ((led1=(Led*)Next1()))
352 {
353 Led *led2 = NULL;
354 TIter Next2(&leds);
355
356 const Double_t x1 = led1->GetX();
357 const Double_t y1 = led1->GetY();
358
359 while ((led2=(Led*)Next2()))
360 {
361 if (led1==led2)
362 continue;
363
364 const Double_t x2 = led2->GetX();
365 const Double_t y2 = led2->GetY();
366
367 const Double_t dx = x2-x1;
368 const Double_t dy = y2-y1;
369
370 if (dx*dx+dy*dy<5*5)
371 {
372 // FIXME: Interpolation
373 leds.Remove(led2);
374 }
375 }
376 }
377}
378
379
380
381void CaosFilter::Execute(byte *img, float *xw, float *yw, float *xl,
382 float *yl, float &prx, float &pry, float &pr, float *v, float *w)
383{
384 const int offset = 10;
385
386 int max;
387 int min;
388
389 const float mean = Mean(img, offset, &min, &max);
390 const float sdev = SDev(img, offset, mean);
391
392 const float cut = mean + 2.5*sdev;
393
394 //
395 // clean image from noise
396 //
397 for (int x=0; x<768; x++)
398 for (int y=0; y<576; y++)
399 {
400 if (img[y*768+x]>cut)
401 continue;
402
403 //
404 // FIXME: Create LOOKUP Table!
405 //
406
407 img[y*768+x] = 0;
408 }
409
410 //
411 // find mean points
412 //
413 const int maxpnt = 0x1000;
414
415 int pos[maxpnt+1][2]; // FIXME
416 int cnt = 0;
417
418 for (int x=offset; x<768-offset; x++)
419 {
420 for (int y=offset; y<576-offset; y++)
421 {
422 if (img[x+768*y]==0)
423 continue;
424
425 const int ipos = GetMeanPosition(img, x, y, 5);
426
427 int j;
428 for (j=0; j<cnt; j++)
429 {
430 if (pos[j][0]==ipos)
431 {
432 if (pos[j][1] < 0xf0)
433 pos[j][1] += 0x10;
434 break;
435 }
436 }
437 if (cnt && j<cnt)
438 continue;
439
440 pos[cnt][0] = ipos; // pixel number
441 pos[cnt][1] = 0x10; // magnitude
442
443 cnt++;
444
445 if (cnt==maxpnt)
446 break;
447 }
448 if (cnt==maxpnt)
449 {
450 cout << "Error! More than " << maxpnt << " stars found." << endl;
451 break;
452 }
453 }
454
455
456 //
457 // Draw marker for found stars into picture
458 //
459 int points=0;
460
461 byte marker[768*576];
462 memset(marker, 0, 768*576);
463
464 for (int i=0; i<cnt; i++)
465 {
466 if (pos[i][1]<=0xa0)
467 continue;
468
469 const int px = pos[i][0]%768;
470 const int py = pos[i][0]/768;
471
472 MarkPoint(px, py, marker, pos[i][1]);
473
474 Float_t mx, my;
475 GetMeanPosition(img, px, py, 5, mx, my);
476
477 //
478 // Write positions in array
479 //
480 // cout << mx << " " << my << " " << cnt << endl;
481 // cout << "points:" << points << endl;
482
483 xw[points]=mx;
484 yw[points]=my;
485
486 points++;
487 }
488
489 int m = FilterLeds(xw, yw, xl, yl);
490 CalcRings(m, xl, yl, prx, pry, pr, v, w);
491
492 //
493 // Copy markers into image
494 //
495 for (int x=0; x<768*576; x++)
496 {
497 if (!marker[x])
498 continue;
499
500 img[x]=marker[x];
501 }
502}
503
504void CaosFilter::Execute(byte *img, Leds &leds, Double_t conv)
505{
506 const int offset = 10;
507
508 int max;
509 int min;
510
511 const float mean = Mean(img, offset, &min, &max);
512 const float sdev = SDev(img, offset, mean);
513
514 const float cut = mean + 2.5*sdev;
515
516 //
517 // clean image from noise
518 //
519 for (int x=0; x<768; x++)
520 for (int y=0; y<576; y++)
521 {
522 if (img[y*768+x]>cut)
523 continue;
524
525 //
526 // FIXME: Create LOOKUP Table!
527 //
528
529 img[y*768+x] = 0;
530 }
531
532 //
533 // find mean points
534 //
535 const int maxpnt = 0x1000;
536
537 int pos[maxpnt+1][2]; // FIXME
538 int cnt = 0;
539
540 for (int x=offset; x<768-offset; x++)
541 {
542 for (int y=offset; y<576-offset; y++)
543 {
544 if (img[x+768*y]==0)
545 continue;
546
547 const int ipos = GetMeanPosition(img, x, y, 5);
548
549 int j;
550 for (j=0; j<cnt; j++)
551 {
552 if (pos[j][0]==ipos)
553 {
554 if (pos[j][1] < 0xf0)
555 pos[j][1] += 0x10;
556 break;
557 }
558 }
559 if (cnt && j<cnt)
560 continue;
561
562 pos[cnt][0] = ipos; // pixel number
563 pos[cnt][1] = 0x10; // magnitude
564
565 cnt++;
566
567 if (cnt==maxpnt)
568 break;
569 }
570 if (cnt==maxpnt)
571 {
572 cout << "Error! More than " << maxpnt << " stars found." << endl;
573 break;
574 }
575 }
576
577
578 //
579 // Draw marker for found stars into picture
580 //
581 int points=0;
582
583 byte marker[768*576];
584 memset(marker, 0, 768*576);
585
586 for (int i=0; i<cnt; i++)
587 {
588 if (pos[i][1]<=0x80) // 0xa0
589 continue;
590
591 const int px = pos[i][0]%768;
592 const int py = pos[i][0]/768;
593
594 MarkPoint(px, py, marker, pos[i][1]);
595
596 Float_t mx, my;
597 GetMeanPosition(img, px, py, 5, mx, my);
598
599 //
600 // Write positions in array
601 //
602 // cout << mx << " " << my << " " << cnt << endl;
603 // cout << "points:" << points << endl;
604
605 leds.Set(points++, mx/conv, my/conv, 0, 0, pos[i][1]);
606 }
607
608 //
609 // Copy markers into image
610 //
611 for (int x=0; x<768*576; x++)
612 {
613 if (!marker[x])
614 continue;
615
616 img[x]=marker[x];
617 }
618}
619
620
Note: See TracBrowser for help on using the repository browser.