robotrecognition.cc

来自「该文件是包含了机器人足球比赛中的整个系统的代码」· CC 代码 · 共 560 行 · 第 1/2 页

// Filters out noise blobs in our image
void RobotRecognition::RobotNoiseFilter(const Colour c_ROBOT_COLOUR) {
  // Calculate the max limit for the x value so that we always stay beneath the horizon and do not calculate any really terrible points due to noise above the barriers
	double m=0, b=0, BUFFER=0;
  int leftX=0,rightX=0,topY=0,bottomY=0;
  
  if (visionData_->horizonLine.exists) {
		m = visionData_->horizonLine.m;
		b = visionData_->horizonLine.b;
		if (m > DBL_MAX || m < -DBL_MAX || b > DBL_MAX || b < -DBL_MAX) {
			// Bad horizon line - as such accept whole image
      return;
		}
	}  

  // Set up red dog checking code so it is only performed once
  if (visionData_->ball_ != NULL) {
    // Clean up red noise inside ball and red blobs that are too close
    leftX = ((VisionObject*) visionData_->ball_)->topBlob_->minX; 
    rightX = ((VisionObject*) visionData_->ball_)->topBlob_->maxX;
    topY = ((VisionObject*) visionData_->ball_)->topBlob_->minY;
    bottomY = ((VisionObject*) visionData_->ball_)->topBlob_->maxY;
    BUFFER = MAX((rightX - leftX) * 0.20, 10.0);
  }
  
  // Iterate through all blobs in the system - we need to do this once only for blue and red
  for (int i = 0; i < visionData_->numBlobs_[c_ROBOT_COLOUR]; i++) {
    Blob* blob = &(visionData_->blobs_[c_ROBOT_COLOUR][i]); // Retrieve the blob from the array
    if (blob->subsumed) continue;


    // blob holds the current blob that we are looking at
    
    double horizonY = (m * (blob->maxX + blob->minX)/2.0 + b) - HORIZON_ADJUSTMENT; // Note the horizon adjustment is to ensure that we don't have issues with the sideline dipping just below the horizon as it can do from a distance
		if (horizonY < 0) {
			horizonY = 0;
		} 

    if ((blob->maxY < horizonY) || (blob->area < MIN_BLOB_AREA)) {
      visionData_->DeleteBlob(blob);
//    blob = blob->nextBlob;
      continue;
    }

    if (blob->maxX - blob->minX < 2) continue;
    if (blob->maxY - blob->minY < 2) continue;

    // Perform robot specific filtering for red in the ball in the case of the red robot or blue inside
    // red robots in the case of the blue robote
    if (c_ROBOT_COLOUR == c_ROBOT_RED) {
      // Handle specific code for the red robot
      if ((visionData_->ball_ != NULL) && (blob->minX >= leftX - BUFFER) && (blob->maxX <= rightX + BUFFER) && (blob->minY >= topY - BUFFER) && (blob->maxY <= bottomY + BUFFER)) {
        visionData_->DeleteBlob(blob);
//      blob = blob->nextBlob;
        continue;        
      }
    } else {
      // Handle specific code for the blue robot 
      int centreX = (blob->minX + blob->maxX)/2;
//    int centreY = (blob->minY + blob->maxY)/2;

      // Remove the blob if it is a blue blob inside a red robot
      for (int k = 0; k < numRedRobots_; k++) {
        if (redRobots_[k].confidence_ > 0) {
          if (redRobots_[k].topBlob_ == NULL) break;
          if ((centreX >= redRobots_[k].topBlob_->minX) && (centreX <= redRobots_[k].topBlob_->maxX)) {
            visionData_->DeleteBlob(blob);
            break; //NOTE THIS FOR LOOP MUST BE AT END OF METHOD
          }
        }
      }
    }
    
    // Select the next blob of this colour
//  blob = blob->nextBlob;
  }
}



