ellipsefitting.cc

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

#include "stdafx.h" 
// File: EllipseFitting.cc
// Date: 30/03/03

#include <math.h> 
#include <float.h>
#include "../Common/Common.h"
#include "../Globals.h"
#include "EllipseFitting.h"

// Class constructor
EllipseFitting::EllipseFitting() { 
  numFittedPoints=0;
  fittedPoints = new point[DOUBLE_IMAGE_HEIGHT*2]; // Stores the points which have been found so far
}

EllipseFitting::~EllipseFitting() {
  delete fittedPoints;
}

void EllipseFitting::FindPoints(Blob* currentBlob, Colour blobColour,int direction) { 
  if (direction == TOPBOTTOM) {
    numFittedPoints=0;
    for (int col=currentBlob->minX; col < currentBlob->maxX; col++) {
      int currentLine = currentBlob->minY;
      while (currentLine < currentBlob->maxY) {
        if (visionData->classified_[currentLine*currentImageWidth_+col] == blobColour) {
          fittedPoints[numFittedPoints].x = col;
          fittedPoints[numFittedPoints].y = currentLine;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
//          visionData->overlay[currentLine*currentImageWidth_+col] = c_WHITE;
          }
          break;
        }
        currentLine++;
      }
    } 
  } else if (direction == BOTTOMTOP) {
    numFittedPoints=0;
    for (int col=currentBlob->minX; col < currentBlob->maxX; col++) {
      int currentLine = currentBlob->maxY;
      while (currentLine > currentBlob->minY) {
        if (visionData->classified_[currentLine*currentImageWidth_+col] == blobColour) {
          fittedPoints[numFittedPoints].x = col;
          fittedPoints[numFittedPoints].y = currentLine;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
            //visionData->overlay[currentLine*currentImageWidth_+col] = c_WHITE;
          }
          break;
        }
        currentLine--;
      }
    } 
  } else if (direction == LEFTRIGHT) {
    numFittedPoints=0;
    //use a for loop for each line which is in the object - defined by max and min Y
    for(int line=currentBlob->minY; line <= currentBlob->maxY; line++) {
      int currentRun=0;
    
      //access the runs for each line and determine whether the run is within the blob and the correct colour
      for(currentRun=0; currentRun < visionData->numRuns_[line]; currentRun++) {
        if((visionData->runs_[line][currentRun].startPoint >= currentBlob->minX) && (visionData->runs_[line][currentRun].colour == blobColour)){
          // Then we have found our left most point for this line - so add the point to the array
          fittedPoints[numFittedPoints].x = visionData->runs_[line][currentRun].startPoint;
          fittedPoints[numFittedPoints].y = line;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
           // visionData->overlay[line*currentImageWidth_ + fittedPoints[numFittedPoints-1].x] = c_BEACON_PINK;
          }
          break;
        } 
      }
    
      //search from the other end to access the right most run which is part of the blob and the correct colour
      for(currentRun=visionData->numRuns_[line]-1; currentRun >= 0; currentRun--) {
        if((visionData->runs_[line][currentRun].endPoint <= currentBlob->maxX) && (visionData->runs_[line][currentRun].colour == blobColour)){
          // Then we have found our right most point for this line - so add the point to the array
          fittedPoints[numFittedPoints].x = visionData->runs_[line][currentRun].endPoint;
          fittedPoints[numFittedPoints].y = line;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
          }
          break;
        } 
      }
    }
  } else if (direction == RIGHTLEFT) {
    numFittedPoints=0;
    //use a for loop for each line which is in the object - defined by max and min Y
    for(int line=currentBlob->minY; line <= currentBlob->maxY; line++) {
      int currentRun=0;
    
            //search from the other end to access the right most run which is part of the blob and the correct colour
      for(currentRun=visionData->numRuns_[line]-1; currentRun >= 0; currentRun--) {
        if((visionData->runs_[line][currentRun].endPoint <= currentBlob->maxX) && (visionData->runs_[line][currentRun].colour == blobColour)){
          // Then we have found our right most point for this line - so add the point to the array
          fittedPoints[numFittedPoints].x = visionData->runs_[line][currentRun].endPoint;
          fittedPoints[numFittedPoints].y = line;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
          }
          break;
        } 
      }

      //access the runs for each line and determine whether the run is within the blob and the correct colour
      for(currentRun=0; currentRun < visionData->numRuns_[line]; currentRun++) {
        if((visionData->runs_[line][currentRun].startPoint >= currentBlob->minX) && (visionData->runs_[line][currentRun].colour == blobColour)){
          // Then we have found our left most point for this line - so add the point to the array
          fittedPoints[numFittedPoints].x = visionData->runs_[line][currentRun].startPoint;
          fittedPoints[numFittedPoints].y = line;
          if ((fittedPoints[numFittedPoints].x > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].x < (currentImageWidth_ - MIN_POINT_DIST_FROM_EDGE)) && (fittedPoints[numFittedPoints].y > MIN_POINT_DIST_FROM_EDGE) && (fittedPoints[numFittedPoints].y < (currentImageHeight_ - MIN_POINT_DIST_FROM_EDGE))) {
            numFittedPoints++;
          }
          break;
        } 
      }
    
    }
  }
}

