Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

Vision.cc

Go to the documentation of this file.

#include <stdio.h>
#include <math.h>

#include <MCOOP.h>

#include "Shared/Config.h"
#include "Shared/mathutils.h"
#include "Events/EventRouter.h"
#include "cmv_region.h"
#include "Vision.h"
#include "Shared/WorldState.h"
#include "Events/VisionEvent.h"
#include "Shared/get_time.h"
#include "Shared/SystemUtility.h"
#include "VisionSerializer.h"
#include "Wireless/Wireless.h"

Vision *vision=NULL;

using namespace mathutils;
using namespace VisionEventNS;

Vision::Vision() : frameTimestamp(0UL), frame_count(0), cmap(NULL), tmap(NULL),
                   rmap(NULL), rmap2(NULL), reg(NULL), width(0), height(0),
                   max_width(0), max_height(0), max_runs(0), max_regions(0),
                   num_colors(0), num_runs(0), num_regions(0), body_angle(0.0),
                   body_height(100.0), vis_markers(0), obj_info(NULL),
                   imageLayer(ofbkimageLAYER_H), vser(NULL)
{
  initialize();
  vser=new VisionSerializer();
}

void Vision::setCameraParams()
{
  static OPrimitiveID fbkID = 0;
      
  if(fbkID == 0){
    if(OPENR::OpenPrimitive("PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1", &fbkID) != oSUCCESS){
      cout << "Open FbkImageSensor failure." << endl;
    }
  }

  longword wb=config->vision.white_balance;
  longword gain=config->vision.gain;
  longword shutter=config->vision.shutter_speed;

  /* White Balance */
  OPrimitiveControl_CameraParam owb(wb);
  if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &owb, sizeof(owb), 0, 0) != oSUCCESS){
    cout << "CAM_SET_WHITE_BALANCE : Failed!" << endl;
  }

  /* Gain */
  OPrimitiveControl_CameraParam ogain(gain);
  if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &ogain, sizeof(ogain), 0, 0) != oSUCCESS){
    cout << "CAM_SET_GAIN : Failed!" << endl;
  }

  /* Shutter Speed */
  OPrimitiveControl_CameraParam oshutter(shutter);
  if(OPENR::ControlPrimitive(fbkID,oprmreqCAM_SET_SHUTTER_SPEED, &oshutter, sizeof(oshutter), 0, 0) != oSUCCESS){
    cout << "CAM_SET_SHUTTER_SPEED : Failed!" << endl;
  }
}

void Vision::setResolution()
{
  switch(config->vision.resolution) {
    case 2:
      width=88;
      height=72;
      imageLayer=ofbkimageLAYER_M;
      break;
    case 3:
      width=44;
      height=36;
      imageLayer=ofbkimageLAYER_L;
      break;
    default:
      width=176;
      height=144;
      imageLayer=ofbkimageLAYER_H;
      break;
  }

  max_width  = width;
  max_height = height;

  int size = width * height;
  max_runs = size / MIN_EXP_RUN_LENGTH;
  max_regions = size / MIN_EXP_REGION_SIZE;
  size = max_width * max_height;

  if (cmap!=NULL) { DeleteLarge(cmap); cmap=NULL; }
  NewLarge(&cmap,size+1); // +1 to store EncodeRuns terminator value
  if(!cmap) {
    cout << "Couldn't allocate cmap\n\xFF" << endl;
    *((long *)0xDEADCC44)=1;
  }
  if (rmap!=NULL) { DeleteLarge(rmap); rmap=NULL; }
  NewLarge(&rmap,max_runs);
  if(!rmap) {
    cout << "Couldn't allocate rmap\n\xFF" << endl;
    *((long *)0xDEAD7744)=1;
  }
  if (rmap2!=NULL) { DeleteLarge(rmap2); rmap2=NULL; }
  NewLarge(&rmap2,max_runs);
  if(!rmap2) {
    cout << "Couldn't allocate rmap2\n\xFF" << endl;
    *((long *)0xDEAD7745)=1;
  }
  if (reg!=NULL) { DeleteLarge(reg); reg=NULL; }
  NewLarge(&reg,max_regions);
  if(!reg) {
    cout << "Couldn't allocate reg\n" << endl;
    *((long *)0xDEAD7E66)=1;
  }
}

