objectrecognition.cc

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

  // tweaked for better measurements to this beacon.
  for (int j = 0; j < numBlobs_[c_BEACON_COLOUR]; j++) {
    otherBlob = &blobs_[c_BEACON_COLOUR][j];
    if (otherBlob->subsumed == false) {
      ellipseFitting.EnlargeBlobRadial(otherBlob, c_BEACON_COLOUR, 10);

      width  = otherBlob->maxX-otherBlob->minX;
      height = otherBlob->maxY-otherBlob->minY;
      if ((width >= MIN_BEACON_BLOB) && (height >= MIN_BEACON_BLOB)) {
        // upper = pink, lower = colour !
        if (MergeHalfBeacons(pinkBlob, otherBlob) && numBeacons < MAX_BEACONS) {
          if (AddBeacon(pinkBlob, otherBlob, OT_BEACON_PCOLOUR, 0)) numBeacons++;
        }

        // upper = colour, lower = pink!
        if (MergeHalfBeacons(otherBlob, pinkBlob) && numBeacons < MAX_BEACONS) {
          if (AddBeacon(otherBlob, pinkBlob, OT_BEACON_COLOURP, 1)) numBeacons++;
        }
      }
    }
  }
  return numBeacons;
}



bool ObjectRecognition::MergeHalfBeacons(Blob* upperBlob, Blob* lowerBlob) {
  // alternate method, uses angles to check if one blob if 'above' the others.
  // image seems to roll quite badly on fast head pans, hence the need for this code.

  int upperWidth, upperHeight, lowerWidth, lowerHeight;
  int avgWidth, avgHeight;
  int upperX, upperY, lowerX, lowerY, dx, dy;
  double ang, dr;
  // check beacon size. must be at least 2(w) x 3(h)
  upperWidth  = upperBlob->maxX-upperBlob->minX;
  upperHeight = upperBlob->maxY-upperBlob->minY;
  lowerWidth  = lowerBlob->maxX-lowerBlob->minX;
  lowerHeight = lowerBlob->maxY-lowerBlob->minY;
  avgWidth    = (lowerWidth +upperWidth )/2;
  avgHeight   = (lowerHeight+upperHeight)/2;
  if ((lowerWidth < MIN_BEACON_SIZE) || (lowerHeight < MIN_BEACON_SIZE) || (upperWidth < MIN_BEACON_SIZE) || (upperHeight < MIN_BEACON_SIZE))
    return false;

  //check width roughly same size
  //  if (ABS(lowerWidth-upperWidth) > avgWidth/2)
  //    return false;
  //  if (ABS(lowerWidth-upperWidth) > avgWidth/2)
  //    return false;

  upperX = upperBlob->minX + upperWidth/2;
  upperY = upperBlob->minY + upperHeight/2;
  lowerX = lowerBlob->minX + lowerWidth/2;
  lowerY = lowerBlob->minY + lowerHeight/2;
  dx = upperX-lowerX;
  dy = lowerY-upperY;
  dr = sqrt(double(dx*dx+dy*dy));

  //check distance is reasonable compared to blob size
  double size2 = sqrt(double(avgWidth*avgWidth + avgHeight*avgHeight));

// old version... robocup 2003
//if (dr > 2*size2) return false; 
  if (dr > size2 + 4) return false;

  //check that one blob is not more than twice the size of the other
  if (ABS(upperWidth -lowerWidth ) > MAX(upperWidth , lowerWidth )*1/2)
    return false;
  if (ABS(upperHeight-lowerHeight) > MAX(upperHeight, lowerHeight)*3/4)
    return false;

  //check dy is positive
  if (dy<0) return false;
  //check angle close 90
  ang = asin(dx/dr);
//double effectiveRoll = asin(visionData_->sinHeadPan_*visionData_->sinHeadTilt_); 
  double compareAng = RAD_TO_DEG(Normalise_PI(ang - visionData_->effHeadRoll_));
  
  visionData_->DrawOverlayLine(lowerX,lowerY,ang,10,c_ROBOT_RED);
  visionData_->DrawOverlayLine(lowerX,lowerY,visionData_->effHeadRoll_,10,c_ROBOT_BLUE);

  if ((compareAng < -20) || (compareAng > 20))
    return false;
  if (!visionData_->CheckForColourAngle(lowerBlob, lowerX, lowerY, 15, c_WHITE, DEG_TO_RAD(0.0)) && visionData_->useWhiteBelowBeaconCheck) return false;

  return true;
}

