objectrecognition.cc

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

      bool ignoreDistance=false;
      if ((goalBlob->minY < BLOB_DIST_TO_EDGE) || (((currentImageHeight_-1) - goalBlob->maxY) < BLOB_DIST_TO_EDGE)) {
        ignoreDistance = true; // If unable to calculate the distance to the goal return -1 to localisation (Approved: MF 13/02/03)
      }

      double actualHeight = height/(2*sin(ABS(visionData_->effHeadRoll_))+cos(ABS(visionData_->effHeadRoll_)));
      // magic factors !

      double distance = 4800.0*IMAGE_SCALE/((double)(actualHeight));
      //double distance = 10000.0*IMAGE_HEIGHT_SCALE/((double)(actualHeight)*2.5);

      // use distancetopoint as an upper bound on distances returned.
      // if returned distnace exceeds distancetopoint, fuck the hell off and don't use distance.
      // blue goal seems ok so don't fuck with it for now (CM+MQ)
      double distanceToPoint = visionData_->GetDistanceToPoint(goalBlob->maxYx, goalBlob->maxY, HEIGHTOFNECK);
      if (distance > distanceToPoint && BEACON_COLOUR==c_BEACON_YELLOW) {
        ignoreDistance=true;
      }


      // Check whether it is possible to use infrared detection for the goal and if so use this as the distance value
      if (ignoreDistance) {
        double infraDist = visionData_->GetInfraredDistance(goalBlob,BEACON_COLOUR);
        if (infraDist >= 0) {
          distance = infraDist;
          ignoreDistance=false;
        }
      }
   
      int v = 0;
      v = numVisionObjects_;
      // Note elevation check uses maxY as the difference between the goal and the deacon size makes this elevation check more reliable
    
      visionObjects_[v].SetData(OT_GOAL_COLOUR, goalBlob, goalBlob, visionData_->CalculateHeading(cx), visionData_->CalculateElevation(goalBlob->maxY), distance, cx, cy, 0); 
      visionData_->TransformPositionObject(&visionObjects_[v]);
      if (ignoreDistance) visionObjects_[v].distance_=-1.0;
    
      bool acceptGoal = false;
      bool acceptGoal2 = true;

      double vertDistance = visionObjects_[v].distance_*sin(visionObjects_[v].elevation_);
      if (vertDistance > visionData_->maxHeight) {
        acceptGoal2 = false;
      }

      if (visionData_->useSanityFactors) {
        // The confidence indicates on a scale of 1 .. 100 how confident we are that the data collected about the object is correct
        CalculateGoalConfidence(&visionObjects_[v]);
        confidence = visionObjects_[v].confidence_;
        if (visionObjects_[v].confidence_ == 0) {
          acceptGoal = false;
        } else {
          acceptGoal = true;
        }
      } else {
        // this elevation check is finely tuned to avoid picking up beacons as goals (!).
        // verify goal fixed by CM (March 9, 2004)
#ifdef ERS_210
        if ((visionObjects_[v].elevation_*180.0/PI <= -1.5) || ((visionObjects_[v].elevation_*180.0/PI <= 4.5) && (VerifyGoals(goalBlob) < 3))) {
          if ((visionObjects_[v].elevation_*180.0/PI <= -2.5) || (goalBlob->minY >= 5)) {
            if ((visionObjects_[v].elevation_*180.0/PI > -35.0)) {
              acceptGoal = true;
            }
          }
        }
  #endif
  #ifdef ERS_7
        if ((visionObjects_[v].elevation_*180.0/PI <= 1.0) || ((visionObjects_[v].elevation_*180.0/PI <= 2.5) && (VerifyGoals(goalBlob) < 3))) {
          if ((visionObjects_[v].elevation_*180.0/PI <= 0.0) || (goalBlob->minY >= 5)) {
            if ((visionObjects_[v].elevation_*180.0/PI > -25.0)) {
              acceptGoal = true;
            }
          }
        }
// ok, craig has a new check for goals in balls. find bottom middle of goal. search down 5 pixels. if there's no green and there's 2 orange pixels, YOU ARE NOT THE FUCKING GOAL
        if (BEACON_COLOUR == c_BEACON_YELLOW) {
          int yPosition = goalBlob->maxY;
          int yPositionMax = MIN(yPosition+10, currentImageHeight_);
          int numOrange = 0;
          while (yPosition < yPositionMax) {
            if(visionData_->classified_[yPosition * currentImageWidth_ + cx] == c_FIELD_GREEN) { 
              numOrange=0;
              break;
            }
            if(visionData_->classified_[yPosition * currentImageWidth_ + cx] == c_BALL_ORANGE) {
              numOrange++;
            }
            yPosition++;
          }
          if (numOrange > 2) acceptGoal=false;
        }
      
        // another chcek. search up. if there's field green, fuck off out !
        int yPosition = goalBlob->minY;
        int yPositionMin = MAX(yPosition-10, 0);
        int numGreen = 0;
        while (yPosition > yPositionMin) {
          if(visionData_->classified_[yPosition * currentImageWidth_ + cx] == c_FIELD_GREEN) { 
            numGreen++;
          }
          yPosition--;
        }
        if (numGreen > 1 && visionObjects_[v].elevation_*180.0/PI < 2.0) acceptGoal=false;
      

#endif
        // VerifyBlob returns true if it is a legitiment goal
        int blobArea = width * height;
        if ((BEACON_COLOUR == c_BEACON_BLUE) && !VerifyBlob(goalBlob, blobArea, 0.2)) {
          acceptGoal = false; 
        }
      }
      if (acceptGoal && acceptGoal2) {
        numVisionObjects_++;

        // Determine the location of the left most goal post.
        if (visionData_->CheckForColourLeft(goalBlob, (goalBlob->minX+goalBlob->maxX)/2, (goalBlob->minY+goalBlob->maxY)/2, 30, c_WHITE)) {
          v = numVisionObjects_;
          visionObjects_[v].SetData(OT_GOAL_COLOUR_L, goalBlob, goalBlob, visionData_->CalculateHeading(goalBlob->minX), visionData_->CalculateElevation(cy), distance, cx, cy, confidence);
          visionData_->TransformPositionObject(&visionObjects_[v]);
          goalHeadingLeft_=visionObjects_[v].heading_;
          // If the goal post is not on the side of the image then store the object
          if (goalBlob->minX > GOAL_BLOB_DIST_TO_EDGE) {
            if (ignoreDistance) visionObjects_[v].distance_=-1.0;
            numVisionObjects_++; 
          }
        }
  
        // Determine the location of the right most goal post
        if (visionData_->CheckForColourRight(goalBlob, (goalBlob->minX+goalBlob->maxX)/2, (goalBlob->minY+goalBlob->maxY)/2, 30, c_WHITE)) {

          v = numVisionObjects_;
          visionObjects_[v].SetData(OT_GOAL_COLOUR_R, goalBlob, goalBlob, visionData_->CalculateHeading(goalBlob->maxX), visionData_->CalculateElevation(cy), distance, cx, cy, confidence);
          visionData_->TransformPositionObject(&visionObjects_[v]);
          goalHeadingRight_=visionObjects_[v].heading_;
        
          // If the goal post is not on the side of the image then store the object
          if (goalBlob->maxX < currentImageWidth_ - GOAL_BLOB_DIST_TO_EDGE) {
            if (ignoreDistance) visionObjects_[v].distance_=-1.0;        
            numVisionObjects_++;
          }
        }
        return 1; // we can't find more than one goal at once !
      } 
    }
  }
  // If we did not find a blue goal then look for the yellow goal.
  return -1; // A return value of -1 indicates that we need to search for a yellow goal
}