void Vision::setColors()
{
  color_names.clear();
  num_colors =
    CMVision::LoadColorInformation(color,MAX_COLORS,config->vision.colors,
                                   color_names);
  if(num_colors > 0){
    printf("  Loaded %d colors.\n",num_colors);
  }else{
    printf("  ERROR: Could not load colors file:%s\n", config->vision.colors);
  }

  int num_y,num_u,num_v;
  int size = 1 << (bits_y + bits_u + bits_v);
  num_y = 1 << bits_y;
  num_u = 1 << bits_u;
  num_v = 1 << bits_v;

  memset(tmap,0,size*sizeof(cmap_t));

  if(!CMVision::LoadThresholdFile(tmap,num_y,num_u,num_v,
                                  config->vision.thresh)) {
    printf("  ERROR: Could not load threshold file '%s'.\n",
           config->vision.thresh);
  }
  int remapped=CMVision::CheckTMapColors(tmap,num_y,num_u,num_v,num_colors,0);
  printf("remapped %d colors in threshold file '%s'\n",
         remapped, config->vision.thresh);
}

int Vision::getColor(const char* color) {
  hash_map<const char*, int, hash<const char*>, hashcmp_eqstr>::iterator i;
  i=color_names.find(color);
  if (i==color_names.end()) return -1;
  return i->second;
}

void Vision::initialize()
{
  int result;
  int size = 1 << (bits_y + bits_u + bits_v);

  obj_info=new ObjectInfo;

  setResolution();

  // Set up threshold

#ifdef PLATFORM_APERIOS
  result = NewRegion(size, reinterpret_cast<void**>(&tmap));
  if (result != sSUCCESS)
    printf("Unable to allocate tmap buffer of size %d\n",size);
#else
  tmap = new cmap_t[size];
#endif

  setColors();


  initializeEventSpecs();
  setCameraParams();
}

void Vision::initializeEventSpecs() {
  for(int event_idx=0; event_idx<NUM_VEVENTS; event_idx++) {
    vevent_spec[event_idx].listeners = 0; // disable events
    vevent_spec[event_idx].filter = 0; // disable noise removal
    vevent_spec[event_idx].count = 0;
    vevent_spec[event_idx].cx = 0;
    vevent_spec[event_idx].cy = 0;
    vevent_spec[event_idx].present = false;
  }
  vevent_spec[RedBallSID].confidence=0.7;
  vevent_spec[PinkBallSID].confidence=0.7;
  vevent_spec[HandSID].confidence=0.5;
  vevent_spec[ThumbsupSID].confidence=0.45;
  vevent_spec[ThingSID].confidence=0.5;
  vevent_spec[MarkersSID].confidence=1.0001;  // disabled
}

void Vision::enableEvents(int vevent) {
  vevent_spec[vevent].listeners++;
}

void Vision::enableEvents(int vevent, int noise) {
  enableEvents(vevent);
  setNoiseThreshold(vevent, noise);
}

void Vision::disableEvents(int vevent) {
  if (vevent_spec[vevent].listeners>0)
    vevent_spec[vevent].listeners--;
}

void Vision::setNoiseThreshold(int vevent, int noise) {
  vevent_spec[vevent].filter=noise;
}

bool Vision::generateEvent(int vevent, double conf, int cenX, int cenY) {
  float cx, cy;
  if (conf>vevent_spec[vevent].confidence) {  
    cx=(cenX-88.0)/88.0;
    cy=(cenY-72.0)/72.0;
    vevent_spec[vevent].cx=cx;
    vevent_spec[vevent].cy=cy;

    if (vevent_spec[vevent].present) {
      vevent_spec[vevent].count=0;
      createEvent(EventBase::statusETID,vevent,cx,cy);
    } else {
      vevent_spec[vevent].count++;
      if (vevent_spec[vevent].count>vevent_spec[vevent].filter) {
        vevent_spec[vevent].count=0;
        vevent_spec[vevent].present=true;
        createEvent(EventBase::activateETID,vevent,cx,cy);
      }
    }
    return true;
  } else {
    if (!vevent_spec[vevent].present) {
      vevent_spec[vevent].count=0;
    } else {
      vevent_spec[vevent].count++;
      if (vevent_spec[vevent].count>vevent_spec[vevent].filter) {
        vevent_spec[vevent].count=0;
        vevent_spec[vevent].present=false;
        createEvent(EventBase::deactivateETID,vevent,0,0);
      } else {
        createEvent(EventBase::statusETID,vevent,cx,cy);
      }
    }
    return false;
  }
}

