shapetracker.cc

机器人仿真软件

  {
    CvMat *predictionx, *predictiony;

    // Predict the next position
    predictionx = cvKalmanPredict(this->kalmanX, 0);
    predictiony = cvKalmanPredict(this->kalmanY, 0);

    //if(debug > 1)
      //{
        //xmeanp = predictionx->data.fl[0];
        //ymeanp = predictiony->data.fl[0];

        //xmeanvp = predictionx->data.fl[1];
        // ymeanvp = predictiony->data.fl[1];
        //fprintf(stderr, "predict %f %f %f %f\n",xmeanp,xmeanvp,ymeanp, ymeanvp);
        //fprintf(stderr, "meas %f %f\n",(float)xmean, (float)ymean);
    //}

    if(xmean == (width/2) && ymean == (height/2))
    {

      pt3.x=(int)predictionx->data.fl[0];
      pt4.x=(int)predictionx->data.fl[1];

      pt3.y = (int)predictiony->data.fl[0];
      pt4.y=(int)predictiony->data.fl[1];

      this->kalmanX->state_post->data.fl[0] = predictionx->data.fl[0];
      this->kalmanX->state_post->data.fl[1] = predictionx->data.fl[1];
      this->kalmanY->state_post->data.fl[0] = predictiony->data.fl[0];
      this->kalmanY->state_post->data.fl[1] = predictiony->data.fl[1];
    } else {

      CvMat *corrx, *corry;

      this->measurementX->data.fl[0] = (float)xmean;
      this->measurementY->data.fl[0] = (float)ymean;

      corrx = cvKalmanCorrect(this->kalmanX, this->measurementX);
      corry = cvKalmanCorrect(this->kalmanY, this->measurementY);

      pt3.x=(int)(corrx->data.fl[0]);
      pt4.x=(int)(corrx->data.fl[1]);
      pt3.y = (int)(corry->data.fl[0]);
      pt4.x=(int)(corry->data.fl[1]);

    }


    //if(debug == 3)
    //{
      //fprintf(stderr, "post %d %d %d %d\n", pt3.x, pt3.y, pt4.x, pt4.y);
      //fprintf(stderr, "pre after %f %f %f %f\n",
          //this->kalmanX->state_post->data.fl[0],
          //this->kalmanY->state_post->data.fl[0],
          //this->kalmanX->state_post->data.fl[1],
          //this->kalmanY->state_post->data.fl[1]);
    //}

    //cvCircle(init_image, pt3, 15.0, CV_RGB(255,0,0), 2);

    this->trackVelocityX = (float)(pt3.x - (this->cameraData.width/2)) / 
      (this->cameraData.width/2);
    this->trackVelocityY = (float)(-pt3.y + (this->cameraData.height/2)) /
      (this->cameraData.height/2);

  }

  //if(reset_kalman == 1)
  //{
    //if(debug == 2)
    //{
      //fprintf(stderr, " xmean  %d, ymean %d\n",xmean, ymean);
    //}
    //vx = (float)(xmean - (width/2))/(width/2);
    //vy = (float)(-ymean + (height/2))/(height/2);
    //this->kalmanFirst =1; // reset the kalman filter
  //}

  if(this->trackVelocityX > 1.0)
    this->trackVelocityX = 1.0;
  if(this->trackVelocityX < -1.0)
    this->trackVelocityX = -1.0;

  if(this->trackVelocityY > 1.0)
    this->trackVelocityY = 1.0;
  if(this->trackVelocityY < -1.0)
    this->trackVelocityY = -1.0;

  //visual_servo_command.vel_right = vx;
  //visual_servo_command.vel_forward = vy;
  //visual_servo_command.heading_offset = (float)heading;
  //visual_servo_command.tracking_state = HV_OBJECT_TRACK_STATE;

  //if(debug > 1 ){
    //fprintf(stderr, "Sent x %f, y %f, z %f, state %d\n",
        //visual_servo_command.vel_right, 
        //visual_servo_command.vel_forward,
        //visual_servo_command.heading_offset,
        //visual_servo_command.tracking_state);
  //}

  //put_data_on_image(width, height, vx, vy, heading, font);
  //draw_lines(width, height, heading, pt3);
*/
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Reset the kalman filter
/*void ShapeTracker::ResetKalman()
{
  this->kalmanFirst = 1;
}*/

