objectrecognition.cc

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

        VisionObject tempBall;
        if (pixelDistance < 5) pixelDistance = distance;
        tempBall.SetData(VisionObject::OT_BALL, currentBlob, currentBlob, visionData_->CalculateHeading(currentBlob->minYx), visionData_->CalculateElevation(centreY), pixelDistance, centreX, currentBlob->minY, 0);
        visionData_->TransformPositionObject(&tempBall);

        bool foundGreen = false;
        bool foundWhite = false;

        int j = 0;
        // must find white or green to the sides to detect a ball
        double xPos = centreX;
        double yPos = centreY;
        double xInc = cos(visionData_->effHeadRoll_);
        double yInc = sin(visionData_->effHeadRoll_);
        int xPosInt = (int)xPos;
        int yPosInt = (int)yPos;
/*      while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
          xPos+=xInc;
          yPos+=yInc;
          xPosInt = (int)xPos;
          yPosInt = (int)yPos;
        }

        for (j = 0; j < 30; j++) {
          if (yPosInt >= currentImageHeight_-10 || xPosInt >= currentImageWidth_-10 || xPosInt < 10) {
            foundWhite=true;
            foundGreen=true;
            break;
          }
          if (yPosInt < 0) break;
    #ifdef WIN32
          visionData_->overlay[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] = c_BEACON_BLUE;
    #endif
        if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_WHITE) {
          foundWhite=true;
          break;
        } 
        if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_FIELD_GREEN) {
          foundGreen=true;
          break;
        }
        if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_BEACON_BLUE) {
          foundGreen=true;
          break;
        }

          xPos+=xInc;
          yPos+=yInc;
          xPosInt = (int)xPos;
          yPosInt = (int)yPos;
          
        }
        if (!foundWhite && !foundGreen) {
          continue;
        }

          xPos = centreX;
        yPos = centreY;
        xInc *= -1;
        yInc *= -1;
        xPosInt = (int)xPos;
        yPosInt = (int)yPos;
        while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
          xPos+=xInc;
              yPos+=yInc;
          xPosInt = (int)xPos;
          yPosInt = (int)yPos;
        }
        foundGreen = false;
        foundWhite = false;
        for (j = 0; j < 30; j++) {
          if (yPosInt >= currentImageHeight_-10 || xPosInt >= currentImageWidth_-10 || xPosInt < 10) {
            foundWhite=true;
            foundGreen=true;
            break;
          }
          if (yPosInt < 0) break;
    #ifdef WIN32
          visionData_->overlay[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] = c_BEACON_BLUE;
    #endif
        if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_WHITE) {
          foundWhite=true;
          break;
        } if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_FIELD_GREEN) {
          foundGreen=true;
          break;
        }
        if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_BEACON_BLUE) {
          foundGreen=true;
          break;
        }

          xPos+=xInc;
          yPos+=yInc;
          xPosInt = (int)xPos;
          yPosInt = (int)yPos;
          
        }
        if (!foundWhite && !foundGreen) {
          continue;
        }
*/
 


      // chec kgreen below !
       foundGreen = false;
       xPos = centreX;
       yPos = centreY;
       xInc = cos(visionData_->effHeadRoll_+PI/2.0);
       yInc = sin(visionData_->effHeadRoll_+PI/2.0);
       xPosInt = (int)xPos;
       yPosInt = (int)yPos;
       while (xPosInt < currentBlob->maxX && xPosInt >= currentBlob->minX && yPosInt < currentBlob->maxY && yPosInt >= currentBlob->minY) {
         xPos+=xInc;
         yPos+=yInc;
         xPosInt = (int)xPos;
         yPosInt = (int)yPos;
       }

       if (colourCounter[c_ROBOT_RED] > 5) {
         while (true) {
           if (yPosInt >= currentImageHeight_-5 || xPosInt >= currentImageWidth_ || xPosInt < 0) {
              // if we fucked up downwards, and small ball, fuck off  - not the ball
             if (yPosInt >= currentImageHeight_-5 && ABS(distance) > 150) {
                foundGreen=false;
                break;
             }
             foundGreen=true;
             break;
           }
           if (yPosInt < 0) break;
     #ifdef WIN32
           visionData_->overlay[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] = c_BEACON_BLUE;
     #endif
         if (visionData_->classified_[(int)(yPosInt)*currentImageWidth_+(int)(xPosInt)] == c_FIELD_GREEN) {
           foundGreen=true;
           break;
         }
           xPos+=xInc;
           yPos+=yInc;
           xPosInt = (int)xPos;
           yPosInt = (int)yPos;
         
         }
         if (!foundGreen) {
            continue;
          }
        }

        visionData_->TransformPositionObject(&balls_[numBalls_]);

        double vertDistance = balls_[numBalls_].distance_*sin(balls_[numBalls_].elevation_);
        if (vertDistance > visionData_->maxHeight) {
          continue;
        }
          
        if (lowTable && balls_[numBalls_].distance_ > 35) continue;

        if (ignoreDistance) { // Checks that the ball should not be ignored because of boundary constraints and that the distance returned is not totally rediculous
          // balls_[numBalls_].distance_=-1.0; -- Note: In cases where we cannot determine a distance simply return a negative distance so Bruce Lee knows roughly how far away the ball is
          balls_[numBalls_].distance_ *= -1;
        }

        // Check if we are using sanity factors and if not then perform the old sanity check
        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
          CalculateBallConfidence(&balls_[numBalls_]);
          if (balls_[numBalls_].confidence_ == 0) {
            continue;
          }
        } else {
          // ball elevation sanity check !! in fukuoka, we kept seeing sexy heads at ground level and attacking them
          // CS/CM - Elevation check fixed to 1.0 degree 12/02/2003
// FIXME problem here (-CM)
#ifdef ERS_7
//        cout << "e: " << (balls_[numBalls_].elevation_*180.0/PI) << endl << flush;
        if (RAD_TO_DEG(balls_[numBalls_].elevation_) > 5.0) {
/*        if (RAD_TO_DEG(balls_[numBalls_].elevation_) < 6.5) {
            if (!visionData_->CheckForMultipleColourAngle(currentBlob, (currentBlob->minX+currentBlob->maxX)/2, (currentBlob->minY+currentBlob->maxY)/2, 8, c_FIELD_GREEN, 5, -PI/2.0)) continue;
          } else {
            continue;
          }*/


/*
          if (RAD_TO_DEG(balls_[numBalls_].elevation_) < 5.0) {
            if (previousBallObject.type_ == VisionObject::OT_BALL) {
              double distanceBetweenBlobCentres = sqrt(pow(previousBallObject.centreX_-balls_[numBalls_].centreX_,2.0)+pow(previousBallObject.centreY_-balls_[numBalls_].centreY_,2.0));
              if (distanceBetweenBlobCentres > 15) continue;
            } else {
              continue;
            }
          } else {
            continue;
          }*/
          continue;
        }
#endif
#ifdef ERS_210
          if (balls_[numBalls_].elevation_*180.0/PI > 1.0) {
            continue;
          }
#endif
        }