bool Vision::close()
{
  DeleteLarge(tmap);
  DeleteLarge(cmap);
  DeleteLarge(rmap);
  DeleteLarge(rmap2);
  DeleteLarge(reg);

  tmap = NULL;
  cmap = NULL;
  rmap = NULL;
  rmap2 = NULL;
  reg  = NULL;

  max_width  = 0;
  max_height = 0;

  return(true);
}

inline double pct_from_mean(double a,double b) {
  double s = (a - b) / (a + b);
  return fabs(s);
}

int Vision::calcEdgeMask(int x1,int x2,int y1,int y2)
{
  static const int boundary_pixel_size=1;

  int edge;

  edge = 0;
  if(x1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_LEFT  ;
  if(x2 >= width -1-boundary_pixel_size) edge |= OFF_EDGE_RIGHT ;
  if(y1 <= 0       +boundary_pixel_size) edge |= OFF_EDGE_TOP   ;
  if(y2 >= height-1-boundary_pixel_size) edge |= OFF_EDGE_BOTTOM;

  return edge;
}

int Vision::addToHistHorizStrip(int y, int x1, int x2, int *color_cnt)
{
  int x;

  y  = bound(y ,0,height-1);
  x1 = bound(x1,0,width -1);
  x2 = bound(x2,0,width -1);

  for(x=x1; x<=x2; x++) {
    color_cnt[getColorUnsafe(x,y)]++;
  }

  return x2-x1+1;
}

int Vision::addToHistVertStrip(int x, int y1, int y2, int *color_cnt)
{
  int y;

  x  = bound(x ,0,width -1);
  y1 = bound(y1,0,height-1);
  y2 = bound(y2,0,height-1);

  for(y=y1; y<=y2; y++) {
    color_cnt[getColorUnsafe(x,y)]++;
  }

  return y2-y1+1;
}

bool Vision::findHand(VObject *hand, VisionObjectInfo *hand_info)
{
  if (vevent_spec[HandSID].listeners==0) return false;

  color_class_state *skin;
  int n;
  double conf;

  skin=&color[getColor("skin")];

  region *or_reg; // hand region(s)

  or_reg=skin->list;

  hand->confidence = 0.0;

  if(!or_reg) return false;

  n = 0;
  while(or_reg && n<10) {
    conf = ((double)or_reg->area/500);

    if(conf > 1.0) conf = 1.0;
    
    if(conf > hand->confidence) {
        hand->confidence = conf;
        hand_info->reg = or_reg;
    }

    or_reg = or_reg->next;
    n++;
  }

  return generateEvent (HandSID, hand->confidence, hand_info->reg->cen_x, hand_info->reg->cen_y);
}

bool Vision::findBall(int ball_color, VObject *ball,VisionObjectInfo *ball_info) {
  if ((ball_color==getColor("red") && vevent_spec[RedBallSID].listeners==0)
      ||(ball_color==getColor("pink") && vevent_spec[PinkBallSID].listeners==0)) return false;

  static const bool debug_ball = false;
  static const bool debug_conf = false;

  static int frame_cnt=0;
  static const int print_period=1;

  if(debug_ball)
    frame_cnt = (frame_cnt + 1) % print_period;

  color_class_state *pink;
  int n;
  int w,h;
  double conf;
  double green_f;

  pink=&color[ball_color];

  region *or_reg; // pink region

  or_reg=pink->list;

  ball->confidence = 0.0;

  if(!or_reg) return false;

  n = 0;
  while(or_reg && n<10) {
    double conf0,conf_square_bbox,conf_area,conf_green,conf_area_bonus;
    double conf_red_v_area;
    int edge;

    w = or_reg->x2 - or_reg->x1 + 1;
    h = or_reg->y2 - or_reg->y1 + 1;
    
    edge = calcEdgeMask(or_reg);
    conf0 = (w >= 3) * (h >= 3) * (or_reg->area >= 7);
    conf_square_bbox = 
      edge ?
      gaussian_with_min(pct_from_mean(w,h) / .6, 1e-3) :
      gaussian_with_min(pct_from_mean(w,h) / .2, 1e-3);
    conf_area =
      edge ?
      gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .6, 1e-3) :
      gaussian_with_min(pct_from_mean(M_PI*w*h/4.0,or_reg->area) / .2, 1e-3);
    conf_red_v_area = 1.0;
    conf_green = 1.0;
    conf_area_bonus = ((double)or_reg->area / 1000);

    conf = 
      conf0 *
      conf_square_bbox *
      conf_area *
      conf_red_v_area *
      conf_green +
      conf_area_bonus;

    if(conf > 1.0) conf = 1.0;
    
    if(debug_conf &&
         frame_cnt == 0) {
      printf("conf0 %g conf_square_bbox %g conf_area %g conf_area_bonus %g final %g\n",
              conf0,conf_square_bbox,conf_area,conf_area_bonus,conf);
    }
    
/*
    if(conf > ball->confidence) {
      if(debug_ball &&
       (frame_cnt == 0 ||
        frame_cnt == 1)) {
        printf("%s ball n%d cen (%f,%f) area %d bbox (%d,%d)-(%d,%d) conf %f\n", (ball_color==COLOR_PINK)?"Pink":"Orange",
                n,or_reg->cen_x,or_reg->cen_y,or_reg->area,
                or_reg->x1,or_reg->y1,or_reg->x2,or_reg->y2,
                conf);
      }

      int sx[2],sy[2]; // scan bounding coordinates;
      bool edge_x[2],edge_y[2]; // true if scan coordinate went off edge
      static const int scan_distance = 3;
      static int color_cnts[num_colors];
      int scan_pixels;
      int good_value;
    
      sx[0] = or_reg->x1 - scan_distance;
      edge_x[0] = (sx[0] < 0);
      if(edge_x[0]) sx[0] = 0;
      sx[1] = or_reg->x2 + scan_distance;
      edge_x[1] = (sx[1] > width-1);
      if(edge_x[1]) sx[1] = width-1;
      sy[0] = or_reg->y1 - scan_distance;
      edge_y[0] = (sy[0] < 0);
      if(edge_y[0]) sy[0] = 0;
      sy[1] = or_reg->y2 + scan_distance;
      edge_y[1] = (sy[1] > height-1);
      if(edge_y[1]) sy[1] = height-1;

      scan_pixels = 0;
      for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
        color_cnts[color_idx]=0;
    
      // do horizontal strips
      for(int side=0; side<2; side++) {
        if(!edge_y[side]) {
          scan_pixels+=addToHistHorizStrip(sy[side],sx[0],sx[1],color_cnts);
        }
      }
    
      // do vertical strips
      for(int side=0; side<2; side++) {
        if(!edge_x[side]) {
          scan_pixels+=addToHistVertStrip(sx[side],sy[0],sy[1],color_cnts);
        }
      }

      int non_robot_fringe=0;
      non_robot_fringe += 5*color_cnts[getColor("blue")];
      non_robot_fringe -=   color_cnts[getColor("green")];

      conf_red_v_area = 1 + non_robot_fringe / or_reg->area;
      conf_red_v_area = bound(conf_red_v_area,0.0,1.0);
      
      good_value = 0;
      good_value += 2*color_cnts[getColor("blue")];
      good_value += 3*color_cnts[getColor("green")]/2;

      green_f = std::max(good_value+1,1) / (scan_pixels + 1.0);
      green_f = bound(green_f,0.0,1.0);
      conf_green = green_f;
    
      conf = 
        conf0 *
        conf_square_bbox *
        conf_area *
        + conf_area_bonus;
    
      if(conf > 1.0) conf = 1.0;
    }*/
    
    if(conf > ball->confidence) {
      /*
      d = sqrt((FocalDist * FocalDist * YPixelSize) * (M_PI * BallRadius * BallRadius) /
               (or_reg->area));
    
      vector3d ball_dir; // direction of ball from camera in robot coordinates
      ball_dir = getPixelDirection(or_reg->cen_x,or_reg->cen_y);
    
      // Reject if ball above level plane
//      if(atan2(ball_dir.z,hypot(ball_dir.x,ball_dir.y)) <= (5*(M_PI/180))) {
        ball->edge = calcEdgeMask(or_reg);
        
        vector3d intersect_ball_loc,pixel_size_ball_loc;

        bool intersects=false;
        vector3d intersection_pt(0.0,0.0,0.0);
        intersects=GVector::intersect_ray_plane(camera_loc,ball_dir,
                                                vector3d(0.0,0.0,BallRadius),vector3d(0.0,0.0,1.0),
                                                intersection_pt);
        if(intersects) {
          intersect_ball_loc = intersection_pt;
        }

        pixel_size_ball_loc = camera_loc + ball_dir * d;

        vector3d ball_loc,alt_ball_loc;
        if(ball->edge!=0 && intersects) {
          ball_loc     = intersect_ball_loc;
          alt_ball_loc = pixel_size_ball_loc;
        } else {
          alt_ball_loc = intersect_ball_loc;
          ball_loc     = pixel_size_ball_loc;
        }*/ 

        ball->confidence = conf;
        
//        ball->loc = ball_loc;
        
//        ball->distance = hypot(ball_loc.x,ball_loc.y);
        
//        findSpan(ball->left,ball->right,or_reg->x1,or_reg->x2,or_reg->y1,or_reg->y2);

        ball_info->reg = or_reg;

/*        if(debug_ball &&
           frame_cnt==0) {
          printf("###found ball, conf %g loc (%g,%g,%g) alt (%g,%g,%g) dist %g left %g right %g edge %d\n",
                  ball->confidence,
                  ball->loc.x,ball->loc.y,ball->loc.z,
                  alt_ball_loc.x,alt_ball_loc.y,alt_ball_loc.z,
                  ball->distance,ball->left,ball->right,ball->edge);
        }*/
     // }
    }

    or_reg = or_reg->next;
    n++;
  }
  return (ball_color==getColor("red"))?generateEvent (RedBallSID, ball->confidence, ball_info->reg->cen_x, ball_info->reg->cen_y):generateEvent(PinkBallSID,ball->confidence,ball_info->reg->cen_x,ball_info->reg->cen_y);
}