// Calculates the distance to the beacon
double ObjectRecognition::CalculateBeaconDistance(Blob* upperBlob, Blob* lowerBlob) {

  // do distance using infrared directly ..
#ifdef ERS_210
  double infraDistance = visionData_->GetInfraredDistance(upperBlob,c_BEACON_PINK); // actual colour doesn't matter - juts don't use orange here, since that's different ..
  if (infraDistance < 0) {
    infraDistance = visionData_->GetInfraredDistance(lowerBlob,c_BEACON_PINK); // as above
  }
  if (infraDistance >= 0) return infraDistance;
#endif

    double magicFactor = 2140.0; //new by CM. empirical.

  int upperCx = (upperBlob->maxX + upperBlob->minX)/2;
  int lowerCx = (lowerBlob->maxX + lowerBlob->minX)/2;
  int upperCy = (upperBlob->maxY + upperBlob->minY)/2;
  int lowerCy = (lowerBlob->maxY + lowerBlob->minY)/2;

  // projects height of upper beacon based on lower one... seems to do better, particularly when head tilt is low.
  // Removed bug which in previous code and added options for interfearence in either the top or bottom of the beacon
  // NOTE: may need to consider for interfearence in the x direction as well (or maybe not)

  int upperBlobHeight = upperBlob->maxY - upperBlob->minY;
  int lowerBlobHeight = lowerBlob->maxY - lowerBlob->minY;
  if (upperBlobHeight > lowerBlobHeight) {
    int newLowerBlobMaxY = lowerBlob->minY + upperBlobHeight;
    lowerCy = (newLowerBlobMaxY + lowerBlob->minY)/2;
    //upperCy = (upperBlob->maxY+upperBlob->minY)/2;
  }
  else {
    int newUpperBlobMinY = upperBlob->maxY - lowerBlobHeight;
    upperCy = (newUpperBlobMinY + upperBlob->maxY)/2;
    //lowerCy = (lowerBlob->maxY+lowerBlob->minY)/2;
  }

  double d = visionData_->GetDistance(upperCx, upperCy, lowerCx, lowerCy); //sqrt ( (double)((upperCx-lowerCx)*(upperCx-lowerCx)+(upperCy-lowerCy)*(upperCy-lowerCy) ));
  double distance = magicFactor/(d);

  return distance;
}

// find all beacons in the current image. also has to 'extend' partially visible beacon halves (?)
// or perhaps we can just use width for distance in that case
void ObjectRecognition::FindBeacons() {
//Run** runs_ = visionData_->runs_;
//int* numRuns_ = visionData_->numRuns_;
  Blob** blobs_ = visionData_->blobs_;
  int* numBlobs_ = visionData_->numBlobs_;

  // As all beacons contain pink we ensure that at least a single blob of pink is found before preceeding
  if (numBlobs_[c_BEACON_PINK] == 0) {
    return; // no pink ? no beacons !
  }

  int numBeacons = 0;
  Blob* pinkBlob;
  int width, height; 

  // Iterate through all of the pink and colour blobs and form them into beacons
  for (int i = 0; i < numBlobs_[c_BEACON_PINK]; i++) {
    pinkBlob = &blobs_[c_BEACON_PINK][i]; // Retrieve the blob from the array
    if (pinkBlob->subsumed == false) {

      ellipseFitting.EnlargeBlobRadial(pinkBlob, c_BEACON_PINK, 10);

      width  = pinkBlob->maxX-pinkBlob->minX;
      height = pinkBlob->maxY-pinkBlob->minY;
      numBeacons = 0;
      if ((width >= MIN_BEACON_BLOB) && (height >= MIN_BEACON_BLOB)) { // Check that the blob is large enough to be considered a reasonable chance of being a beacon

        numBeacons = FindHalfBeacons(c_BEACON_YELLOW, VisionObject::OT_BEACON_PY, VisionObject::OT_BEACON_YP, pinkBlob, 0);
        numBeacons += FindHalfBeacons(c_BEACON_BLUE, VisionObject::OT_BEACON_PB, VisionObject::OT_BEACON_BP, pinkBlob, 0);
// green beacons are gone gone.
//      numBeacons += FindHalfBeacons(c_BEACON_GREEN, VisionObject::OT_BEACON_PG, VisionObject::OT_BEACON_GP, pinkBlob, 0);

        // we formed more than one beacon from a single bit of pink. what ??
        // some have to go. find the lowest one and use it, basically ...
        if (numBeacons > 1) {
          int startIndex = numVisionObjects_-numBeacons;
          int endIndex = numVisionObjects_;
          int bestElevationIndex = 0;
          double bestElevation = 10.0;
          for (int b = startIndex; b < endIndex; b++) {
            if (visionObjects_[b].elevation_ < bestElevation) {
              bestElevationIndex = b;
              bestElevation = visionObjects_[b].elevation_;
            }
          }
          memcpy(&visionObjects_[startIndex], &visionObjects_[bestElevationIndex], sizeof(VisionObject));
          numVisionObjects_ -= (numBeacons-1);
        }
        if (numBeacons > 0) {
          visionData_->DeleteBlob(pinkBlob);
        }
      }
    }
  }
}


