cmvision.cc

机器人仿真平台,和stage配合运行

  for(i=0; i<passes; i++){
    // split list into buckets
    shift = CMV_RBITS * i;
    p = list;
    while(p){
      pn = p->next;
      slot = ((p->area) >> shift) & CMV_RMASK;
      p->next = tbl[slot];
      tbl[slot] = p;
      p = pn;
    }

    // integrate back into partially ordered list
    list = NULL;
    for(j=0; j<CMV_RADIX; j++){
      p = tbl[j];
      tbl[j] = NULL;  // clear out table for next pass
      while(p){
        pn = p->next;
        p->next = list;
        list = p;
        p = pn;
      }
    }
  }

  return(list);
}

void CMVision::sortRegions(int max_area)
// Sorts entire region table by area, using the above
// function to sort each threaded region list.
{
  int i,p;

  // do minimal number of passes sufficient to touch all set bits
  p = top_bit((max_area + CMV_RBITS-1) / CMV_RBITS);

  // sort each list
  for(i=0; i<CMV_MAX_COLORS; i++){
    region_list[i] = sortRegionListByArea(region_list[i],p);
  }
}

int CMVision::mergeRegions(region *p,int num,double density_thresh)
// Looks through regions and merges pairs of the same color that would
// have a high density after combining them (where density is the area
// in pixels of the region divided by the bounding box area).  This
// implementation sucks, and I promise real spatial data structures in
// the future so n^2 ugliness like this is not necessary.
{
  region *q,*s;
  int l,r,t,b;
  int a;
  int merged;

  //double tmp;

  merged = 0;

  while(p && merged<num){
    q = p->next;
    s = p;

    while(q){
      // find union box and get its total area
      l = min(p->x1,q->x1);
      r = max(p->x2,q->x2);
      t = min(p->y1,q->y1);
      b = max(p->y2,q->y2);
      a = (r-l) * (b-t);

      // if density of merged region is still above threshold
      if((double)(p->area + q->area) / a > density_thresh){
	// merge them to create a new region
	a = p->area + q->area;
	p->x1 = l;
	p->x2 = r;
	p->y1 = t;
	p->y2 = b;
	p->cen_x = ((p->cen_x * p->area) + (q->cen_x * q->area)) / a;
	p->cen_y = ((p->cen_y * p->area) + (q->cen_y * q->area)) / a;
	p->area = a;

	// remove q from list (old smaller region)
	q = q->next;
	s->next = q;
	merged++;
      }else{
	s = q;
	q = q->next;
      }
    }
    p = p->next;
  }

  return(merged);
}

int CMVision::mergeRegions()
// Apply merge operation to all regions using the above function.
{
  int i,m;
  int num;

  num = 0;

  for(i=0; i<CMV_MAX_COLORS; i++){
    m = mergeRegions(region_list[i],colors[i].expected_num,colors[i].merge);
    region_count[i] -= m;
    num += m;
  }

  return(num);
}

//==== Interface/Public Functions ==================================//

#define ZERO(x) memset(x,0,sizeof(x))

void CMVision::clear()
{
  ZERO(y_class);
  ZERO(u_class);
  ZERO(v_class);

  ZERO(region_list);
  ZERO(region_count);

  ZERO(colors);

  map = NULL;
}

bool CMVision::initialize(int nwidth,int nheight)
// Initializes library to work with images of specified size
{
  width = nwidth;
  height = nheight;

  if(map) delete(map);

  map = new unsigned[width * height + 1];
  // Need 1 extra element to store terminator value in encodeRuns()

  options = CMV_THRESHOLD;

  return(map != NULL);
}

// sets bits in k in array arr[l..r]
template <class num>
void set_bits(num *arr,int len,int l,int r,num k)
{
  int i;

  l = max(l,0);
  r = min(r+1,len);

  for(i=l; i<r; i++) arr[i] |= k;
}

template <class num>
void clear_bits(num *arr,int len,int l,int r,num k)
{
  int i;

  l = max(l,0);
  r = min(r+1,len);

  k = ~k;
  for(i=l; i<r; i++) arr[i] &= k;
}

#define CMV_STATE_SCAN   0
#define CMV_STATE_COLORS 1
#define CMV_STATE_THRESH 2
#define CMV_MAX_BUF 256