////////////////////////////////////////////////////////////////////////////////
// Look for stuff in the image.
int ShapeTracker::ProcessFrame()
{

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

  // Create a new mainImage if it doesn't already exist
  //if (this->mainImage == NULL)
    this->mainImage = cvCreateImage( cvSize(this->stored_data.width,
          this->stored_data.height), IPL_DEPTH_8U, 3);

  // Create a work image if it doesn't already exist
  //if (this->workImage == NULL)
    workImage = cvCreateImage( cvSize(this->stored_data.width,
          this->stored_data.height), IPL_DEPTH_8U, 1);

  if (this->hist == NULL)
  {
    this->histSize = 256;
    float range0[] = {0, 256};
    float *ranges[] = {range0};

    this->hist = cvCreateHist(1, &histSize, CV_HIST_ARRAY, ranges, 1);
    this->lutMat = cvCreateMatHeader(1, 256, CV_8UC1);
    cvSetData(this->lutMat, this->lut, 0);
  }

  // Initialize the main image
  memcpy(this->mainImage->imageData, this->stored_data.image, 
         this->mainImage->imageSize);

  // Make dest a gray scale image
  cvCvtColor(this->mainImage, this->workImage, CV_BGR2GRAY);

  //cvSaveImage("orig.jpg", this->mainImage);

  this->ContrastStretch( this->mainImage, this->workImage );

  //cvSaveImage("hist.jpg", this->workImage);

  cvCvtColor(this->mainImage, this->workImage, CV_BGR2GRAY);

  // Create a binary image
  cvThreshold(this->workImage, this->workImage, this->threshold, 255, 
      CV_THRESH_BINARY);

  //cvSaveImage("work.jpg", this->workImage);

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


  //this->KalmanFilter();
  

  // Clear the images
  cvReleaseImage(&(this->workImage));
  this->workImage = NULL;
  cvReleaseImage(&(this->mainImage));
  this->mainImage = NULL;
  
  WriteData();
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Update the device data (the data going back to the client).
void ShapeTracker::WriteData()
{
  unsigned int i;
  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 = shapeCount;
  
  // Go through the blobs
  for (i = 0; i < this->shapeCount; i++)
  {
    shape = this->shapes + i;

    // Set the data to pass back
    data.blobs[i].id = shape->id;
    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));
  
  
  return;
}

////////////////////////////////////////////////////////////////////////////////
// Calculate the angle between three points
double ShapeTracker::CalcAngle( CvPoint *pt1, CvPoint *pt2, CvPoint *pt0)
{
  double dx1 = pt1->x - pt0->x;
  double dy1 = pt1->y - pt0->y;
  double dx2 = pt2->x - pt0->x;
  double dy2 = pt2->y - pt0->y;
  return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

void ShapeTracker::ContrastStretch( IplImage *src, IplImage *gray )
{
  int high = 0;
  int low = 0;
  int index;
  float hist_value;
  float scale_factor;
  IplImage *R = cvCreateImage(cvGetSize(src), src->depth, 1);
  IplImage *G = cvCreateImage(cvGetSize(src), src->depth, 1);
  IplImage *B = cvCreateImage(cvGetSize(src), src->depth, 1);

  cvCalcHist(&gray, this->hist, 0, NULL);

  for(index = 0; index < this->histSize; index++){
    hist_value = cvQueryHistValue_1D(this->hist, index);
    if(hist_value != 0){
      low = index;
      break;
    }
  } 

  for(index = this->histSize-1; index >= 0; index--){
    hist_value = cvQueryHistValue_1D(this->hist, index);
    if(hist_value != 0){
      high = index;
      break;
    }
  }             

  scale_factor = 255.0f/(float)(high - low);
  for(index = 0; index < 256; index++){
    if((index >= low) && (index <= high))
    {
      this->lut[index] = (unsigned char)((float)(index - low)*scale_factor);
    }
    if(index > high) this->lut[index] = 255;
  }

  cvCvtPixToPlane(src, R, G, B, NULL);
  cvLUT(R, R, this->lutMat);
  cvLUT(G, G, this->lutMat);
  cvLUT(B, B, this->lutMat);
  cvCvtPlaneToPix(R, G, B, NULL, src);

  cvReleaseImage(&R);
  cvReleaseImage(&G);
  cvReleaseImage(&B);
}