bool Vision::findThing(VObject *thing, VisionObjectInfo *thing_info) {
  if (vevent_spec[ThingSID].listeners==0) return false;
  thing->confidence=0;
  return generateEvent (ThingSID, thing->confidence, thing_info->reg->cen_x, thing_info->reg->cen_y);
}

int Vision::isIn(region *r1, region *r2) {
  int r1w=r1->x2-r1->x1;
  int r1h=r1->y2-r1->y1;

  if ((((r2->x1 < r1->x1) && (r2->x2 > r1->x1+r1w*0.75)) ||
       ((r2->x2 > r1->x2) && (r2->x1 < r1->x2-r1w*0.75)))
      && (((r2->y1 < r1->y1) && (r2->y2 > r1->y1+r1h*0.75)) ||
         ((r2->y2 > r1->y2) && (r2->y1 < r1->y2-r1h*0.75))))
    return 1;
  return 0;
}

int Vision::isAdjacent(region *r1, region *r2) {
  // bounding boxes <= two pixels apart
  if (r1->x1 < r2->x1)
    return r1->x2>(r2->x1-2);
  else if (r1->x2 > r2->x2)
    return r1->x1<(r2->x2+2);
  return 0;
}

int Vision::identifyMarker(int color1, int color2, int color3) {
  static const int marker_colors[27]={
      -1, -1, -1, MARKER_GOG, -1, MARKER_GOP, MARKER_GPG, MARKER_GPO, -1,
      -1, MARKER_OGO, MARKER_OGP, -1, -1, -1, MARKER_OPG, MARKER_OPO, -1,
      -1, MARKER_PGO, MARKER_PGP, MARKER_POG, -1, MARKER_POP, -1, -1, -1 };
  int markeridx=0;
  switch (color1) {
    case COLOR_ORANGE:
      markeridx+=9;
      break;
    case COLOR_PURPLE:
      markeridx+=18;
      break;
  }
  switch (color2) {
    case COLOR_ORANGE:
      markeridx+=3;
      break;
    case COLOR_PURPLE:
      markeridx+=6;
      break;
  }
  switch (color3) {
    case COLOR_ORANGE:
      markeridx+=1;
      break;
    case COLOR_PURPLE:
      markeridx+=2;
      break;
  }
  return marker_colors[markeridx];
}