// This method is used to verify that the goal found is actually a goal
// The method searches down and if it finds white assumes that the goal is actually a beacon
int ObjectRecognition::VerifyGoals(Blob* goalBlob) {
  int whiteCount = 0;
  int leftX = goalBlob->minX+2;
  int rightX = goalBlob->maxX-2;

  // Now that we have the equation upon which we are meant to be operating perform our downwards search
  // Note: this doesn't work to well if the dog is lying on his side and a beacon lines up with the edge of a goal :)
  while(leftX < rightX) {    
    int valueY = goalBlob->maxY;
    //int difference = 0;
    while((valueY < currentImageHeight_) && ((valueY-goalBlob->maxY) < GOAL_Y_SEARCH)) {
      // The if statement essentially checks from either end of the goal and slowly works its way to wards the centre. 
      // This should minimise the cases where we have an obstruction at one end not at the other 

/*
visionData_->overlay[valueY * currentImageWidth_ + leftX] = c_ROBOT_RED;
visionData_->overlay[valueY * currentImageWidth_ + rightX] = c_ROBOT_RED;
*/

      if((visionData_->classified_[valueY * currentImageWidth_ + leftX] == c_WHITE) || (visionData_->classified_[valueY * currentImageWidth_ + rightX] == c_WHITE)) { 
        // We have discovered the colour white so this is not a goal so return false
        whiteCount++;
        if (whiteCount == 5) 
          return whiteCount;
      }
      valueY  = valueY + 1; // Move y down the image with a large tranition
    }
    leftX = leftX + GOAL_X_INCREMENT; // Update the position of the left and right x values
    rightX = rightX - GOAL_X_INCREMENT;
  }
  // Return true as we have not found any white beneath the goal in the pattern that we searched
return whiteCount;
}