// Rotates the blobs in the image around the centre so they can be correctly compared
void RobotRecognition::RotateBlobs(Colour c_ROBOT_COLOUR, double effHeadRoll) {
  const int IMAGE_CENTRE_X = currentImageWidth_/2, IMAGE_CENTRE_Y = currentImageHeight_/2;

  // Moved out of the loop so the ints are not redefined everytime the loop runs
  int topX, topY, bottomX, bottomY, leftX, leftY, rightX, rightY;
  int rotatedTopX, rotatedTopY, rotatedBottomX, rotatedBottomY, rotatedLeftX, rotatedLeftY, rotatedRightX, rotatedRightY;

  double sinEffRoll = sin(effHeadRoll);
  double cosEffRoll = cos(effHeadRoll);
/*
  Blob* blob = &(visionData_->blobs_[c_ROBOT_COLOUR][0]);
  if (blob->subsumed) blob = blob->nextBlob;
  while (blob !=  NULL) {
    // Rotate the blob
  */                        


  for (int i = 0; i < visionData_->numBlobs_[c_ROBOT_COLOUR]; i++) {
    Blob* blob = &(visionData_->blobs_[c_ROBOT_COLOUR][i]); // Retrieve the blob from the array
    if (blob->subsumed) continue;


    topX = (IMAGE_CENTRE_X - (blob->minX + blob->maxX)/2);
    topY = (IMAGE_CENTRE_Y - blob->minY);

    rotatedTopX = (int) (cosEffRoll * topX - sinEffRoll * topY);
    rotatedTopY = (int) (sinEffRoll * topX + cosEffRoll * topY);

    bottomX = (IMAGE_CENTRE_X - (blob->minX + blob->maxX)/2);
    bottomY = (IMAGE_CENTRE_Y - blob->maxY);

    rotatedBottomX = (int) (cosEffRoll * bottomX - sinEffRoll * bottomY);
    rotatedBottomY = (int) (sinEffRoll * bottomX + cosEffRoll * bottomY);

    rightX = (IMAGE_CENTRE_X - blob->maxX);
    rightY = (IMAGE_CENTRE_Y - (blob->minY + blob->maxY)/2);

    rotatedRightX = (int) (cosEffRoll * rightX - sinEffRoll * rightY);
    rotatedRightY = (int) (sinEffRoll * rightX + cosEffRoll * rightY);

    leftX = (IMAGE_CENTRE_X - blob->minX);
    leftY = (IMAGE_CENTRE_Y - (blob->minY + blob->maxY)/2);

    rotatedLeftX = (int) (cosEffRoll * leftX - sinEffRoll * leftY);
    rotatedLeftY = (int) (sinEffRoll * leftX + cosEffRoll * leftY);

    blob->minXRotated = IMAGE_CENTRE_X - MAX(MAX(MAX(rotatedTopX, rotatedBottomX), rotatedRightX), rotatedLeftX);
    blob->maxXRotated = IMAGE_CENTRE_X - MIN(MIN(MIN(rotatedTopX, rotatedBottomX), rotatedRightX), rotatedLeftX);

    blob->minYRotated = IMAGE_CENTRE_Y - MAX(MAX(MAX(rotatedTopY, rotatedBottomY), rotatedRightY), rotatedLeftY);
    blob->maxYRotated = IMAGE_CENTRE_Y - MIN(MIN(MIN(rotatedTopY, rotatedBottomY), rotatedRightY), rotatedLeftY);

    // Locate the next blob
//    blob = blob->nextBlob;
  }
}

// Rotates the blobs in the image around the centre so they can be correctly compared
void RobotRecognition::UnrotateBlobs(Blob* blob, double effHeadRoll) {
  const int IMAGE_CENTRE_X = currentImageWidth_/2, IMAGE_CENTRE_Y = currentImageHeight_/2;

  // Moved out of the loop so the ints are not redefined everytime the loop runs
  int topX, topY, bottomX, bottomY, leftX, leftY, rightX, rightY;
  int rotatedTopX, rotatedTopY, rotatedBottomX, rotatedBottomY, rotatedLeftX, rotatedLeftY, rotatedRightX, rotatedRightY;

  double sinEffRoll = sin(-effHeadRoll); // Note: As we are performing an unrotate the angle has been negated
  double cosEffRoll = cos(-effHeadRoll);
  
  // Un-rotate the blob
  topX = (IMAGE_CENTRE_X - (blob->minXRotated + blob->maxXRotated)/2);
  topY = (IMAGE_CENTRE_Y - blob->minYRotated);

  rotatedTopX = (int) (cosEffRoll * topX - sinEffRoll * topY);
  rotatedTopY = (int) (sinEffRoll * topX + cosEffRoll * topY);

  bottomX = (IMAGE_CENTRE_X - (blob->minXRotated + blob->maxXRotated)/2);
  bottomY = (IMAGE_CENTRE_Y - blob->maxYRotated);

  rotatedBottomX = (int) (cosEffRoll * bottomX - sinEffRoll * bottomY);
  rotatedBottomY = (int) (sinEffRoll * bottomX + cosEffRoll * bottomY);

  rightX = (IMAGE_CENTRE_X - blob->maxXRotated);
  rightY = (IMAGE_CENTRE_Y - (blob->minYRotated + blob->maxYRotated)/2);

  rotatedRightX = (int) (cosEffRoll * rightX - sinEffRoll * rightY);
  rotatedRightY = (int) (sinEffRoll * rightX + cosEffRoll * rightY);

  leftX = (IMAGE_CENTRE_X - blob->minXRotated);
  leftY = (IMAGE_CENTRE_Y - (blob->minYRotated + blob->maxYRotated)/2);

  rotatedLeftX = (int) (cosEffRoll * leftX - sinEffRoll * leftY);
  rotatedLeftY = (int) (sinEffRoll * leftX + cosEffRoll * leftY);

  blob->minX = IMAGE_CENTRE_X - MAX(MAX(MAX(rotatedTopX, rotatedBottomX), rotatedRightX), rotatedLeftX);
  blob->maxX = IMAGE_CENTRE_X - MIN(MIN(MIN(rotatedTopX, rotatedBottomX), rotatedRightX), rotatedLeftX);

  blob->minY = IMAGE_CENTRE_Y - MAX(MAX(MAX(rotatedTopY, rotatedBottomY), rotatedRightY), rotatedLeftY);
  blob->maxY = IMAGE_CENTRE_Y - MIN(MIN(MIN(rotatedTopY, rotatedBottomY), rotatedRightY), rotatedLeftY);

  return;
}

