objectrecognition.cc

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

//    #include "stdafx.h" 
// Blob forming problems appear to all be fixed (-CM/NC)
// show some respect for the coding style and keep your tabs out of this class !
// {'s on separate lines are for the weak, and whitespace is for the feeble.

//#define OLD_FIND_BEACONS

#include "../Common/VisionObject.h"
#include "../Common/Common.h"
#include "ObjectRecognition.h"
#include "VisionData.h"
#include "../Globals.h" 
#include <math.h>
#include <list>


/* 
  Code to iterate through all blobs using nextBlob mechanism
  for (int colour = 0; colour < NUMCOLOURS; colour++) {
    Blob* blob = &blobs_[colour][0];
    if (blob->subsumed) blob = blob->nextBlob;
    while (blob !=  NULL) {
      // do stuff here !!
      blob = blob->nextBlob;
    }
  }
*/

ObjectRecognition::ObjectRecognition() {
  numBalls_ = 0; // obsolete- we don't actually have to track multiple balls for the ball
                 // challenge...
  balls_ = new VisionObject[MAXBALLS]; // Array if vision objects believed to be balls

  // used by the EOTN application
  #ifdef _WIN32
    blobsClone_ = new Blob*[NUMCOLOURS];
    numBlobsClone_ = new int[NUMCOLOURS];
    for (int k = 0; k < NUMCOLOURS; k++) {
      blobsClone_[k] = new Blob[MAXBLOBS];
      numBlobsClone_[k] = 0;
    }
    // Stores all the vision object blobs so they don't get deleted :)
    numBlobsVO_ = 0;
    pixg = new int[500];
  #endif 
}

// resets the counter arrays. better than reallocating memory. could use memset instead tho
void ObjectRecognition::Initialize() {
  for (int i = 0; i < DOUBLE_IMAGE_HEIGHT; i++) {
    visionData_->numRuns_[i] = 0;
  }
  for (int j = 0; j < NUMCOLOURS; j++) {
    visionData_->numBlobs_[j] = 0;
    visionData_->blobs_[j][0].nextBlob = NULL;
    visionData_->blobs_[j][0].prevBlob = &visionData_->blobs_[j][0];
    prevBlob[j] = NULL;
  }
  numBalls_ = 0;
}

inline void ObjectRecognition::ConsumeBlob(Blob* consumingBlob, Blob* consumedBlob) {
  if (consumedBlob->minX < consumingBlob->minX) {
    consumingBlob->minX = consumedBlob->minX;
    consumingBlob->minXy = consumedBlob->minXy;
  }
  if (consumedBlob->maxX > consumingBlob->maxX) {
    consumingBlob->maxX = consumedBlob->maxX;
    consumingBlob->maxXy = consumedBlob->maxXy;
  }
  if (consumedBlob->minY < consumingBlob->minY) {
    consumingBlob->minY = consumedBlob->minY;
    consumingBlob->minYx = consumedBlob->minYx;
  }
  if (consumedBlob->maxY > consumingBlob->maxY) {
    consumingBlob->maxY = consumedBlob->maxY;
    consumingBlob->maxYx = consumedBlob->maxYx;
  }
  consumingBlob->area += consumedBlob->area;
  
  if (consumedBlob->prevBlob != NULL) {
    consumedBlob->prevBlob->nextBlob = consumedBlob->nextBlob;
  }
  if (consumedBlob->nextBlob != NULL) {
  consumedBlob->nextBlob->prevBlob = consumedBlob->prevBlob;
  }
  //consumedBlob->nextBlob = NULL;
  //special case for front sentinel here..
  if (consumedBlob->prevBlob != consumedBlob) {
    consumedBlob->prevBlob = NULL;
  }
  consumedBlob->subsumed = true;
}