bool CMVision::loadOptions(char *filename)
// Loads in options file specifying color names and representative
// rgb triplets.  Also loads in color class threshold values.
{
  char buf[CMV_MAX_BUF],str[CMV_MAX_BUF];
  FILE *in;
  int state,i,n;

  int r,g,b;
  int exp_num;
  double merge;
  color_info *c;

  int y1,y2,u1,u2,v1,v2;
  unsigned k;

  // Open options file
  in = fopen(filename,"rt");
  if(!in) return(false);

  // Clear out previously set options
  for(i=0; i<CMV_COLOR_LEVELS; i++){
    y_class[i] = u_class[i] = v_class[i] = 0;
  }
  for(i=0; i<CMV_MAX_COLORS; i++){
    if(colors[i].name){
      delete(colors[i].name);
      colors[i].name = NULL;
    }
  }

  // Loop ever lines, processing via a simple parser
  state = 0;
  while(fgets(buf,CMV_MAX_BUF,in)){
    switch(state){
      case CMV_STATE_SCAN:
        n = sscanf(buf,"[%s",str);
        if(n == 1){
          if(!strncasecmp(str,"colors]",CMV_MAX_BUF)){
            state = CMV_STATE_COLORS;
            i = 0;
	  }else if(!strncasecmp(str,"thresholds]",CMV_MAX_BUF)){
	    state = CMV_STATE_THRESH;
            i = 0;
	  }else{
            printf("CMVision: Ignoring unknown option header '%s'.\n",str);
          }
	}
        break;
      case CMV_STATE_COLORS:
        n = sscanf(buf,"(%d,%d,%d) %lf %d %s",&r,&g,&b,&merge,&exp_num,str);
        if(n == 6){
          // printf("(%d,%d,%d) %lf %d '%s'\n",
	  //        r,g,b,merge,exp_num,str); fflush(stdout);
          if(i < CMV_MAX_COLORS){
            c = &colors[i];
            c->color.red   = r;
            c->color.green = g;
            c->color.blue  = b;
            c->name  = strdup(str);
            c->merge = merge;
	    c->expected_num = exp_num;
            i++;
	  }else{
	    printf("CMVision: Too many colors, ignoring '%s'.\n",str);
	  }
	}else if(n == 0){
          state = CMV_STATE_SCAN;
        }
        break;
      case CMV_STATE_THRESH:
        n = sscanf(buf,"(%d:%d,%d:%d,%d:%d)",&y1,&y2,&u1,&u2,&v1,&v2);
        if(n == 6){
          // printf("(%d:%d,%d:%d,%d:%d)\n",y1,y2,u1,u2,v1,v2);
          if(i < CMV_MAX_COLORS){
            c = &colors[i];
            c->y_low = y1;  c->y_high = y2;
            c->u_low = u1;  c->u_high = u2;
            c->v_low = v1;  c->v_high = v2;

            k = (1 << i);
            set_bits(y_class,CMV_COLOR_LEVELS,y1,y2,k);
            set_bits(u_class,CMV_COLOR_LEVELS,u1,u2,k);
            set_bits(v_class,CMV_COLOR_LEVELS,v1,v2,k);
            i++;
	  }else{
	    printf("CMVision: Too many thresholds.\n");
	  }
	}else if(n == 0){
          state = CMV_STATE_SCAN;
        }
        break;
    }
  }

  /*
  for(i=0; i<CMV_COLOR_LEVELS; i++){
    printf("%08X %08X %08X\n",y_class[i],u_class[i],v_class[i]);
  }
  */

  fclose(in);

  return(true);
}

bool CMVision::saveOptions(char *filename)
{
  color_info *c;
  FILE *out;
  int i;

  out = fopen(filename,"wt");
  if(!out) return(false);

  fprintf(out,"[Colors]\n");
  i = 0;
  while(colors[i].name){
    c = &colors[i];
    fprintf(out,"(%3d,%3d,%3d) %6.4f %d %s\n",
      c->color.red,c->color.green,c->color.blue,
      c->merge,c->expected_num,c->name);
    i++;
  }

  fprintf(out,"\n[Thresholds]\n");
  i = 0;
  while(colors[i].name){
    c = &colors[i];
    fprintf(out,"(%3d:%3d,%3d:%3d,%3d:%3d)\n",
      c->y_low,c->y_high,
      c->u_low,c->u_high,
      c->v_low,c->v_high);
    i++;
  }

  fclose(out);

  return(true);
}