// Performs a quick sort of the blobs updating the necessary pointers once the blobs have been sorted
inline void RobotRecognition::QuickSort(Colour c_COLOUR_ROBOT) {
	// First step is to convert the larger blob array into an array which does not contain subsumed blobs
  int numColourBlobs = 0;
  int numBlobs = visionData_->numBlobs_[c_COLOUR_ROBOT];
  sortedBlobs_ = 0;
  // Check that we have blobs we need to order
  if (numBlobs == 0) {
    return;
  }
  
  Blob* blobArray = new Blob[numBlobs]; // Data structure which will hold all non subsumed blobs
  
  // Iterate through the blobs storing them sequentially into the array
  Blob* blob = &(visionData_->blobs_[c_COLOUR_ROBOT][0]);
  if (blob->subsumed) blob = blob->nextBlob;
  while (blob !=  NULL) {
    blobArray[numColourBlobs] = *blob;
    numColourBlobs++;
    blob = blob->nextBlob;
  }

  sortedBlobs_ = numColourBlobs;
  if (numColourBlobs == 0) {
    delete blobArray;
    return;
  }
  
  // Now that we have our blob array we can being our sorting process
  OrderMin(blobArray, 0, numColourBlobs-1);

  // Now that the values have been reordered it is time to fix the various pointers
  // Update the first blob in the array to have a NULL pointer to the previous blob
  visionData_->blobs_[c_COLOUR_ROBOT][0] = blobArray[0];
  visionData_->blobs_[c_COLOUR_ROBOT][0].prevBlob = NULL;
  int i = 1;
  // Fix the pointers for the remainder of the array
  for (i=1; i < visionData_->numBlobs_[c_COLOUR_ROBOT]; i++) {
    visionData_->blobs_[c_COLOUR_ROBOT][i].subsumed = true;
  }
 
  for (i=1; i < numColourBlobs; i++) {
    visionData_->blobs_[c_COLOUR_ROBOT][i].subsumed=false;
    visionData_->blobs_[c_COLOUR_ROBOT][i] = blobArray[i];
    visionData_->blobs_[c_COLOUR_ROBOT][i-1].nextBlob = &(visionData_->blobs_[c_COLOUR_ROBOT][i]);
    visionData_->blobs_[c_COLOUR_ROBOT][i].prevBlob = &(visionData_->blobs_[c_COLOUR_ROBOT][i-1]);
  }

  // Update the last blob in the array to have a NULL pointer to the next blob
  visionData_->blobs_[c_COLOUR_ROBOT][numColourBlobs-1].nextBlob = NULL;
  
  // Remove our temporary blob array which we no longer require the order as the information has now been restored into the original data structure
  delete blobArray;
}

// Order the blowerbs in order of position
void RobotRecognition::OrderMin(Blob* blobArray, int lowerStart, int higherStart) {
  int lower = lowerStart;
  int higher = higherStart;
  int mid;

  if (higherStart > lowerStart) {
    mid = blobArray[(lowerStart + higherStart)/2].minXRotated;

    while(lower <= higher) {
      // find the first element that is greater than or equal to the partition element starting from the left Index.
	    while((lower < higherStart) && (blobArray[lower].minXRotated < mid)) {
		    lower++;
      }

      // find an element that is smaller than or equal to the partition element starting from the right Index.
      while((higher > lowerStart) && (blobArray[higher].minXRotated > mid )) {
		    higher--;
      }

      // if the indexes have not crossed, swap
      if(lower <= higher) {
	      Blob temp = (blobArray[lower]);
	      blobArray[lower] = blobArray[higher];
	      blobArray[higher] = temp;
        lower++;
        higher--;
      }
    }

    // If the right index has not reached the left side of array must now sort the left partition.    
	  if(lowerStart < higher) {
      OrderMin(blobArray, lowerStart, higher);
    }

    // If the left index has not reached the right side of array must now sort the right partition.
    if( lower < higherStart ) {
      OrderMin(blobArray, lower, higherStart);
    }
  }
}