// This method is repsonsible for working it's way through the vision object array and determining whether there are any duplicate objects present
// The duplicate objects are removed should they be found
void ObjectRecognition::CullRepeats() {
  bool found = true;
  while (found) {
    found = false;
    for (int i = 0; i < numVisionObjects_-1; i++) {
      if (visionObjects_[i].type_ != VisionObject::OT_BALL) {
        for (int j = i+1; j < numVisionObjects_; j++) {
          if (visionObjects_[i].type_ == visionObjects_[j].type_) {
            found = true;
            if (visionObjects_[i].distance_ < visionObjects_[j].distance_) {
              // cull j
              for (int k = j+1; k < numVisionObjects_; k++) {
                visionObjects_[k-1] = visionObjects_[k];
              }
            } else {
              // cull i
              for (int k = i+1; k < numVisionObjects_; k++) {
                visionObjects_[k-1] = visionObjects_[k];
              }
            }
            if (i != 0) {
              i--;
            }
            numVisionObjects_--;
            break;
          }
        }
      }
    } 
  }
}
 
// Checks that the blob is sufficiently close to the horizon to be considered as the specified object
bool ObjectRecognition::CloseToHorizon(int x, int y, const int MAX_DISTANCE) {
  if (visionData_->horizonLine.exists) {
    double distance = ABS((y - visionData_->horizonLine.m * x - visionData_->horizonLine.b)/((visionData_->horizonLine.m * visionData_->horizonLine.m)+1));
    double lineY = visionData_->horizonLine.m * x + visionData_->horizonLine.b;
    if ((distance > MAX_DISTANCE) && (y < lineY)) {
      //cout << "CloseToHorizon check failed: Ball Object Rejected" << endl;
      return false; // Return false if the distance from the horizon is excessive
    }
  }
  return true; // If the horizon line does not exist return true anyway as we cannot check the elevation
}

// Checks the ratio of the real area of the blob to the area used for the determination of such aspects as distance
// Note: The ratio limit specified must be between 0 and 1 as the blob area is always greater than the real area
bool ObjectRecognition::VerifyBlob(Blob* goalBlob, int testArea, const double RATIO_LIMIT) {
  if (testArea < 1) {
    return false;
  }
  return(((double)goalBlob->area/(double)testArea) > RATIO_LIMIT);
}

// Processes the image, locating objects and updating the shared data areas
void ObjectRecognition::FindVisionObjects(VisionData* vd) {
  visionData_ = vd;

  previousBallObject.type_ = VisionObject::OT_INVALID;
  for (int k = 0; k < numVisionObjects_; k++) {
    if (visionObjects_[k].type_ == VisionObject::OT_BALL) {
      memcpy(&previousBallObject,&visionObjects_[k], sizeof(VisionObject));
    }
  }

  numVisionObjects_ = 0;
  Initialize();
  FindBlobs();

  ellipseFitting.SetVisionData(vd);


  #ifdef _WIN32
    numBlobsVO_ = 0;
    for (int i = 0; i < NUMCOLOURS; i++) {
      numBlobsClone_[i] = visionData_->numBlobs_[i];
    }
    for (int j = 0; j < NUMCOLOURS; j++) {
      for (int k = 0; k < MAXBLOBS; k++) {
        blobsClone_[j][k] = visionData_->blobs_[j][k]; 
      }
    }
  #endif 

  FindBalls();
  FindBeacons();
  FindGoals();
  CullRepeats(); // remove repeated beacons etc (keeps the closest one)
}

// Calculate the confidence with which we believe the object discovered is a Ball
inline void ObjectRecognition::CalculateBallConfidence(VisionObject* visObject) {
  // Calculate the probability of the ball based on the ration of the balls area

  // Look at the actual number of pixels which make up the blob and the expected number of pixels given the size of the blob
  double blobArea = (visObject->topBlob_->maxX - visObject->topBlob_->minX + 1) * (visObject->topBlob_->maxY - visObject->topBlob_->minY + 1) * (PI/4.0);
  
  // Handle the possible divide by zero error - even if it should never occur
  if (blobArea == 0) {
    visObject->confidence_ = 0;    
    return;
  }  
  
  // Calculate the probability based on the area
  double areaProb = visObject->topBlob_->area/blobArea;
  
  // Scale the factor over an appropriate range
  areaProb = visionData_->NonLinearScaling(areaProb, 0.25, 0.75);

  // Calculate the probability based on the elevation of the ball
  double elevProb = 1.0 - visionData_->NonLinearScalin