Circle EllipseFitting::FitEllipseLMA(VisionData* vd, Blob* ballBlob, int direction) {
  visionData = vd;
#ifdef WIN32
  FindPointsRadial(&numFittedPoints, fittedPoints, ballBlob,c_BALL_ORANGE, 10, false);
#endif
  FindPoints(ballBlob,c_BALL_ORANGE,direction); // Finds the points on the  edge of the ball
  if (numFittedPoints > 5) {
    Circle algebraicCircle = AlgebraicCircleFit(); // Generates an approximate centre using an algebraic approximation to the centre of the circle
    if (algebraicCircle.isDefined) {
      Circle geometricCircle = GeometricCircleFitLMA(algebraicCircle); // Uses the algebraic approximation to generate a more accurate centre and radius using geometric fitting
      return geometricCircle;
    }       
  }
  // If we make it to this point we cannot return a circle
  return InvalidCircle();  
}


Circle EllipseFitting::FitEllipseLMF(VisionData* vd, Blob* ballBlob, int direction) {
  visionData = vd;
#ifdef WIN32
  FindPointsRadial(&numFittedPoints, fittedPoints, ballBlob,c_BALL_ORANGE, 10, false);
#endif
  FindPoints(ballBlob,c_BALL_ORANGE,direction); // Finds the points on the  edge of the ball
  if (numFittedPoints > 5) {
    Circle algebraicCircle = AlgebraicCircleFit(); // Generates an approximate centre using an algebraic approximation to the centre of the circle
    if (algebraicCircle.isDefined) {
      Circle geometricCircle = GeometricCircleFitLMF(algebraicCircle); // Uses the algebraic approximation to generate a more accurate centre and radius using geometric fitting
#ifdef WIN32
      double x, y;
  	  double theta = 0.0;
  	  while (theta < 360.0) {
    		y = geometricCircle.centreY+geometricCircle.radius*sin(DEG_TO_RAD(theta));
    		x = geometricCircle.centreX+geometricCircle.radius*cos(DEG_TO_RAD(theta));
  	    theta+=0.5;
        if (x < currentImageWidth_ && y < currentImageHeight_ && x >= 0 && y >= 0) {
    		  visionData->overlay[(int)(y)*currentImageWidth_+(int)(x)] = c_BEACON_BLUE;
        }
  	  }
#endif

      return geometricCircle;
    }       
  }
  // If we make it to this point we cannot return a circle
  return InvalidCircle();  
}

// Returns an invalid circle which can be returned in cases where an error condition occurs
Circle EllipseFitting::InvalidCircle() {
  Circle invalidCircle;
  invalidCircle.isDefined = false;
  return invalidCircle;  
}