// Used when a beacon has been discovered and verfified to be a beacon
bool ObjectRecognition::AddBeacon(Blob *topBlob, Blob *bottomBlob, VisionObject::ObjectType type, int pinkIndex) {

  // Set the variables. cx and cy represent the centre of the object
  double distance = CalculateBeaconDistance(topBlob, bottomBlob);
  int cx = (topBlob->minX+topBlob->maxX+bottomBlob->minX+bottomBlob->maxX)/4;
  int cy = (topBlob->maxY+bottomBlob->minY)/2;

  visionObjects_[numVisionObjects_].SetData(type, topBlob, bottomBlob, visionData_->CalculateHeading(cx), visionData_->CalculateElevation(cy), distance, cx, cy, 0);
  visionData_->TransformPositionObject(&visionObjects_[numVisionObjects_]);

  double vertDistance = visionObjects_[numVisionObjects_].distance_*sin(visionObjects_[numVisionObjects_].elevation_);
// Changed by CM. Beacons are harder to fuck up with cos the other checks are pretty good...
  if ((distance < 200 && vertDistance > visionData_->maxHeight) || (distance >= 200 && vertDistance > visionData_->maxHeight*1.4)) {
    return 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
    CalculateBeaconConfidence(&visionObjects_[numVisionObjects_]);
    if (visionObjects_[numVisionObjects_].confidence_ == 0) {
      return false;
    }
  } else {
    // Elevation check added 14/6/2003 (CS)
#ifdef ERS_210
    double elevationCheck = 20.0;
    if (strcmp(currentDudeName_,"Walker")==0 || strcmp(currentDudeName_,"EHCM")==0) {
      elevationCheck = 50.0;
    }
#endif
#ifdef ERS_7
    double elevationCheck = 20.0;
    if (strcmp(currentDudeName_,"Walker")==0 || strcmp(currentDudeName_,"EHCM")==0) {
      elevationCheck = 60.0;
    }
#endif
    if (RAD_TO_DEG(visionObjects_[numVisionObjects_].elevation_) > elevationCheck) {
      return false;
    }
  }
/*
fix by cm. don't kill pink blobs here. we're going to form multiple beacons with it and choose the best
  if (pinkIndex == 0) {
    visionData_->DeleteBlob(bottomBlob);
    topBlob->area=0;
  } else {
    visionData_->DeleteBlob(topBlob);
    bottomBlob->area=0;
  }
*/
  numVisionObjects_++;
  return true;
}

// Find goals essentially locates the goal blobs and forms them into vision objects
void ObjectRecognition::FindGoals() {
  int returnValue = FindGoalsWork(c_BEACON_BLUE,VisionObject::OT_GOAL_B,VisionObject::OT_GOAL_B_L,VisionObject::OT_GOAL_B_R);

  // A return value of -1 indicates that we did not find a goal and need to search for the yellow goal
  if (returnValue == -1) {
    FindGoalsWork(c_BEACON_YELLOW,VisionObject::OT_GOAL_Y,VisionObject::OT_GOAL_Y_L,VisionObject::OT_GOAL_Y_R);
  }
}

int ObjectRecognition::FindGoalsWork(const Colour BEACON_COLOUR,const VisionObject::ObjectType OT_GOAL_COLOUR, const VisionObject::ObjectType OT_GOAL_COLOUR_L, const VisionObject::ObjectType OT_GOAL_COLOUR_R ) {
  // Beacon finding MUST be called before this, otherwise we may pick up beacons as goals (!!)
//Run** runs_ = visionData_->runs_;
//int* numRuns_ = visionData_->numRuns_;

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

  int confidence = -1;

  Blob* goalBlob;

  // Process for each blob of blue
  for (int i = 0; i < numBlobs_[BEACON_COLOUR]; i++) {
    goalBlob = &blobs_[BEACON_COLOUR][i];
    if (goalBlob->subsumed) continue;

    ellipseFitting.EnlargeBlobRadial(goalBlob, BEACON_COLOUR, 20);

    int width = goalBlob->maxX-goalBlob->minX;
    int height = goalBlob->maxY-goalBlob->minY;
    //  Check that the blob is roughly the right size
    if ( (width*height) > 80 && width > 15 && height > 8) {
      // found a goal !
      int cx = width/2+goalBlob->minX; // Determine the centre of the blob
      int cy = height/2+goalBlob->minY;

      bool valid = true;
      // At this stage the blob is considered to be a goal. Double check by ensuring the blob is not next to any pink blobs
      for (int k = 0; k < numBlobs_[c_BEACON_PINK]; k++) {
        Blob* beaconBlob = &blobs_[c_BEACON_PINK][k];
        if (beaconBlob->subsumed == false) {
          int cx2 = (beaconBlob->maxX+beaconBlob->minX)/2;
          int cy2 = (beaconBlob->maxY+beaconBlob->minY)/2;
          int w = beaconBlob->maxX-beaconBlob->minX;
          int h = beaconBlob->maxY-beaconBlob->minY;
          if (visionData_->GetDistance(cx, cy, cx2, cy2) < 100 && (w > 5 && h > 5)) {
            valid = false;
          }
        }
      }
      // This check simply ensures that we have not yet found a goal. If we have found a goal run the code below
      if (valid == false) {
        continue;
      }

      // Check that the distance is not affected by part of the goal being cropped by the edge of the image