bool Vision::findMarkers() {
  if (vevent_spec[MarkersSID].listeners==0) return false;

  const bool debug_markers    = false;
  static int frame_cnt=0;

  static const int print_period=30;

  if(debug_markers)
    frame_cnt = (frame_cnt + 1) % print_period;

  region *or_reg, *gr_reg, *pu_reg;

  or_reg = color[getColor("orange")].list;
  gr_reg = color[getColor("bgreen")].list;
  pu_reg = color[getColor("purple")].list;

  region* marker_regs[9];
  int num_regs=0;

  for (int i=0; i<9; i++) marker_regs[i]=NULL;

  while (num_regs<9) {
    int rarea=0;

    if (or_reg) {
      marker_regs[num_regs]=or_reg;
      rarea=or_reg->area;
    }
    if (gr_reg && gr_reg->area>rarea) {
      marker_regs[num_regs]=gr_reg;
      rarea=gr_reg->area;
    }
    if (pu_reg && pu_reg->area>rarea) {
      marker_regs[num_regs]=pu_reg;
      rarea=pu_reg->area;
    }
    if (rarea<30) break;

    switch(marker_regs[num_regs]->color) {
      case COLOR_ORANGE:
        or_reg=or_reg->next;
        break;
      case COLOR_BGREEN:
        gr_reg=gr_reg->next;
        break;
      case COLOR_PURPLE:
        pu_reg=pu_reg->next;
        break;
    }
    num_regs++;
  }

  vis_markers=0;

  for (int i=0;  i<num_regs/3; i++) {
    int j;
    for (j=0; j<3; j++) markers[vis_markers].regs[j]=NULL;
    for (j=i; j<num_regs; j++)
      if (marker_regs[j]) break;
    markers[vis_markers].regs[0]=marker_regs[j];
    marker_regs[j]=NULL;
    for (j++; j<num_regs; j++) {
      if (marker_regs[j]) {
        if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])) {
          markers[vis_markers].regs[1]=marker_regs[j];
          marker_regs[j]=NULL;
          break;
        }
      }
    }
    if (!markers[vis_markers].regs[1]) continue;
    for (j=i+1; j<num_regs; j++) {
      if (marker_regs[j]) {
        if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])
            && isAdjacent(markers[vis_markers].regs[1],marker_regs[j])) {
          markers[vis_markers].regs[2]=markers[vis_markers].regs[1];
          markers[vis_markers].regs[1]=marker_regs[j];
          marker_regs[j]=NULL;
          break;
        } else if (isAdjacent(markers[vis_markers].regs[0],marker_regs[j])) {
          markers[vis_markers].regs[2]=markers[vis_markers].regs[1];
          markers[vis_markers].regs[1]=markers[vis_markers].regs[0];
          markers[vis_markers].regs[0]=marker_regs[j];
          marker_regs[j]=NULL;
          break;
        } else if (isAdjacent(markers[vis_markers].regs[1],marker_regs[j])) {
          markers[vis_markers].regs[2]=marker_regs[j];
          marker_regs[j]=NULL;
          break;
        }
      }
    }
    if (!markers[vis_markers].regs[2]) continue;
    if (markers[vis_markers].regs[0]->cen_x>markers[vis_markers].regs[2]->cen_x) {
      region* t_marker=markers[vis_markers].regs[0];
      markers[vis_markers].regs[0]=markers[vis_markers].regs[2];
      markers[vis_markers].regs[2]=t_marker;
    }
    markers[vis_markers].cen_x=markers[vis_markers].regs[1]->cen_x;
    markers[vis_markers].cen_y=markers[vis_markers].regs[1]->cen_y;
    markers[vis_markers].marker=identifyMarker(markers[vis_markers].regs[0]->color, markers[vis_markers].regs[1]->color,markers[vis_markers].regs[2]->color);
    if (markers[vis_markers].marker<0) continue;

    if (debug_markers && frame_cnt==0)
      printf("marker colors: %d%d%d, id: %d\n",markers[vis_markers].regs[0]->color, markers[vis_markers].regs[1]->color, markers[vis_markers].regs[2]->color,markers[vis_markers].marker);
    vis_markers++;
  }
  for (int i=0; i<vis_markers; i++) {
#ifdef PLATFORM_APERIOS
    VisionEvent *mevent=new VisionEvent(EventBase::statusETID,MarkersSID,
                                     markers[i].cen_x, markers[i].cen_y);
    mevent->setProperty(markers[i].marker);
    erouter->postEvent(mevent);
#endif
  }
  return (vis_markers>0);
}

