00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "cmvision.h"
00035 #include <string.h>
00036 #include <strings.h>
00037
00038
00039
00040
00041
00042
00043 inline int range_sum(int x,int w)
00044 {
00045 return(w*(2*x + w-1) / 2);
00046 }
00047
00048
00049 template <class num>
00050 inline num max(num a,num b)
00051 {
00052 return((a > b)? a : b);
00053 }
00054
00055
00056 template <class num>
00057 inline num min(num a,num b)
00058 {
00059 return((a < b)? a : b);
00060 }
00061
00062
00063 int log2modp[37] = {
00064 0, 1, 2,27, 3,24,28, 0, 4,17,25,31,29,12, 0,14, 5, 8,18,
00065 0,26,23,32,16,30,11,13, 7, 0,22,15,10, 6,21, 9,20,19
00066 };
00067
00068 template <class num>
00069 inline int bottom_bit(num n)
00070 {
00071 return(log2modp[(n & -n) % 37]);
00072 }
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 template <class num>
00087 inline num top_bit(num n)
00088 {
00089 int i = 1;
00090 if(!n) return(0);
00091 while(n>>i) i++;
00092 return(i);
00093 }
00094
00095
00096
00097
00098 void CMVision::classifyFrame(image_pixel * restrict img,unsigned * restrict map)
00099
00100
00101 {
00102 int i,m,s;
00103 int m1,m2;
00104 image_pixel p;
00105
00106 unsigned *uclas = u_class;
00107 unsigned *vclas = v_class;
00108 unsigned *yclas = y_class;
00109
00110 s = width * height;
00111
00112 if(options & CMV_DUAL_THRESHOLD){
00113 for(i=0; i<s; i+=2){
00114 p = img[i/2];
00115 m = uclas[p.u] & vclas[p.v];
00116 m1 = m & yclas[p.y1];
00117 m2 = m & yclas[p.y2];
00118 map[i + 0] = m1 | (m1 >> 16);
00119 map[i + 1] = m2 | (m2 >> 16);
00120 }
00121 }else{
00122 for(i=0; i<s; i+=2){
00123 p = img[i/2];
00124 m = uclas[p.u] & vclas[p.v];
00125 map[i + 0] = m & yclas[p.y1];
00126 map[i + 1] = m & yclas[p.y2];
00127 }
00128 }
00129 }
00130
00131 int CMVision::encodeRuns(rle * restrict out,unsigned * restrict map)
00132
00133
00134
00135 {
00136 int x,y,j,l;
00137 unsigned m,save;
00138 int size;
00139 unsigned *row;
00140 rle r;
00141
00142 size = width * height;
00143
00144
00145 save = map[0];
00146
00147 j = 0;
00148 for(y=0; y<height; y++){
00149 row = &map[y * width];
00150
00151
00152
00153 row[0] = save;
00154 save = row[width];
00155 row[width] = CMV_NONE;
00156
00157 x = 0;
00158 while(x < width){
00159 m = row[x];
00160
00161 l = x;
00162 while(row[x] == m) x++;
00163
00164
00165 r.color = m;
00166 r.length = x - l;
00167 r.parent = j;
00168 out[j++] = r;
00169 if(j >= CMV_MAX_RUNS) return(0);
00170 }
00171 }
00172
00173 return(j);
00174 }
00175
00176 void CMVision::connectComponents(rle * restrict map,int num)
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187 {
00188 int x1,x2;
00189 int l1,l2;
00190 rle r1,r2;
00191 int i,p,s,n;
00192
00193 l1 = l2 = 0;
00194 x1 = x2 = 0;
00195
00196
00197 while(x1 < width){
00198 x1 += map[l1++].length;
00199 }
00200 x1 = 0;
00201
00202
00203 r1 = map[l1];
00204 r2 = map[l2];
00205 s = l1;
00206 while(l1 < num){
00207 if(r1.color==r2.color && r1.color){
00208 if((x1>=x2 && x1<x2+r2.length) || (x2>=x1 && x2<x1+r1.length)){
00209 if(s != l1){
00210 map[l1].parent = r1.parent = r2.parent;
00211 s = l1;
00212 }else{
00213
00214 n = r1.parent;
00215 while(n != map[n].parent) n = map[n].parent;
00216 p = r2.parent;
00217 while(p != map[p].parent) p = map[p].parent;
00218
00219
00220 if(n < p){
00221 map[p].parent = n;
00222 }else{
00223 map[n].parent = p;
00224 }
00225 }
00226 }
00227 }
00228
00229
00230 if(x1+r1.length < x2+r2.length){
00231 x1 += r1.length;
00232 r1 = map[++l1];
00233 }else{
00234 x2 += r2.length;
00235 r2 = map[++l2];
00236 }
00237 }
00238
00239
00240 for(i=0; i<num; i++){
00241 p = map[i].parent;
00242 if(p > i){
00243 while(p != map[p].parent) p = map[p].parent;
00244 map[i].parent = p;
00245 }else{
00246 map[i].parent = map[p].parent;
00247 }
00248 }
00249
00250
00251 }
00252
00253 int CMVision::extractRegions(region * restrict reg,rle * restrict rmap,int num)
00254
00255
00256
00257
00258
00259 {
00260 int x,y,i;
00261 int b,n,a;
00262 rle r;
00263 yuv black = {0,0,0};
00264
00265 x = y = n = 0;
00266 for(i=0; i<num; i++){
00267 r = rmap[i];
00268
00269 if(r.color){
00270 if(r.parent == i){
00271
00272 rmap[i].parent = b = n;
00273 reg[b].color = bottom_bit(r.color) - 1;
00274 reg[b].area = r.length;
00275 reg[b].x1 = x;
00276 reg[b].y1 = y;
00277 reg[b].x2 = x + r.length;
00278 reg[b].y2 = y;
00279 reg[b].sum_x = range_sum(x,r.length);
00280 reg[b].sum_y = y * r.length;
00281 reg[b].average = black;
00282
00283 n++;
00284 if(n >= CMV_MAX_REGIONS) return(CMV_MAX_REGIONS);
00285 }else{
00286
00287 b = rmap[r.parent].parent;
00288 rmap[i].parent = b;
00289 reg[b].area += r.length;
00290 reg[b].x2 = max(x + r.length,reg[b].x2);
00291 reg[b].x1 = min(x,reg[b].x1);
00292 reg[b].y2 = y;
00293 reg[b].sum_x += range_sum(x,r.length);
00294 reg[b].sum_y += y * r.length;
00295 }
00296
00297
00298
00299
00300
00301 }
00302
00303
00304 x = (x + r.length) % width;
00305 y += (x == 0);
00306 }
00307
00308
00309
00310
00311 for(i=0; i<n; i++){
00312 a = reg[i].area;
00313 reg[i].cen_x = (float)reg[i].sum_x / a;
00314 reg[i].cen_y = (float)reg[i].sum_y / a;
00315 }
00316
00317 return(n);
00318 }
00319
00320 void CMVision::calcAverageColors(region * restrict reg,int num_reg,
00321 image_pixel * restrict img,
00322 rle * restrict rmap,int num_runs)
00323
00324
00325
00326
00327
00328 {
00329 int i,j,x,l;
00330 image_pixel p;
00331 rle r;
00332 int sum_y,sum_u,sum_v;
00333 int b,xs;
00334
00335 yuv avg;
00336 int area;
00337
00338
00339 for(i=0; i<num_reg; i++){
00340 reg[i].sum_x = 0;
00341 reg[i].sum_y = 0;
00342 reg[i].sum_z = 0;
00343 }
00344
00345 x = 0;
00346
00347
00348
00349
00350 for(i=0; i<num_runs; i++){
00351 r = rmap[i];
00352 l = r.length;
00353
00354 if(!r.color){
00355 x += l;
00356 }else{
00357 xs = x;
00358 p = img[x / 2];
00359
00360 if(x & 1){
00361 sum_y = p.y2;
00362 sum_u = p.u;
00363 sum_v = p.v;
00364
00365 x++;
00366 l--;
00367 }else{
00368 sum_y = sum_u = sum_v = 0;
00369 area = 0;
00370 }
00371
00372 for(j=0; j<l/2; j++){
00373 p = img[x / 2];
00374 sum_y += p.y1 + p.y2;
00375 sum_u += 2 * p.u;
00376 sum_v += 2 * p.v;
00377 x+=2;
00378
00379 }
00380
00381 if(l & 1){
00382 x++;
00383 p = img[x / 2];
00384 sum_y += p.y1;
00385 sum_u += p.u;
00386 sum_v += p.v;
00387
00388 }
00389
00390
00391 b = r.parent;
00392 reg[b].sum_x += sum_y;
00393 reg[b].sum_y += sum_u;
00394 reg[b].sum_z += sum_v;
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407 x = xs + r.length;
00408 }
00409 }
00410
00411
00412 for(i=0; i<num_reg; i++){
00413 area = reg[i].area;
00414 avg.y = reg[i].sum_x / area;
00415 avg.u = reg[i].sum_y / area;
00416 avg.v = reg[i].sum_z / area;
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434 reg[i].average = avg;
00435 }
00436 }
00437
00438 int CMVision::separateRegions(region * restrict reg,int num)
00439
00440
00441
00442
00443 {
00444 region *p;
00445 int i,l;
00446 int area,max_area;
00447
00448
00449 for(i=0; i<CMV_MAX_COLORS; i++){
00450 region_count[i] = 0;
00451 region_list[i] = NULL;
00452 }
00453
00454
00455
00456 max_area = 0;
00457 for(i=0; i<num; i++){
00458 p = ®[i];
00459 area = p->area;
00460 if(area >= CMV_MIN_AREA){
00461 if(area > max_area) max_area = area;
00462 l = p->color;
00463 region_count[l]++;
00464 p->next = region_list[l];
00465 region_list[l] = p;
00466 }
00467 }
00468
00469 return(max_area);
00470 }
00471
00472
00473
00474
00475
00476
00477 #define CMV_RBITS 6
00478 #define CMV_RADIX (1 << CMV_RBITS)
00479 #define CMV_RMASK (CMV_RADIX-1)
00480
00481 CMVision::region *CMVision::sortRegionListByArea(region * restrict list,int passes)
00482
00483
00484 {
00485 region *tbl[CMV_RADIX],*p,*pn;
00486 int slot,shift;
00487 int i,j;
00488
00489
00490 if(!list || !list->next) return(list);
00491
00492
00493 for(j=0; j<CMV_RADIX; j++) tbl[j] = NULL;
00494
00495 for(i=0; i<passes; i++){
00496
00497 shift = CMV_RBITS * i;
00498 p = list;
00499 while(p){
00500 pn = p->next;
00501 slot = ((p->area) >> shift) & CMV_RMASK;
00502 p->next = tbl[slot];
00503 tbl[slot] = p;
00504 p = pn;
00505 }
00506
00507
00508 list = NULL;
00509 for(j=0; j<CMV_RADIX; j++){
00510 p = tbl[j];
00511 tbl[j] = NULL;
00512 while(p){
00513 pn = p->next;
00514 p->next = list;
00515 list = p;
00516 p = pn;
00517 }
00518 }
00519 }
00520
00521 return(list);
00522 }
00523
00524 void CMVision::sortRegions(int max_area)
00525
00526
00527 {
00528 int i,p;
00529
00530
00531 p = top_bit((max_area + CMV_RBITS-1) / CMV_RBITS);
00532
00533
00534 for(i=0; i<CMV_MAX_COLORS; i++){
00535 region_list[i] = sortRegionListByArea(region_list[i],p);
00536 }
00537 }
00538
00539 int CMVision::mergeRegions(region *p,int num,double density_thresh)
00540
00541
00542
00543
00544
00545 {
00546 region *q,*s;
00547 int l,r,t,b;
00548 int a;
00549 int merged;
00550
00551
00552
00553 merged = 0;
00554
00555 while(p && merged<num){
00556 q = p->next;
00557 s = p;
00558
00559 while(q){
00560
00561 l = min(p->x1,q->x1);
00562 r = max(p->x2,q->x2);
00563 t = min(p->y1,q->y1);
00564 b = max(p->y2,q->y2);
00565 a = (r-l) * (b-t);
00566
00567
00568 if((double)(p->area + q->area) / a > density_thresh){
00569
00570 a = p->area + q->area;
00571 p->x1 = l;
00572 p->x2 = r;
00573 p->y1 = t;
00574 p->y2 = b;
00575 p->cen_x = ((p->cen_x * p->area) + (q->cen_x * q->area)) / a;
00576 p->cen_y = ((p->cen_y * p->area) + (q->cen_y * q->area)) / a;
00577 p->area = a;
00578
00579
00580 q = q->next;
00581 s->next = q;
00582 merged++;
00583 }else{
00584 s = q;
00585 q = q->next;
00586 }
00587 }
00588 p = p->next;
00589 }
00590
00591 return(merged);
00592 }
00593
00594 int CMVision::mergeRegions()
00595
00596 {
00597 int i,m;
00598 int num;
00599
00600 num = 0;
00601
00602 for(i=0; i<CMV_MAX_COLORS; i++){
00603 m = mergeRegions(region_list[i],colors[i].expected_num,colors[i].merge);
00604 region_count[i] -= m;
00605 num += m;
00606 }
00607
00608 return(num);
00609 }
00610
00611
00612
00613 #define ZERO(x) memset(x,0,sizeof(x))
00614
00615 void CMVision::clear()
00616 {
00617 ZERO(y_class);
00618 ZERO(u_class);
00619 ZERO(v_class);
00620
00621 ZERO(region_list);
00622 ZERO(region_count);
00623
00624 ZERO(colors);
00625
00626 map = NULL;
00627 }
00628
00629 bool CMVision::initialize(int nwidth,int nheight)
00630
00631 {
00632 width = nwidth;
00633 height = nheight;
00634
00635 if(map) delete(map);
00636
00637 map = new unsigned[width * height + 1];
00638
00639
00640 options = CMV_THRESHOLD;
00641
00642 return(map != NULL);
00643 }
00644
00645
00646 template <class num>
00647 void set_bits(num *arr,int len,int l,int r,num k)
00648 {
00649 int i;
00650
00651 l = max(l,0);
00652 r = min(r+1,len);
00653
00654 for(i=l; i<r; i++) arr[i] |= k;
00655 }
00656
00657 template <class num>
00658 void clear_bits(num *arr,int len,int l,int r,num k)
00659 {
00660 int i;
00661
00662 l = max(l,0);
00663 r = min(r+1,len);
00664
00665 k = ~k;
00666 for(i=l; i<r; i++) arr[i] &= k;
00667 }
00668
00669 #define CMV_STATE_SCAN 0
00670 #define CMV_STATE_COLORS 1
00671 #define CMV_STATE_THRESH 2
00672 #define CMV_MAX_BUF 256
00673
00674 bool CMVision::loadOptions(char *filename)
00675
00676
00677 {
00678 char buf[CMV_MAX_BUF],str[CMV_MAX_BUF];
00679 FILE *in;
00680 int state,i,n;
00681
00682 int r,g,b;
00683 int exp_num;
00684 double merge;
00685 color_info *c;
00686
00687 int y1,y2,u1,u2,v1,v2;
00688 unsigned k;
00689
00690
00691 in = fopen(filename,"rt");
00692 if(!in) return(false);
00693
00694
00695 for(i=0; i<CMV_COLOR_LEVELS; i++){
00696 y_class[i] = u_class[i] = v_class[i] = 0;
00697 }
00698 for(i=0; i<CMV_MAX_COLORS; i++){
00699 if(colors[i].name){
00700 delete(colors[i].name);
00701 colors[i].name = NULL;
00702 }
00703 }
00704
00705
00706 state = 0;
00707 while(fgets(buf,CMV_MAX_BUF,in)){
00708 switch(state){
00709 case CMV_STATE_SCAN:
00710 n = sscanf(buf,"[%s",str);
00711 if(n == 1){
00712 if(!strncasecmp(str,"colors]",CMV_MAX_BUF)){
00713 state = CMV_STATE_COLORS;
00714 i = 0;
00715 }else if(!strncasecmp(str,"thresholds]",CMV_MAX_BUF)){
00716 state = CMV_STATE_THRESH;
00717 i = 0;
00718 }else{
00719 printf("CMVision: Ignoring unknown option header '%s'.\n",str);
00720 }
00721 }
00722 break;
00723 case CMV_STATE_COLORS:
00724 n = sscanf(buf,"(%d,%d,%d) %lf %d %s",&r,&g,&b,&merge,&exp_num,str);
00725 if(n == 6){
00726
00727
00728 if(i < CMV_MAX_COLORS){
00729 c = &colors[i];
00730 c->color.red = r;
00731 c->color.green = g;
00732 c->color.blue = b;
00733 c->name = strdup(str);
00734 c->merge = merge;
00735 c->expected_num = exp_num;
00736 i++;
00737 }else{
00738 printf("CMVision: Too many colors, ignoring '%s'.\n",str);
00739 }
00740 }else if(n == 0){
00741 state = CMV_STATE_SCAN;
00742 }
00743 break;
00744 case CMV_STATE_THRESH:
00745 n = sscanf(buf,"(%d:%d,%d:%d,%d:%d)",&y1,&y2,&u1,&u2,&v1,&v2);
00746 if(n == 6){
00747
00748 if(i < CMV_MAX_COLORS){
00749 c = &colors[i];
00750 c->y_low = y1; c->y_high = y2;
00751 c->u_low = u1; c->u_high = u2;
00752 c->v_low = v1; c->v_high = v2;
00753
00754 k = (1 << i);
00755 set_bits(y_class,CMV_COLOR_LEVELS,y1,y2,k);
00756 set_bits(u_class,CMV_COLOR_LEVELS,u1,u2,k);
00757 set_bits(v_class,CMV_COLOR_LEVELS,v1,v2,k);
00758 i++;
00759 }else{
00760 printf("CMVision: Too many thresholds.\n");
00761 }
00762 }else if(n == 0){
00763 state = CMV_STATE_SCAN;
00764 }
00765 break;
00766 }
00767 }
00768
00769
00770
00771
00772
00773
00774
00775 fclose(in);
00776
00777 return(true);
00778 }
00779
00780 bool CMVision::saveOptions(char *filename)
00781 {
00782 color_info *c;
00783 FILE *out;
00784 int i;
00785
00786 out = fopen(filename,"wt");
00787 if(!out) return(false);
00788
00789 fprintf(out,"[Colors]\n");
00790 i = 0;
00791 while(colors[i].name){
00792 c = &colors[i];
00793 fprintf(out,"(%3d,%3d,%3d) %6.4f %d %s\n",
00794 c->color.red,c->color.green,c->color.blue,
00795 c->merge,c->expected_num,c->name);
00796 i++;
00797 }
00798
00799 fprintf(out,"\n[Thresholds]\n");
00800 i = 0;
00801 while(colors[i].name){
00802 c = &colors[i];
00803 fprintf(out,"(%3d:%3d,%3d:%3d,%3d:%3d)\n",
00804 c->y_low,c->y_high,
00805 c->u_low,c->u_high,
00806 c->v_low,c->v_high);
00807 i++;
00808 }
00809
00810 fclose(out);
00811
00812 return(true);
00813 }
00814
00815 bool CMVision::enable(unsigned opt)
00816 {
00817 unsigned int valid;
00818
00819 valid = opt & CMV_VALID_OPTIONS;
00820 options |= valid;
00821
00822 return(opt == valid);
00823 }
00824
00825 bool CMVision::disable(unsigned opt)
00826 {
00827 unsigned int valid;
00828
00829 valid = opt & CMV_VALID_OPTIONS;
00830 options &= ~valid;
00831
00832 return(opt == valid);
00833 }
00834
00835 void CMVision::close()
00836 {
00837 if(map) delete(map);
00838 map = NULL;
00839 }
00840
00841
00842
00843
00844 bool CMVision::testClassify(rgb * restrict out,image_pixel * restrict image)
00845 {
00846 int i,s;
00847 rgb black = {0,0,0};
00848
00849 if(!image || !out) return(false);
00850
00851 classifyFrame(image,map);
00852
00853 s = width * height;
00854
00855 i = 0;
00856 while(i < s){
00857 while(i<s && !map[i]){
00858 out[i] = black;
00859 i++;
00860 }
00861 while(i<s && map[i]){
00862 out[i] = colors[bottom_bit(map[i])-1].color;
00863 i++;
00864 }
00865 }
00866
00867 return(true);
00868 }
00869
00870 bool CMVision::getThreshold(int color,
00871 int &y_low,int &y_high,
00872 int &u_low,int &u_high,
00873 int &v_low,int &v_high)
00874 {
00875 color_info *c;
00876
00877 if(color<0 || color>=CMV_MAX_COLORS) return(false);
00878
00879 c = &colors[color];
00880 y_low = c->y_low; y_high = c->y_high;
00881 u_low = c->u_low; u_high = c->u_high;
00882 v_low = c->v_low; v_high = c->v_high;
00883
00884 return(true);
00885 }
00886
00887 bool CMVision::setThreshold(int color,
00888 int y_low,int y_high,
00889 int u_low,int u_high,
00890 int v_low,int v_high)
00891 {
00892 color_info *c;
00893 unsigned k;
00894
00895 if(color<0 || color>=CMV_MAX_COLORS) return(false);
00896
00897 c = &colors[color];
00898 k = 1 << color;
00899
00900 clear_bits(y_class,CMV_COLOR_LEVELS,c->y_low,c->y_high,k);
00901 clear_bits(u_class,CMV_COLOR_LEVELS,c->u_low,c->u_high,k);
00902 clear_bits(v_class,CMV_COLOR_LEVELS,c->v_low,c->v_high,k);
00903
00904 c->y_low = y_low; c->y_high = y_high;
00905 c->u_low = u_low; c->u_high = u_high;
00906 c->v_low = v_low; c->v_high = v_high;
00907
00908 set_bits(y_class,CMV_COLOR_LEVELS,y_low,y_high,k);
00909 set_bits(u_class,CMV_COLOR_LEVELS,u_low,u_high,k);
00910 set_bits(v_class,CMV_COLOR_LEVELS,v_low,v_high,k);
00911
00912 return(true);
00913 }
00914
00915
00916
00917 bool CMVision::processFrame(image_pixel *image)
00918 {
00919 int runs;
00920 int regions;
00921 int max_area;
00922
00923 if(!image) return(false);
00924
00925 if(options & CMV_THRESHOLD){
00926
00927 classifyFrame(image,map);
00928 runs = encodeRuns(rmap,map);
00929 connectComponents(rmap,runs);
00930
00931 regions = extractRegions(region_table,rmap,runs);
00932
00933 if(options & CMV_COLOR_AVERAGES){
00934 calcAverageColors(region_table,regions,image,rmap,runs);
00935 }
00936
00937 max_area = separateRegions(region_table,regions);
00938 sortRegions(max_area);
00939
00940 if(options & CMV_DENSITY_MERGE){
00941 mergeRegions();
00942 }
00943 }
00944
00945 return(true);
00946 }
00947
00948 bool CMVision::processFrame(unsigned *map)
00949 {
00950 int runs;
00951 int regions;
00952 int max_area;
00953
00954 if(!map) return(false);
00955
00956 runs = encodeRuns(rmap,map);
00957 connectComponents(rmap,runs);
00958
00959 regions = extractRegions(region_table,rmap,runs);
00960
00961
00962
00963
00964
00965 max_area = separateRegions(region_table,regions);
00966 sortRegions(max_area);
00967
00968 if(options & CMV_DENSITY_MERGE){
00969 mergeRegions();
00970 }
00971
00972 return(true);
00973 }
00974
00975 int CMVision::numRegions(int color_id)
00976 {
00977 if(color_id<0 || color_id>=CMV_MAX_COLORS) return(CMV_NONE);
00978 return(region_count[color_id]);
00979 }
00980
00981 CMVision::region *CMVision::getRegions(int color_id)
00982 {
00983 if(color_id<0 || color_id>=CMV_MAX_COLORS) return(NULL);
00984 return(region_list[color_id]);
00985 }