bool CMVision::enable(unsigned opt)
{
  unsigned int valid;

  valid = opt & CMV_VALID_OPTIONS;
  options |= valid;

  return(opt == valid);
}

bool CMVision::disable(unsigned opt)
{
  unsigned int valid;

  valid = opt & CMV_VALID_OPTIONS;
  options &= ~valid;

  return(opt == valid);
}

void CMVision::close()
{
  if(map) delete(map);
  map = NULL;
}


//==== Vision Testing Functions ====================================//

bool CMVision::testClassify(rgb * restrict out,image_pixel * restrict image)
{
  int i,s;
  rgb black = {0,0,0};

  if(!image || !out) return(false);

  classifyFrame(image,map);

  s = width * height;

  i = 0;
  while(i < s){
    while(i<s && !map[i]){
      out[i] = black;
      i++;
    }
    while(i<s && map[i]){
      out[i] = colors[bottom_bit(map[i])-1].color;
      i++;
    }
  }

  return(true);
}

bool CMVision::getThreshold(int color,
       int &y_low,int &y_high,
       int &u_low,int &u_high,
       int &v_low,int &v_high)
{
  color_info *c;

  if(color<0 || color>=CMV_MAX_COLORS) return(false);

  c = &colors[color];
  y_low = c->y_low;  y_high = c->y_high;
  u_low = c->u_low;  u_high = c->u_high;
  v_low = c->v_low;  v_high = c->v_high;

  return(true);
}

bool CMVision::setThreshold(int color,
       int y_low,int y_high,
       int u_low,int u_high,
       int v_low,int v_high)
{
  color_info *c;
  unsigned k;

  if(color<0 || color>=CMV_MAX_COLORS) return(false);

  c = &colors[color];
  k = 1 << color;

  clear_bits(y_class,CMV_COLOR_LEVELS,c->y_low,c->y_high,k);
  clear_bits(u_class,CMV_COLOR_LEVELS,c->u_low,c->u_high,k);
  clear_bits(v_class,CMV_COLOR_LEVELS,c->v_low,c->v_high,k);

  c->y_low = y_low;  c->y_high = y_high;
  c->u_low = u_low;  c->u_high = u_high;
  c->v_low = v_low;  c->v_high = v_high;

  set_bits(y_class,CMV_COLOR_LEVELS,y_low,y_high,k);
  set_bits(u_class,CMV_COLOR_LEVELS,u_low,u_high,k);
  set_bits(v_class,CMV_COLOR_LEVELS,v_low,v_high,k);

  return(true);
}

//==== Main Vision Functions =======================================//

bool CMVision::processFrame(image_pixel *image)
{
  int runs;
  int regions;
  int max_area;

  if(!image) return(false);

  if(options & CMV_THRESHOLD){

    classifyFrame(image,map);
    runs = encodeRuns(rmap,map);
    connectComponents(rmap,runs);

    regions = extractRegions(region_table,rmap,runs);

    if(options & CMV_COLOR_AVERAGES){
      calcAverageColors(region_table,regions,image,rmap,runs);
    }

    max_area = separateRegions(region_table,regions);
    sortRegions(max_area);

    if(options & CMV_DENSITY_MERGE){
      mergeRegions();
    }
  }

  return(true);
}

bool CMVision::processFrame(unsigned *map)
{
  int runs;
  int regions;
  int max_area;

  if(!map) return(false);

  runs = encodeRuns(rmap,map);
  connectComponents(rmap,runs);

  regions = extractRegions(region_table,rmap,runs);

  // if(options & CMV_COLOR_AVERAGES){
  //   calcAverageColors(region_table,regions,image,rmap,runs);
  // }

  max_area = separateRegions(region_table,regions);
  sortRegions(max_area);

  if(options & CMV_DENSITY_MERGE){
    mergeRegions();
  }

  return(true);
}

int CMVision::numRegions(int color_id)
{
  if(color_id<0 || color_id>=CMV_MAX_COLORS) return(CMV_NONE);
  return(region_count[color_id]);
}

CMVision::region *CMVision::getRegions(int color_id)
{
  if(color_id<0 || color_id>=CMV_MAX_COLORS) return(NULL);
  return(region_list[color_id]);
}