void ObjectRecognition::FindBlobs() {
  Run** runs_ = visionData_->runs_;
  Blob** blobs_ = visionData_->blobs_;
  int* numRuns_ = visionData_->numRuns_;
  int* numBlobs_ = visionData_->numBlobs_;
  for (int i = 0; i < NUMCOLOURS; i++) {
    colourCounter[i]=0;
  }

  //static bool secondPass = false;
  // NB-the last line of the camera is kind of borked, which is why we have EFFECTIVE_IMAGE_HEIGHT

  // normal horizontal 'run' finding is very simple. just increment along each line of the image and
  //   look for a pixel that's the same colour as the previous one you encountered - this is the start of the run.
  //   the run continues until you find a pixel that's a different colour, in which case you resume searching as before.

  // this method is similar, but it utilizes a specific pecularity of the images we're dealing with to be much faster.
  //   In particular, we're not interested in runs of every colour. Unclassified, white and green
  //   pixels are irrelevant, and yet these make up MOST of each image. This means that we should not actually
  //   consider the whole image - we need only consider every SECOND pixel. When we encounter a pixel that's
  //   'interesting' (ie, not unclassified, white or green), we check to see if the previous pixel (which we
  //   skipped over) is the same colour- if so, we have a run. this is a minor simplification of the process,
  //   however (eg, there is a certain configuration that requires a run to be created with an initial size of 3
  //   pixels; This is a pain to handle).
  // note that on an image with 100% 'interesting' pixels, this method will both suck and blow at the
  //   same time. still, it's definitely worth it in this case - despite the slightly tricky implementation.

  unsigned char* p = visionData_->classified_;
  unsigned char* h = visionData_->classified_;

  int incrementAmount = 2;
  for (int line = 0; line < currentImageHeight_-1; line++) { // last line is b0rked, hence the -1
    p = h+1; //Note this has been changed from p=h (28-05-2003)
    h += currentImageWidth_; // h is now on the next line of the image
    int runNumber = 0;
    int currentRun = 0;
    int lastColour = -1;
    bool inRun = false;
    int w = 0;
    while (w < currentImageWidth_) {
      int currentColour = *p;
      colourCounter[*p]++;
      if (currentColour >= c_BALL_ORANGE) { // ie, not unknown, white or field green
                                            // this is why the colour enum is constructed how it is...
        int modifier = 0;
        if (incrementAmount == 2) { // we WERE going through pixels 2 at a time, but we encountered an 'interesting' colour
          // FIXED - *(p-1) is not necessarily within the image. this isn't handled?? perhaps start on pixels+1 to fix
          if (*(p-1) != currentColour) { // if the previous pixel was NOT interesting
            incrementAmount = 1; // change to only go through pixels 1 at a time
            p++; w++; // increment our place in the image array, and increment our place in this line
            lastColour = currentColour;
            continue; // go back to start of while loop - we don't form a run unless we find TWO pixels that are the same.

          // else branch - previous pixel was the same as this one.
          } else if (lastColour == currentColour) { // now, if the pixel previous to the previous pixel (!) was ALSO
                                                    // the same colour, we have to adjust our run to include it (below)
                                                    // that is, we have a run of at least 3 pixels
            modifier = 1;
          } else {
            // we have a run of at least 2 pixels
            lastColour = currentColour;
          }
        }
        if (currentColour == lastColour) {
          if (inRun) {
            runs_[line][currentRun].endPoint = w;
          } else {
            inRun=true;
            currentRun = runNumber++;
            runs_[line][currentRun].startPoint = w-1-modifier; // modifier allows us to get runs of 3 pixels
            runs_[line][currentRun].endPoint = w;
            runs_[line][currentRun].colour = currentColour;
            runs_[line][currentRun].line = line;
            runs_[line][currentRun].blobNumber = numBlobs_[currentColour]++;
          }
        } else {
          // no longer in a run- current colour != lastcolour. but we still have an interesting colour, so
          // keep going through the image one at a time (we may be about to start another run)
          inRun = false;
        }
        // since we saw an interesting colour, we want to go through the image one at a time
        incrementAmount = 1;
      }
      else {
        // did NOT see an interesting colour - so go through the image faster. also we're not in a run anymore.
        incrementAmount=2;
        inRun = false;
      }
      // gogo!
      p+=incrementAmount;
      w+=incrementAmount;
      lastColour = currentColour;
    }
    for (int k = 0; k < runNumber; k++) {
      AddRunToBlob(runs_[line][k]);
    }
    numRuns_[line] = runNumber;
    inRun = false;
  }


// VERY OLD BLOB FORMATION !!
/*
  for (int line = 0; line < currentImageHeight_-1; line++) {
    int runNumber = 0;
    int lastColour = -1;
    bool inRun = false;
    for (int w = 0; w < currentImageWidth_; w++) {
      int currentColour = *p++;
      colourCounter[currentColour]++;

      if (currentColour == lastColour) {
        // we can't find a run of green or of unknown.
        if (inRun) {
          runs_[line][runNumber].endPoint = w;
        }
      }
      else {
        // we were in a run.
        if (inRun) {
          AddRunToBlob(runs_[line][runNumber]);
          runNumber++;
          if (runNumber >= MAXRUNSPERLINE) {
            break;
          }
        }

        if (currentColour > c_WHITE) {
          inRun=true;
          runs_[line][runNumber].startPoint = w-1;
          runs_[line][runNumber].endPoint = w;
          runs_[line][runNumber].colour = currentColour;
          runs_[line][runNumber].line = line;
          runs_[line][runNumber].blobNumber = numBlobs_[currentColour]++;
          if (numBlobs_[currentColour] >= MAXBLOBS) {
            break;
          }
        }
        else {
          inRun = false;
        }
      }
      lastColour = currentColour;
    }
    // we were in a run when we hit the side of the image. dirty code. how can i fix ?
    if (inRun) {
      AddRunToBlob(runs_[line][runNumber]);
      runNumber++;
    }
    numRuns_[line] = runNumber; 
    inRun = false;
  }
*/
// merge any and all overlapping blobs of the same colour !
// keep track of how many merges. we don't need to, but it's useful for debugging wrong code
  int merges = 0;
  for (int currentLine = 1; currentLine < currentImageHeight_-1; currentLine++) {
    int previousLine = currentLine-1;
    for (int currRun = 0; currRun < numRuns_[currentLine]; currRun++) {
      for (int abRun = 0; abRun < numRuns_[previousLine]; abRun++) {
        Run* currentRun = &runs_[currentLine][currRun];
        Run* aboveRun = &runs_[previousLine][abRun];
        Blob* aboveBlob = &blobs_[aboveRun->colour][aboveRun->blobNumber];
        Blob* currentBlob = &blobs_[currentRun->colour][currentRun->blobNumber];

        // require same colour
        if (aboveRun->colour == currentRun->colour && aboveBlob->subsumed == false) {
          bool merge = true;
          // check if NO overlap. this is easier than checking for overlap (which take 4 conditions)
          if (aboveRun->endPoint < currentRun->startPoint) merge = false;
          if (aboveRun->startPoint > currentRun->endPoint) merge = false;
          if (merge) {
            merges++;
            if (aboveBlob != currentBlob) {
              // aboveBlob subsumes currentBlob
              ConsumeBlob(aboveBlob, currentBlob);
            }
            currentRun->blobNumber = aboveRun->blobNumber;
          }
        }
      }
    }
  }

  // There are special cases where blobs that should be merged are separated.
  // Account for them here.
  int iterations = 0;
  bool reconsiderCurrent = false;
  for (int colour = 0; colour < NUMCOLOURS; colour++) {
    Blob* consumingBlob = &blobs_[colour][0];
    if (consumingBlob->subsumed) consumingBlob = consumingBlob->nextBlob;

    while (consumingBlob !=  NULL) {
      Blob* consumableBlob = consumingBlob->nextBlob;
      while (consumableBlob != NULL) {
        iterations++;

        bool mergeBlobsX = true;
        if ((consumableBlob->minX > consumingBlob->maxX) || (consumableBlob->maxX < consumingBlob->minX)) {
          mergeBlobsX=false;
        }

        if (mergeBlobsX) {
          if ((consumableBlob->minY <= 1+consumingBlob->maxY) && (consumableBlob->maxY >= consumingBlob->minY)) {
            ConsumeBlob(consumingBlob, consumableBlob);
                   
            // consumingBlob just got enlarged. That means we have to reconsider it all over again (so don't increment through the consumingBlobs below..)