bool Vision::findGesture(VisionObjectInfo *hand_info) {
  if (vevent_spec[ThumbsupSID].listeners==0) return false;
  if (hand_info->reg->cen_y>=144) return false;

  int thumbsup=0, thumbx=0;
  int idx=hand_info->reg->run_start;
  int parent=rmap[idx].parent;

  int i=0, y=0;

  while (y<=hand_info->reg->cen_y) {
    int r=0;
    int xl=0;
    while (rmap[i].y<=y) {
      if (r>0) {
        if (rmap[i].parent==parent
            && rmap[i].width>5
            && rmap[i].x > xl+10) {
          thumbsup++;
          thumbx+=xl+(rmap[i].x-xl)/2;
        } else {
          xl=rmap[i].x+rmap[i].width;
        }
      } else if (r==0) {
        if (rmap[i].parent==parent
            && rmap[i].width>5) {
          r=i;
          xl=rmap[i].x+rmap[i].width;
        }
      }
      i++;
      if (i>=num_runs) break;
    }
    if (i>=num_runs) break;
    y=rmap[i].y;
  }
  
  return generateEvent (ThumbsupSID, limitRange(thumbsup/20.00,0.0,1.0), hand_info->reg->cen_x, hand_info->reg->cen_y);
}

void Vision::createEvent(unsigned int tid, unsigned int sid,
                         float cenX, float cenY) {
#ifdef PLATFORM_APERIOS
  erouter->postEvent(new VisionEvent((EventBase::EventTypeID_t)tid,sid,cenX,cenY));
#endif
}

