objectrecognition.cc

该文件是包含了机器人足球比赛中的整个系统的代码

            reconsiderCurrent = true;
            // we'll consider the rest on the next run through. no need to do it now ..
            break;
          }
        }
        consumableBlob = consumableBlob->nextBlob;
      }
      if (reconsiderCurrent == false) {
        consumingBlob = consumingBlob->nextBlob;
      }
      reconsiderCurrent = false;
    }
  }

/*if (!(visionData_->headTiltCombined_ > DEG_TO_RAD(DOWNCHECK) || ABS(visionData_->headPan_) > DEG_TO_RAD(20))) return;
  for (int colourS = 0; colourS < NUMCOLOURS; colourS++) {
    Blob* currentBlob = &blobs_[colourS][0];
  
    if (currentBlob->subsumed) currentBlob = currentBlob->nextBlob;

    while (currentBlob != NULL) {
      if (!currentBlob->subsumed && currentBlob!=NULL) {
        bool foundColour = false;
        int totalFound = 0;
        int numRequired = 1 + (int)(currentBlob->area / 30);
         // this check scans through a second lut and checks if some orange exists in the blob
        for (int iY = currentBlob->minY; iY < currentBlob->maxY; iY++) {
          for (int iX = currentBlob->minX; iX < currentBlob->maxX; iX++) {
            if (visionData_->classifiedTight_[iY * currentImageWidth_ + iX] == colourS) { 
              totalFound++;
              if (totalFound == numRequired) {
                foundColour = true;
                break;
              }
            }
          }
        }
        if (foundColour) {
          currentBlob->subsumed=false;
        } else 
          currentBlob->subsumed=true;
      }
      currentBlob = currentBlob->nextBlob;
    }
  }*/
}

// Updates the blob array in the case where the end of the image was reached and a run was still in progress
inline void ObjectRecognition::AddRunToBlob(Run addRun) {
  //Run** runs_ = visionData_->runs_;
  Blob** blobs_ = visionData_->blobs_;
  //int* numRuns_ = visionData_->numRuns_;
  //int* numBlobs_ = visionData_->numBlobs_;

  Blob* blob = &blobs_[addRun.colour][addRun.blobNumber];

  blob->minX = addRun.startPoint;
  blob->minXy = addRun.line;
  blob->maxX = addRun.endPoint;
  blob->maxXy = addRun.line;

  blob->minY = addRun.line;
  blob->minYx = addRun.startPoint;
  blob->maxY = addRun.line;
  blob->maxYx = addRun.endPoint;

  blob->area = addRun.endPoint - addRun.startPoint+1;
  blob->subsumed = false;
  blob->prevBlob = prevBlob[addRun.colour];
  blob->nextBlob = NULL;
  if (blob->prevBlob != NULL) {
    blob->prevBlob->nextBlob = blob;
  } else {
    blob->prevBlob = blob; // special case for front sentinel. points to self as previous blob. be very very careful !
  }
  prevBlob[addRun.colour] = blob;
}
    
bool ObjectRecognition::VerifyBallRadius(Blob* currentBlob) {

  double height = currentBlob->maxY - currentBlob->minY;
  double width = currentBlob->maxX - currentBlob->minX;
  if (height > width) {
    if (height/2 <= width) 
      return true;
    else
      return false;
  } else {
    if (width/2 <= height) 
      return true;
    else
      return false;
  }
}

