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

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