bool Vision::runHighLevelVision(ObjectInfo *obj_info) {
  for(int obj_idx=0; obj_idx<NUM_VISION_OBJECTS; obj_idx++) {
    vobj_info[obj_idx].reg = NULL;
  }


//  findMarkers();
//  bool rball_exist=findBall(getColor("red"),&obj_info->rball,&vobj_info[RBALL]);
  if (getColor("red")>=0)
    bool pball_exist=findBall(getColor("red"),&obj_info->pball,&vobj_info[PBALL]);
  /*
  bool hand_exist=findHand(&obj_info->hand,&vobj_info[HAND]);
  if (hand_exist
      && !(rball_exist && isIn(vobj_info[RBALL].reg,vobj_info[HAND].reg))
      && !(pball_exist && isIn(vobj_info[PBALL].reg,vobj_info[HAND].reg)))
      // hand exists and hand is not holding ball
    findGesture(&vobj_info[HAND]);*/
  return true;
}

bool Vision::thresholdImage(CMVision::image_idx<rgb> &img) {
  static rgb rgb_colors[MAX_COLORS];
  static bool initted=false;

  if(!initted) {
    for(int color_idx=0; color_idx<MAX_COLORS; color_idx++)
      rgb_colors[color_idx]=color[color_idx].color;
    initted=true;
  }

  CMVision::RgbToIndex(cmap,img.buf,width,height,rgb_colors,MAX_COLORS);

  return true;
}

bool Vision::thresholdImage(CMVision::image_yuv<const uchar> &img) {
  CMVision::ThresholdImageYUVPlanar<cmap_t,CMVision::image_yuv<const uchar>,const uchar,bits_y,bits_u,bits_v>(cmap,img,tmap);

  return true;
}

//#define ENABLE_JOIN_NEARBY

#ifdef ENABLE_JOIN_NEARBY

//#define DEBUG_JOIN

#endif /* ENABLE_JOIN_NEARBY */

//#define CHECK_REGIONS