// Performs an algebraic fit to the points already stored in the fitted points array. 
// This gives us a rough approximation for the centre which we later use in the geometric ellipse fit
Circle EllipseFitting::AlgebraicCircleFit() {
	double sumXY,sumXX,sumYY,sumXZ,sumYZ;	
  double meanXY,meanXX,meanYY,meanXZ,meanYZ;	
	double B,C,G11,G12,G22,D1,D2;	
  // Routine that shifts the data to the center of mass (i.e. 0.0)
	meanX = meanY = 0; // The mean values represent the offset from the original postion in the image

  // Sum all the elements in the array
  for (int i=0; i < numFittedPoints; i++) {
		meanX += fittedPoints[i].x;
		meanY += fittedPoints[i].y;
	}

  // Calculate the average 
	meanX = (int) meanX/numFittedPoints;
	meanY = (int) meanY/numFittedPoints;

	// Shift the data to the mean
	for (int j=0; j < numFittedPoints; j++) {
		fittedPoints[j].x -= meanX;
		fittedPoints[j].y -= meanY;
	}

  // Now that all data has been shifted toward the centre we can continue
  // computing moments (note: all moments are normed, i.e. divided by N)
  double Xi,Yi,Zi;
  sumXY=sumXX=sumYY=sumXZ=sumYZ=0;
  for (int k=0; k < numFittedPoints; k++) {
    Xi = fittedPoints[k].x;
		Yi = fittedPoints[k].y;
		Zi = Xi*Xi + Yi*Yi;

		sumXY = sumXY + Xi*Yi;
		sumXX = sumXX + Xi*Xi;
		sumYY = sumYY + Yi*Yi;
		sumXZ = sumXZ + Xi*Zi;
		sumYZ = sumYZ + Yi*Zi;
	}

	meanXX = sumXX/numFittedPoints;
	meanYY = sumYY/numFittedPoints;
	meanXY = sumXY/numFittedPoints;
	meanXZ = sumXZ/numFittedPoints;
	meanYZ = sumYZ/numFittedPoints;

  // computing the circle parameters
	G11 = sqrt(meanXX);

  // Check that we have not generated an infinity error
	if ((G11 < DBL_MIN) || (G11 > DBL_MAX))
    return InvalidCircle();    
  
  G12 = meanXY/G11;
  
  // Check that the value to be squared rooted is not negative
  if ((meanYY - G12*G12) < 0) 
    return InvalidCircle();    
  
	G22 = sqrt(meanYY - G12*G12);

  // Checking that we have not generated an infinity error
  if ((G22 < DBL_MIN) || (G22 > DBL_MAX))
    return InvalidCircle();    
  

	D1 = meanXZ/G11;
	D2 = (meanYZ - D1*G12)/G22;
	C = D2/G22;
	B = (D1 - G12*C)/G11;

  Circle algebraicCircle; // Defines the circle which is returned as part the result from the method
  algebraicCircle.centreX = B/2; // Sets the x coordinate of the centre of the circle
	algebraicCircle.centreY = C/2; // Sets the y coordinate of the centre of the circle
	algebraicCircle.radius = sqrt(algebraicCircle.centreX*algebraicCircle.centreX+algebraicCircle.centreY*algebraicCircle.centreY+meanXX+meanYY); // Determine the radius of the centr of the circle
  algebraicCircle.sd = Sigma(algebraicCircle);
	algebraicCircle.isDefined = true; 
  return algebraicCircle;
}

// LMABEST - Note computationally expensive
 
Circle EllipseFitting::GeometricCircleFitLMA(Circle initialCircle) {
  int iteration;
   Xshift=0.0,Yshift=0.0,dX=0.0,dY=0.0,dMax=0.0;

  // Calculate the maximum standard deviation
  dMax = MaximumDeviation();
 
  if (!Initalise(initialCircle)) {
//     "FIXME:  Need to return an undefined circle and make sure its not updated at a later stage"

  }
  // initializing lambda and iteration
  lambda = 0.01; // Note 0.01 is arbitararily small number from which to start from
  iteration = 0;
 
  while(true) {
    oldA = newA;
    oldF = newF;
    oldT = newT;
    oldStandardDeviation = newStandardDeviation;
    oldCircle = newCircle;
   
  shiftXY:
    iteration++;
    if (iteration > MAX_ITERATIONS) break;

    ComputeMoments();
   
    // computing matrices
    try_again:

    CholeskyDecomposition(); 
     
    if ((1.0+4.0*newA*newF < LMAEPSILON) && (lambda > 1.0)) {
      double bTemp = UpdateOldParameters();
      if (bTemp > 0) oldT = 2.0*PI - oldT;
        goto shiftXY;
    }

    if (1.0+4.0*newA*newF < LMAEPSILON) {
      lambda *= FACTOR_UP;
      if ((iteration > MAX_ITERATIONS) || (lambda > LAMBDA_MAX)) break;
      else goto try_again;
    }
   
    UpdateCircleParameters();   
      
    // checking if improvement is gained
    if (newStandardDeviation <= oldStandardDeviation) {    //   yes, improvement
      if (Distance(newCircle,oldCircle) < LMAEPSILON) {
        oldA = newA;
        oldF = newF;
        oldT = newT;
        while (oldT > 2.0*PI) oldT-=2.0*PI;
        while (oldT < 0.0) oldT+=2.*PI;
        oldStandardDeviation = newStandardDeviation;  
        break;
      }
      lambda *= FACTOR_DOWN;
      if (lambda < LAMBDA_MIN) lambda=LAMBDA_MIN;
        continue;
    } else { //   no improvement