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

Last change on this file since 2256 was 1810, checked in by tbretz, 23 years ago
*** empty log message ***
File size: 12.6 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 RemoveTwins(leds, 5);
348}
349
350void CaosFilter::RemoveTwins(Leds &leds, Double_t radius)
351{
352 TIter Next1(&leds);
353
354 Led *led1 = NULL;
355
356 while ((led1=(Led*)Next1()))
357 {
358 Led *led2 = NULL;
359 TIter Next2(&leds);
360
361 const Double_t x1 = led1->GetX();
362 const Double_t y1 = led1->GetY();
363
364 while ((led2=(Led*)Next2()))
365 {
366 if (led1==led2)
367 continue;
368
369 const Double_t x2 = led2->GetX();
370 const Double_t y2 = led2->GetY();
371
372 const Double_t dx = x2-x1;
373 const Double_t dy = y2-y1;
374
375 if (dx*dx+dy*dy<radius*radius)
376 {
377 // FIXME: Interpolation
378 leds.Remove(led2);
379 }
380 }
381 }
382}
383
384
385
386void CaosFilter::Execute(byte *img, float *xw, float *yw, float *xl,
387 float *yl, float &prx, float &pry, float &pr, float *v, float *w)
388{
389 const int offset = 10;
390
391 int max;
392 int min;
393
394 const float mean = Mean(img, offset, &min, &max);
395 const float sdev = SDev(img, offset, mean);
396
397 const float cut = mean + 2.5*sdev;
398
399 //
400 // clean image from noise
401 //
402 for (int x=0; x<768; x++)
403 for (int y=0; y<576; y++)
404 {
405 if (img[y*768+x]>cut)
406 continue;
407
408 //
409 // FIXME: Create LOOKUP Table!
410 //
411
412 img[y*768+x] = 0;
413 }
414
415 //
416 // find mean points
417 //
418 const int maxpnt = 0x1000;
419
420 int pos[maxpnt+1][2]; // FIXME
421 int cnt = 0;
422
423 for (int x=offset; x<768-offset; x++)
424 {
425 for (int y=offset; y<576-offset; y++)
426 {
427 if (img[x+768*y]==0)
428 continue;
429
430 const int ipos = GetMeanPosition(img, x, y, 5);
431
432 int j;
433 for (j=0; j<cnt; j++)
434 {
435 if (pos[j][0]==ipos)
436 {
437 if (pos[j][1] < 0xf0)
438 pos[j][1] += 0x10;
439 break;
440 }
441 }
442 if (cnt && j<cnt)
443 continue;
444
445 pos[cnt][0] = ipos; // pixel number
446 pos[cnt][1] = 0x10; // magnitude
447
448 cnt++;
449
450 if (cnt==maxpnt)
451 break;
452 }
453 if (cnt==maxpnt)
454 {
455 cout << "Error! More than " << maxpnt << " stars found." << endl;
456 break;
457 }
458 }
459
460
461 //
462 // Draw marker for found stars into picture
463 //
464 int points=0;
465
466 byte marker[768*576];
467 memset(marker, 0, 768*576);
468
469 for (int i=0; i<cnt; i++)
470 {
471 if (pos[i][1]<=0xa0)
472 continue;
473
474 const int px = pos[i][0]%768;
475 const int py = pos[i][0]/768;
476
477 MarkPoint(px, py, marker, pos[i][1]);
478
479 Float_t mx, my;
480 GetMeanPosition(img, px, py, 5, mx, my);
481
482 //
483 // Write positions in array
484 //
485 // cout << mx << " " << my << " " << cnt << endl;
486 // cout << "points:" << points << endl;
487
488 xw[points]=mx;
489 yw[points]=my;
490
491 points++;
492 }
493
494 int m = FilterLeds(xw, yw, xl, yl);
495 CalcRings(m, xl, yl, prx, pry, pr, v, w);
496
497 //
498 // Copy markers into image
499 //
500 for (int x=0; x<768*576; x++)
501 {
502 if (!marker[x])
503 continue;
504
505 img[x]=marker[x];
506 }
507}
508
509void CaosFilter::Execute(byte *img, Leds &leds, Double_t conv)
510{
511 const int offset = 10;
512
513 int max;
514 int min;
515
516 const float mean = Mean(img, offset, &min, &max);
517 const float sdev = SDev(img, offset, mean);
518
519 const float cut = mean + 2.5*sdev;
520
521 //
522 // clean image from noise
523 //
524 for (int x=0; x<768; x++)
525 for (int y=0; y<576; y++)
526 {
527 if (img[y*768+x]>cut)
528 continue;
529
530 //
531 // FIXME: Create LOOKUP Table!
532 //
533
534 img[y*768+x] = 0;
535 }
536
537 //
538 // find mean points
539 //
540 const int maxpnt = 0x1000;
541
542 int pos[maxpnt+1][2]; // FIXME
543 int cnt = 0;
544
545 for (int x=offset; x<768-offset; x++)
546 {
547 for (int y=offset; y<576-offset; y++)
548 {
549 if (img[x+768*y]==0)
550 continue;
551
552 const int ipos = GetMeanPosition(img, x, y, 5);
553
554 int j;
555 for (j=0; j<cnt; j++)
556 {
557 if (pos[j][0]==ipos)
558 {
559 if (pos[j][1] < 0xf0)
560 pos[j][1] += 0x10;
561 break;
562 }
563 }
564 if (cnt && j<cnt)
565 continue;
566
567 pos[cnt][0] = ipos; // pixel number
568 pos[cnt][1] = 0x10; // magnitude
569
570 cnt++;
571
572 if (cnt==maxpnt)
573 break;
574 }
575 if (cnt==maxpnt)
576 {
577 cout << "Error! More than " << maxpnt << " stars found." << endl;
578 break;
579 }
580 }
581
582
583 //
584 // Draw marker for found stars into picture
585 //
586 int points=0;
587
588 byte marker[768*576];
589 memset(marker, 0, 768*576);
590
591 for (int i=0; i<cnt; i++)
592 {
593 if (pos[i][1]<=0x80) // 0xa0
594 continue;
595
596 const int px = pos[i][0]%768;
597 const int py = pos[i][0]/768;
598
599 MarkPoint(px, py, marker, pos[i][1]);
600
601 Float_t mx, my;
602 GetMeanPosition(img, px, py, 5, mx, my);
603
604 //
605 // Write positions in array
606 //
607 // cout << mx << " " << my << " " << cnt << endl;
608 // cout << "points:" << points << endl;
609
610 leds.Set(points++, mx/conv, my/conv, 0, 0, pos[i][1]);
611 }
612
613 //
614 // Copy markers into image
615 //
616 for (int x=0; x<768*576; x++)
617 {
618 if (!marker[x])
619 continue;
620
621 img[x]=marker[x];
622 }
623}
624
625
Note: See TracBrowser for help on using the repository browser.