template<class image>
bool Vision::runLowLevelVision(image &img)
{
  static int frame_cnt=-1;
  frame_cnt++;

#ifdef ENABLE_JOIN_NEARBY
  int num_runs_old;
#endif
  int max_area;

  width  = img.width;
  height = img.height;

  thresholdImage(img);
  num_runs = CMVision::EncodeRuns(rmap,cmap,img.width,img.height,max_runs);

#ifdef ENABLE_JOIN_NEARBY
  num_runs_old = num_runs;
#endif

#ifdef ENABLE_JOIN_NEARBY
  num_runs = CMVision::JoinNearbyRuns(rmap,rmap2,num_runs,width,height);
#ifdef DEBUG_JOIN_PARANOID
  {
    bool ok=CMVision::CheckRuns(rmap2+1,num_runs-2,width,height);
    if(!ok) {
      printf("error in run data, consistency check failed\n");
    }
  }
#endif
#endif

#ifdef ENABLE_JOIN_NEARBY
  memcpy(rmap,rmap2,sizeof(*rmap2)*max_runs);
#endif

#ifdef DEBUG_JOIN
  {
    bool ok=CMVision::CheckRuns(rmap+1,num_runs-2,width,height);
    if(!ok) {
      printf("error in run data, consistency check failed\n");
    }
  }
#endif

  CMVision::ConnectComponents(rmap,num_runs);

  num_regions = CMVision::ExtractRegions(reg,max_regions,rmap,num_runs);

  //printf("runs:%6d (%6d) regions:%6d (%6d)\n",
  //        num_runs,max_runs,
  //        num_regions,max_regions);

  max_area = CMVision::SeparateRegions(color,num_colors,reg,num_regions);
  CMVision::SortRegions(color,num_colors,max_area);

  CMVision::MergeRegions(color,num_colors,rmap);

  for(int i=0; i<num_colors; i++)
    color[i].total_area = -1;

#ifdef CHECK_REGIONS
  {
    bool ok=CMVision::CheckRegions(color,num_colors,rmap);
    if(!ok) {
      printf("CheckRegions failed\n");
    }
  }
#endif

//   CMVision::CreateRunIndex(yindex,rmap,num_runs);
  return(true);
}

bool Vision::processFrame(const uchar *data_y, const uchar *data_u, const uchar *data_v, int width, int height, int skip) {
  frameTimestamp = get_time();
  frame_count++;

  img.buf_y=data_y;
  img.buf_u=data_u;
  img.buf_v=data_v;

  img.width=width;
  img.height=height;
  //img.row_stride=3*width;
  img.row_stride=skip+width;

  bool ok=true;

  if(ok) ok=runLowLevelVision(img);
  if(ok) ok=runHighLevelVision(obj_info);

  vser->serialize();

  return ok;
}

int WritePPM(char *filename,rgb *img,int width,int height)
{
  FILE *out;
  int wrote;

  out = fopen(filename,"wb");
  if(!out) return(0);

  fprintf(out,"P6\n%d %d\n%d\n",width,height,255);
  wrote = fwrite(img,sizeof(rgb),width*height,out);
  fclose(out);

  return(wrote);
}

bool Vision::saveThresholdImage(char *filename)
{
  rgb *buf;
  int wrote;

  buf = new rgb[width * height];
  if(!buf) return(false);

  CMVision::IndexToRgb<cmap_t,color_class_state>(buf,cmap,width,height,color,num_colors);
  wrote = WritePPM(filename,buf,width,height);
  delete(buf);

  return(wrote > 0);
}

vector3d Vision::getPixelDirection(double x, double y) {
/*  vector3d img_pixel_dir; // direction vector of pixel in image coordinates
                                                                                
  img_pixel_dir.set((x+.5-width/2.0),(height/2.0-(y+.5))*YPixelSize,FocalDist);
                                                                                
  vector3d robot_pixel_dir; // direction vector of pixel in robot coordinates
  robot_pixel_dir =
    camera_dir  *img_pixel_dir.z +
    camera_up   *img_pixel_dir.y +
    camera_right*img_pixel_dir.x;
                                                                                
  robot_pixel_dir = robot_pixel_dir.norm();
                                                                                
  return robot_pixel_dir;*/
}

/*! @file
 * @brief Does majority of vision processing
 * @author CMU RoboSoccer 2001-2002 (Creator)
 * @author alokl (ported)
 * 
 * @verbinclude CMPack_license.txt
 *
 * $Author: ejt $
 * $Name: tekkotsu-1_5 $
 * $Revision: 1.15 $
 * $State: Rel $
 * $Date: 2003/10/10 05:38:25 $
 */

Tekkotsu v1.5

Generated Fri Oct 10 15:52:00 2003 by Doxygen 1.3.4