// Find balls from blobs !
void ObjectRecognition::FindBalls() {
//Run** runs_ = visionData_->runs_;
//int* numRuns_ = visionData_->numRuns_;


  if (colourCounter[c_BEACON_BLUE] == 0 
      && colourCounter[c_BEACON_PINK] == 0 
      && colourCounter[c_ROBOT_BLUE] == 0 
      && colourCounter[c_ROBOT_RED] == 0
      && colourCounter[c_WHITE] == 0 
      && colourCounter[c_FIELD_GREEN] == 0) {
      if (colourCounter[c_BALL_ORANGE] < currentImageWidth_*currentImageHeight_*3/10) {
/*      FILE* imageFile = fopen("/ms/fuck.rvs", "wb"); // Open the file which contains the LUT for read access
        fwrite(visionData_->unclassified_,1,FULL_IMAGE_WIDTH*FULL_IMAGE_HEIGHT*6,imageFile);
        fclose(imageFile);*/
        emergencyTurnAround_ = true;
        return;
      }
  }

  Blob** blobs_ = visionData_->blobs_;
  int* numBlobs_ = visionData_->numBlobs_;

  visionData_->ball_ = NULL;
  numBalls_ = 0;
  int numOrangeBlobs = numBlobs_[c_BALL_ORANGE];
  int numYellowBlobs = numBlobs_[c_BEACON_YELLOW];
  Blob* currentBlob;
  Blob* goalBlob;
  bool confidenceOverRide = false;
  if (numOrangeBlobs == 0) {
    return;  
  }
  int lA = -1; // lA stands for largest area of the ball located so far
  int largestIndex = -1;
  int largestArea = -1;


  bool lowTable = false;
  for (int i = 0; i < numOrangeBlobs; i++) { // Process each orange blob in turn
    currentBlob = &blobs_[c_BALL_ORANGE][i]; // Retrieve the first blob from the array
    if (currentBlob->subsumed == false) { // Check that the ball is not subsumed i.e. not contained within a larger blob
      int k = 0;
      for (k = 0; k < numYellowBlobs; k++) { // Process each yellow blob
        Blob* yellowBlob = &blobs_[c_BEACON_YELLOW][k]; // Retrieve the yellow blob from the array
        Blob* orangeBlob = currentBlob;
        if (yellowBlob->subsumed == false) {

          // check if yellowblob is within orange blob, and if so, remove it.
          // i haven't seen this happen for months (new luts have been better)
          if (yellowBlob->maxX <= orangeBlob->maxX && yellowBlob->minX >= orangeBlob->minX && yellowBlob->maxY <= orangeBlob->maxY && yellowBlob->minY >= orangeBlob->minY)  {
            visionData_->DeleteBlob(yellowBlob);
          }
        }
      }
    
      if ( (currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY) > lA ) {
        lA = (currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY);
      }
      // We now have a possible ball. The following code determines whether contained completely within a yellow blob and ignores it if this is the case ... (part of goals ???) never seems to go bad, surprisingly.
      bool invalidBall = false;
      for (k = 0; k < numYellowBlobs; k++) {
        goalBlob = &blobs_[c_BEACON_YELLOW][k];
        if (goalBlob->maxX >= currentBlob->maxX && goalBlob->maxY >= currentBlob->maxY && goalBlob->minY <= currentBlob->minY && goalBlob->minX <= currentBlob->minX) {
          invalidBall = true;
          break;
        } else if ( (goalBlob->maxX-goalBlob->minX) < (currentBlob->maxX-currentBlob->minX) && (goalBlob->maxY < currentBlob->minY) && (goalBlob->maxY+3 > currentBlob->minY) && goalBlob->minX >= currentBlob->minX && goalBlob->maxX <= currentBlob->maxX && goalBlob->maxY-goalBlob->minY < 3) {
          currentBlob->minY = goalBlob->minY;
        }
      } 
      if (invalidBall) {
        continue;
      }
      // The following condition ensure that the area is greater than 8 and that the width and height are greater than 3. This then is possibly a ball
      bool atLeastBigBall = ((currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY)) > 8 && (currentBlob->maxX-currentBlob->minX > 3) && (currentBlob->maxY-currentBlob->minY > 3);
      bool atLeastSmallBall = ((currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY)) > 5 && (currentBlob->maxX-currentBlob->minX > 2) && (currentBlob->maxY-currentBlob->minY > 2);
      if (atLeastBigBall && numBalls_ < MAXBALLS) {
        double pixelDistance = 0.0;

        // Variable initalisation
        bool ignoreDistance = false;
        int centreX = (currentBlob->maxX+currentBlob->minX)/2; // Simply locate the centre of the blob
        int centreY = (currentBlob->maxY+currentBlob->minY)/2;
        
        int cX = centreX;
        int cY = centreY;
     
        double maxRadius = MAX( ((currentBlob->maxY-currentBlob->minY)/2.0),((currentBlob->maxX-currentBlob->minX)/2.0) ); // Gives us a rough approximation to the radius which will be changed later based on ellipse fitting if possible
        double distance = 0;

        double maxBlobRadius = maxRadius;
        // Check whether it is possible to use infra red and if so use this as the distance value 
        // else determine the distance using ellipse fitting and other techniques
        double infraDist = visionData_->GetInfraredDistance(currentBlob,c_BALL_ORANGE);
        if (false && infraDist >= 0) {
          distance = infraDist;
          ignoreDistance = false; // we have determined a distance using infrared so don't ignore the distance found
          confidenceOverRide = true;
        } else { 
          // Check that the distance is not affected by part of the ball being cropped by the edge of the image
          bool verticalProblemTop = false;
          bool verticalProblemBottom = false;
          bool horizontalProblemLeft = false;
          bool horizontalProblemRight = false;
          
          Circle resultantCircle;
          resultantCircle.isDefined = false;

          // Check whether part of the ball is off the top or bottom of the image
          if (currentBlob->minY < BLOB_DIST_TO_EDGE) verticalProblemTop = true;
          if (((currentImageHeight_-1) - currentBlob->maxY) < BLOB_DIST_TO_EDGE) verticalProblemBottom = true;

          // Check whether part of the ball is off the left or right of the image
          if (currentBlob->minX < BLOB_DIST_TO_EDGE) horizontalProblemLeft = true;
          if ((currentImageWidth_ - currentBlob->maxX) < BLOB_DIST_TO_EDGE) horizontalProblemRight = true;

          int direction = LEFTRIGHT;
          //if (horizontalProblemLeft && !horizontalProblemRight) direction = RIGHTLEFT;
          //if ((horizontalProblemLeft || horizontalProblemRight) && !verticalProblemTop) direction = TOPBOTTOM;
          //else if ((horizontalProblemLeft || horizontalProblemRight) && !verticalProblemBottom) direction = BOTTOMTOP;
          lowTable = !(visionData_->headTiltCombined_ > DEG_TO_RAD(DOWNCHECK) || ABS(visionData_->headPan_)> DEG_TO_RAD(20));
          if (!verticalProblemTop && lowTable) direction = TOPBOTTOM;
          if (!verticalProblemTop && (horizontalProblemLeft || horizontalProblemRight)) direction = TOPBOTTOM;          
 /*
          if (verticalProblemTop) {
            // There is a problem at the top of the image - Check there are no other problems and that a sufficient amount of the ball is showing with which to generate the radius
            if (!verticalProblemBottom && !horizontalProblemLeft && !horizontalProblemRight && VerifyBallRadius(currentBlob)) {
              resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob);
            }
          } else if (verticalProblemBottom) {
            // There is a problem at the bottom of the image - Check there are no other problems and that a sufficient amount of the ball is showing with which to generate the radius
            if (!verticalProblemTop && !horizontalProblemLeft && !horizontalProblemRight && VerifyBallRadius(currentBlob)) {
              resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob);
            }            
          } else if (horizontalProblemLeft) {
            // There is a problem at the left side of the image - Check there are no other problems and that a sufficient amount of the ball is showing with which to generate the radius
            if (!verticalProblemTop && !verticalProblemBottom && !horizontalProblemRight && VerifyBallRadius(currentBlob)) {
              resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob);
            }
          } else if (horizontalProblemRight) {
            // There is a problem at the right side of the image - Check there are no other problems and that a sufficient amount of the ball is showing with which to generate the radius
            if (!verticalProblemTop && !verticalProblemBottom && !horizontalProblemLeft && VerifyBallRadius(currentBlob)) {
              resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob);
            }
          } else {
            // Do ellipse fit as there is no problems with the side of the image
            resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob);
          }*/
          resultantCircle = ellipseFitting.FitEllipseLMF(visionData_,currentBlob,direction);
       
          // Circle is not defined if the ellipse fit algorithm has never been run or if the algorithm determined that it could not generate a radius
          if (resultantCircle.isDefined) {
//          if (resultantCircle.sd > 1.0) continue;
            cX = (int) resultantCircle.centreX; //-- CentreX and CentreY should be relative to the blob on the screen not the expected centre based on ellipse fitting
            cY = (int) resultantCircle.centreY;
            if (resultantCircle.radius > 1) { 
              maxRadius = resultantCircle.radius;
              ignoreDistance = false; // we have determined a distance using ellipse fitting so don't ignore the distance found
            } else {
              ignoreDistance = true;
            }
          } else {
            continue;
            ignoreDistance = true; // we are unable to determine a distance using infrared or ellipse fitting as there is two much of the ball of the image so return -1
          }
          
          if (maxRadius < maxBlobRadius/1.3) {
            maxRadius = maxBlobRadius;
            cX = centreX;
            cY = centreY;
            if (maxRadius < maxBlobRadius/1.5) ignoreDistance = true;
          }
   
          // 680 is a magical ratio to find the distance to the ball based on its size.
          // I think this tends to overestimate, but it's not too bad and we don't care that much anyway...
          distance = 680.0*IMAGE_SCALE/maxRadius;     
          
         //   centreY = currentBlob->minY + maxRadius;
          // Perform a final check to make sure the distance found is not greater than the length of the field
          if ((distance > 500.0) || (distance <= 0)/* || (distance > maxDistance)*/) {
            distance = 500;
            ignoreDistance = true; // The distance found earlier is obviously invalid so ignore the distance
          }
          if (ignoreDistance) {
            if (horizontalProblemLeft || horizontalProblemRight) {
              pixelDistance = RatioDistanceCalc(currentBlob,true);
              distance = 680.0*IMAGE_SCALE/pixelDistance; //The true indicates that this is a problem with the side of the image             
            } else {
              pixelDistance = RatioDistanceCalc(currentBlob,false);
              distance = 680.0*IMAGE_SCALE/pixelDistance; //The true indicates that this is a problem with the top or bottom of the image
            }
          }
          
          
          if (horizontalProblemLeft && horizontalProblemRight && verticalProblemTop && verticalProblemBottom) {
//          distance = 0.01;
// 0.01 just DOES NOT WORK (actually i think <= 1 doesn't work)
// SPEAK TO CM BEFORE CHANGING THIS (chase.rvs, image 36 is the test case)
            distance = 1.1;
            cX = centreX;
            cY = centreY;
          }
        }

        // change by CM - use top of ball for elevation. helps behaviour ..
        balls_[numBalls_].SetData(VisionObject::OT_BALL, currentBlob, currentBlob, visionData_->CalculateHeading(cX), visionData_->CalculateElevation(cY), distance, /*(cX+centreX)/2*/centreX, currentBlob->minY, 0);