#ifdef WIN32
  ellipseFitting.EnlargeBlobRadial(balls_[numBalls_].topBlob_, c_BALL_ORANGE, 10);
#endif
/*
        bool foundOrange = false;
        // 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_->classified2_[iY * currentImageWidth_ + iX] == c_BALL_ORANGE) { 
              foundOrange = true;
              break;
            }
         }
        }

        if (!foundOrange) {
           OutputDebugString("No orange found\n");
           continue;
        } else {
           OutputDebugString("Ball fine\n");
        } */

        // ok, this is a harsh harsh check. but it's about time we fucken did it.
        /*int yPosition = currentBlob->maxY;
        int yPositionMax = MIN(yPosition+20, currentImageHeight_);
        int numWhite = 0;
        int numGreen = 0;
//      bool acceptBall = false;
        while (yPosition < yPositionMax) {
          if(visionData_->classified_[yPosition * currentImageWidth_ + centreX] == c_FIELD_GREEN) { 
            numGreen++;
          }
          if(visionData_->classified_[yPosition * currentImageWidth_ + centreX] == c_WHITE) {
            numWhite++;
          }
          yPosition++;
        }
        if (numWhite > 4 && numGreen < 2) {
          continue;
        }*/
        
        // Now that we think we have a ball -> redo the transform which fixes elevation
        balls_[numBalls_].elevation_ = visionData_->realElevation_;
        
        // Updates the pointers to the blob with the largest area
        if ((currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY) > largestArea) {
          largestIndex = numBalls_;
          largestArea = (currentBlob->maxX-currentBlob->minX)*(currentBlob->maxY-currentBlob->minY);
        }

        //if (RAD_TO_DEG(balls_[numBalls_].elevation_) < -0/* && balls_[numBalls_].distance_ > 20*/) {
        //  cout << "FUCK - " << RAD_TO_DEG(balls_[numBalls_].elevation_) << " " << balls_[numBalls_].distance_ << endl << flush;
  //         balls_[numBalls_].distance_ = 20;
        //}
  
        // Increment the counter storing the ball provided it has passed the sanity checks or sanity factors
        numBalls_++;
        topBallElevation_ = tempBall.elevation_;
      }
    }
  }

  // Update the ball pointer to point to the object which is largest blob which could represent a ball
  if (largestIndex != -1) {
    visionData_->ball_ = &balls_[largestIndex];
    visionObjects_[numVisionObjects_++] =  *((VisionObject*) visionData_->ball_);
  }
}
    
double ObjectRecognition::RatioDistanceCalc(Blob* ballBlob, bool sideProblem) {
  // Generates the distance to the ball using pythagorian ratios
  double a,b;
  if (sideProblem) {
    a = (ballBlob->maxX - ballBlob->minX);
    b = (ballBlob->maxY - ballBlob->minY)/2;
  } else {
    a = (ballBlob->maxY - ballBlob->minY);
    b = (ballBlob->maxX - ballBlob->minX)/2;
  }
  return ((SQUARE(a) + SQUARE(b))/(2*a));
}


int ObjectRecognition::FindHalfBeacons(const Colour c_BEACON_COLOUR, const VisionObject::ObjectType OT_BEACON_PCOLOUR, const VisionObject::ObjectType OT_BEACON_COLOURP, Blob* pinkBlob, int numBeacons) {
  // special check = if already used pink for a beacon, fuck off!
  if (pinkBlob->area == 0) {
    return 0;
  }
  //Run** runs_ = visionData_->runs_;
  //int* numRuns_ = visionData_->numRuns_;
  Blob** blobs_ = visionData_->blobs_;
  int* numBlobs_ = visionData_->numBlobs_;

  Blob* otherBlob;
  int width, height;

  // Iterate through all of the pink and coloured blobs and form them into beacons