simpleshape.cc

机器人仿真软件

    this->outImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
    cvGetSubRect(this->outImage, this->outSubImages + 0, cvRect(0, 0, width, height));
    cvGetSubRect(this->outImage, this->outSubImages + 1, cvRect(width, 0, width, height));
    cvGetSubRect(this->outImage, this->outSubImages + 2, cvRect(0, height, width, height));
    cvGetSubRect(this->outImage, this->outSubImages + 3, cvRect(width, height, width, height));
  }

  // Create a main image and copy in the pixels
  switch (this->stored_data.format)
  {
    case PLAYER_CAMERA_FORMAT_MONO8:
    {
      // Copy pixels to input image (grayscale)
      assert(this->inpImage->imageSize >= (int) this->stored_data.image_count);
      memcpy(this->inpImage->imageData, this->stored_data.image, this->inpImage->imageSize);
      break;
    }
    default:
    {
      PLAYER_WARN1("image format [%d] is not supported", this->stored_data.format);
      return -1;
    }
  }

  // Copy original image to output
/*  if (this->out_camera_id.port)
  {
    cvSetZero(this->outImage);
    cvCopy(this->inpImage, this->outSubImages + 0);
  }*/

  // Clone the input image to our workspace
  this->workImage = cvCloneImage(this->inpImage);

  // Find all the shapes in the working image
  this->FindShapes();

  // Free temp storage
  cvReleaseImage(&this->workImage);
  this->workImage = NULL;

  WriteBlobfinderData();
    
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Having pre-processed the image, find some shapes
void SimpleShape::FindShapes()
{
  int sim;
  double area;
  FeatureSet featureSet;
  CvMemStorage *storage;
  CvSeq *contour;
  CvRect rect;
  Shape *shape;

  // Reset the shape count
  this->shapeCount = 0;

  // Find edges
  cvCanny(this->workImage, this->workImage, this->cannyThresh1, this->cannyThresh2);

  // Copy edges to output image
/*  if (this->out_camera_id.port)
    cvCopy(this->workImage, this->outSubImages + 1);*/

  // Find contours on a binary image
  storage = cvCreateMemStorage(0);
  cvFindContours(this->workImage, storage, &contour, sizeof(CvContour), 
                 CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
    
  for(; contour != NULL; contour = contour->h_next)
  {
    rect = cvBoundingRect(contour);
    area = fabs(cvContourArea(contour));

    // Discard small/open contours
    if (area < 5 * 5)
      continue;

    // Discard the countour generated from the image border
    if (rect.x < 5 || rect.y < 5)
      continue;
    if (rect.x + rect.width >= this->workImage->width - 5)
      continue;
    if (rect.y + rect.height >= this->workImage->height - 5)
      continue;

    
    // Draw eligable contour on the output image; useful for debugging
/*    if (this->out_camera_id.port)
      cvDrawContours(this->outSubImages + 2, contour, CV_RGB(255, 255, 255),
                     CV_RGB(255, 255, 255), 0, 1, 8);*/

    // Compute the contour features
    this->ExtractFeatureSet((CvContour*) contour, &featureSet);

    // Match against the model
    sim = this->MatchFeatureSet(&featureSet, &this->modelFeatureSet);
    if (sim > 0)
      continue;

    // Draw contour on the main image; useful for debugging
/*    if (this->out_camera_id.port)
    {
      cvDrawContours(this->outSubImages + 3, contour, CV_RGB(128, 128, 128),
                     CV_RGB(128, 128, 128), 0, 1, 8);
      cvRectangle(this->outSubImages + 3, cvPoint(rect.x, rect.y),
                  cvPoint(rect.x + rect.width, rect.y + rect.height), CV_RGB(255, 255, 255), 1);
    }*/

    // Check for overrun
    if (this->shapeCount >= sizeof(this->shapes) / sizeof(this->shapes[0]))
    {
      PLAYER_WARN("image contains too many shapes");
      break;
    }
    
    // Add the shape to our internal list
    shape = this->shapes + this->shapeCount++;
    shape->id = -1;
    shape->ax = rect.x;
    shape->ay = rect.y;
    shape->bx = rect.x + rect.width;
    shape->by = rect.y + rect.height;
  }

  cvReleaseMemStorage(&storage);
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Extract a feature set for the given contour
void SimpleShape::ExtractFeatureSet(CvContour *contour, FeatureSet *feature)
{
  int i;
  CvBox2D box;
  CvPoint *p;
  CvRect rect;
  CvSeq *poly;
  double aa, bb;
  double dx, dy;
  double var;
  
  // Get the moments (we will use the Hu invariants)
  cvMoments(contour, &feature->moments);

  // Compute the compactness measure: perimeter squared divided by
  // area.  
  rect = cvBoundingRect(contour);
  feature->compact = (contour->total * contour->total) / fabs(cvContourArea(contour));

  // Compute elliptical variance
  box = cvFitEllipse2(contour);

  //printf("%f %f %f %f\n", box.center.x, box.center.y, box.size.width / 2, box.size.height / 2);

  aa = box.size.width * box.size.width / 4;
  bb = box.size.height * box.size.height / 4;

  var = 0.0;
  for (i = 0; i < contour->total; i++)
  {
    p = CV_GET_SEQ_ELEM(CvPoint, contour, i);
    dx = (p->x - box.center.x);
    dy = (p->y - box.center.y);
    var += dx * dx / aa + dy * dy / bb;
  }
  var /= contour->total;

  feature->variance = var;

  // Fit a polygon
  poly = cvApproxPoly(contour, sizeof(CvContour), NULL, CV_POLY_APPROX_DP,
                      cvContourPerimeter(contour) * 0.02, 0);
  feature->vertexCount = poly->total;

  CvPoint *a, *b, *c;
  double ax, ay, bx, by, cx, cy;
  double d, n, m;
  
  // Construct a string describing the polygon (used for syntactic
  // matching)
  for (i = 0; i < poly->total; i++)
  {
    a = CV_GET_SEQ_ELEM(CvPoint, poly, i);
    b = CV_GET_SEQ_ELEM(CvPoint, poly, (i + 1) % poly->total);
    c = CV_GET_SEQ_ELEM(CvPoint, poly, (i + 2) % poly->total);

    // Compute normalized segment vectors
    ax = b->x - a->x;
    ay = b->y - a->y;
    d = sqrt(ax * ax + ay * ay);
    ax /= d;
    ay /= d;

    bx = -ay;
    by = +ax;
    
    cx = c->x - b->x;
    cy = c->y - b->y;
    d = sqrt(cx * cx + cy * cy);
    cx /= d;
    cy /= d;

    // Compute projections
    n = cx * ax + cy * ay;
    m = cx * bx + cy * by;

    // Add a symbol; right now this is just -1, +1, corresponding to
    // an inside or outside corner
    assert((size_t) i < sizeof(feature->vertexString) / sizeof(feature->vertexString[0]));
    feature->vertexString[i] = (m < 0 ? -1 : +1);
  }

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Compute similarity measure on features
int SimpleShape::MatchFeatureSet(FeatureSet *a, FeatureSet *b)
{
  int i, j;
  int sim, minSim;
  int na, nb;
  
  if (a->vertexCount != b->vertexCount)
    return INT_MAX;

  minSim = INT_MAX;
  
  // Look for the lowest dissimalarity by trying all possible
  // string shifts
  for (i = 0; i < a->vertexCount; i++)
  {
    sim = 0;
    
    for (j = 0; j < a->vertexCount; j++)
    {
      na = a->vertexString[j];
      nb = b->vertexString[(j + i) % a->vertexCount];
      sim += (na != nb);
    }

    if (sim < minSim)
      minSim = sim;
  }

  return minSim;
}


////////////////////////////////////////////////////////////////////////////////
// Write blobfinder data
void SimpleShape::WriteBlobfinderData()
{
  unsigned int i;
  //size_t size;
  Shape *shape;
  player_blobfinder_data_t data;

  // Se the image dimensions
  data.width = (this->stored_data.width);
  data.height = (this->stored_data.height);

  data.blobs_count = (this->shapeCount);
    
  for (i = 0; i < this->shapeCount; i++)
  {
    shape = this->shapes + i;

    // Set the data to pass back
    data.blobs[i].id = shape->id;  // TODO
    data.blobs[i].color = 0;  // TODO
    data.blobs[i].area = ((int) ((shape->bx - shape->ax) * (shape->by - shape->ay)));
    data.blobs[i].x = ((int) ((shape->bx + shape->ax) / 2));
    data.blobs[i].y = ((int) ((shape->by + shape->ay) / 2));
    data.blobs[i].left = ((int) (shape->ax));
    data.blobs[i].top = ((int) (shape->ay));
    data.blobs[i].right = ((int) (shape->bx));
    data.blobs[i].bottom = ((int) (shape->by));
    data.blobs[i].range = (0);
  }

  // Copy data to server.
  Publish(device_addr,NULL,PLAYER_MSGTYPE_DATA,PLAYER_BLOBFINDER_DATA_BLOBS,&data,sizeof(data));
  // Copy data to server
/*  size = sizeof(data) - sizeof(data.blobs) + this->shapeCount * sizeof(data.blobs[0]);
  this->PutMsg(this->blobfinder_id, NULL, PLAYER_MSGTYPE_DATA, 0, &data, size, &this->cameraTime);*/
  
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Write camera data; this is a little bit naughty: we re-use the
// input camera data, but modify the pixels
/*void SimpleShape::WriteCameraData()
{
  size_t size;
  
  if (this->camera_id.port == 0)
    return;
  if (this->outImage == NULL)
    return;

  // Do some byte swapping
  this->outCameraData.width = htons(this->outImage->width);
  this->outCameraData.height = htons(this->outImage->height);
  this->outCameraData.bpp = 8;
  this->outCameraData.format = PLAYER_CAMERA_FORMAT_MONO8;
  this->outCameraData.compression = PLAYER_CAMERA_COMPRESS_RAW;
  this->outCameraData.image_size = htonl(this->outImage->imageSize);

  // Copy in the pixels
  memcpy(this->outCameraData.image, this->outImage->imageData, this->outImage->imageSize);

  // Compute message size
  size = sizeof(this->outCameraData) - sizeof(this->outCameraData.image)
    + this->outImage->imageSize;

  // Copy data to server
  this->PutMsg(this->out_camera_id, NULL, PLAYER_MSGTYPE_DATA, 0, &this->outCameraData, size, &this->cameraTime);
  
  return;
}*/