ellipsefitting.cc

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

      newCircle.centreX = oldCircle.centreX - dX;
      newCircle.centreY = oldCircle.centreY - dY;
      newCircle.radius = oldCircle.radius - dR;
      newCircle.sd = Sigma(newCircle);

      if (fabs(newCircle.centreX)>MAX_PARLIMIT || fabs(newCircle.centreY)>MAX_PARLIMIT) {
        oldCircle.centreX = oldCircle.centreX + meanX;
        oldCircle.centreY = oldCircle.centreY + meanY;
        oldCircle.isDefined = true;
        return oldCircle;
      }

      iteration++;

      // Increment the iteration counter and check if the maximum number of iterations has been reached
      // If so return the last circle found
      if (newCircle.sd <= oldCircle.sd) {   //   yes, improvement
        if (Distance(newCircle,oldCircle) < EPSILON) {
          oldCircle.centreX = oldCircle.centreX + meanX;
          oldCircle.centreY = oldCircle.centreY + meanY;
          oldCircle.isDefined = true;
          return oldCircle; 
        }
        lambda *= FACTOR_DOWN;
        break;
      }	else {                      //   no improvement
        if (iteration > MAX_ITERATIONS) {
          oldCircle.centreX = oldCircle.centreX + meanX;
          oldCircle.centreY = oldCircle.centreY + meanY;
          oldCircle.isDefined = true;
          return oldCircle;
        }
        lambda *= FACTOR_UP;
        continue;
      }
    } 
  }
}


double EllipseFitting::Distance (Circle One, Circle Two) {
	double dist = (fabs(One.centreX-Two.centreX) + fabs(One.centreY-Two.centreY) + fabs(One.radius-Two.radius))/(One.radius + Two.radius);
	return dist;
}

double EllipseFitting::Sigma (Circle circle) {
	double sum = 0.,dx,dy,di;

	for (int i=0; i<numFittedPoints; i++) {
		dx = fittedPoints[i].x - circle.centreX;
		dy = fittedPoints[i].y - circle.centreY;
		di = sqrt(dx*dx+dy*dy) - circle.radius;
		sum += di*di;
	}
	return sqrt(sum/numFittedPoints);
}

// The following method attempts to calculate the radius and the centre using the three points provided 
// and by looking at the interestion of the bisectors

Circle EllipseFitting::ThreePointFit(VisionData* vd, point point1, point point2, point point3) {
  visionData = vd;
  // point centre;
  // GetCenter(point1,point2,point3,&centre) // Determines the centre and checks whether we can generate a circle
  // resultCircle.centreX = centre.x;
  // resultCircle.centreY = centre.y;
  Circle resultCircle;
  resultCircle.centreX = -1;
  resultCircle.centreY = -1;  
  resultCircle.radius = GetRadius(point1,point2,point3);
  resultCircle.sd = -1;
  resultCircle.isDefined = true;
  return resultCircle;
}

bool EllipseFitting::GetCenter(point p1, point p2, point p3, point* center) { 

  double x=0,y=0;
  double ma=0,mb=0;

  bool result = true;
  // we don't want infinite slopes or 0 slope for line 1, since we'll divide by "ma" below 
  if ((p1.x == p2.x) || (p1.y == p2.y)) Swap(p2,p3);
  if (p2.x == p3.x) Swap(p1,p2);

  if (p1.x != p2.x) ma = ((p2.y-p1.y)/(p2.x-p1.x));
  else result = false;
  if (p2.x != p3.x) mb = ((p3.y-p2.y)/(p3.x-p2.x));
  else result = false;
  if ((ma == 0) && (mb == 0)) result = false;
  if (result) {
    x =(ma*mb*(p1.y-p3.y)+mb*(p1.x+p2.x)-ma*(p2.x+p3.x))/(2*(mb-ma));
    y =-(x-(p1.x+p2.x)/2)/ma + (p1.y+p2.y)/2;
    center->x=(int) x;
    center->y=(int) y;
  } 
  return result;
}

// Uses the distance R= (abc/4K) where a,b,c are the lengths of the three sides and K is the area
double EllipseFitting::GetRadius(point p1, point p2, point p3) {
  // Return the radius of a circle defined by 3 points on it's circumference
  double s;
  double a,b,c;
  a = visionData->GetDistance(p1.x,p1.y,p2.x,p2.y); // Note this is just the distance formula
  b = visionData->GetDistance(p2.x,p2.y,p3.x,p3.y); 
  c = visionData->GetDistance(p3.x,p3.y,p1.x,p1.y); 
  s=(a+b+c)/2;
  return ((a*b*c)/(4*sqrt(s*(s-a)*(s-b)*(s-c))));
}

void EllipseFitting::Swap(point p1, point p2) { 
  // Note the points should be changed to use pointers
  point temp;
  temp=p1; 
  p1=p2; 
  p2=temp;
}

void EllipseFitting::EnlargeBlobRadial(Blob* blob, Colour c, int numRays) {
  return;

  int nFP = 0;
  point* fP = new point[numRays*2];
  int t = 1;
  FindPointsRadial(&nFP, fP, blob, c, numRays, true);
  for (int j = 0; j < nFP; j++) {
    if ((fP[j].x-t) > blob->maxX) {
      blob->maxX = fP[j].x-t;
      blob->maxXy = fP[j].y;
    }
    if ((fP[j].x+t) < blob->minX) {
      blob->minX = fP[j].x+t;
      blob->minXy = fP[j].y;
    }
    if ((fP[j].y-t) > blob->maxY) {
      blob->maxY = fP[j].y-t;
      blob->maxYx = fP[j].x;
    }
    if ((fP[j].y+t) < blob->minY) {
      blob->minY = fP[j].y+t;
      blob->minYx = fP[j].x;
    }
#ifdef WIN32
    visionData->overlay[fP[j].y*currentImageWidth_+fP[j].x] = c_WHITE;
#endif
  }
  
#ifdef WIN32
  for (int y = blob->minY; y < blob->maxY; y++) {
    if (y < 0 || y >= currentImageHeight_) continue;
    if (blob->minX >= 0 && blob->minX < currentImageWidth_) {
      visionData->overlay[y*currentImageWidth_+blob->minX] = c_BEACON_GREEN;
    }
    if (blob->maxX-1 >= 0 && blob->maxX-1 < currentImageWidth_) {
      visionData->overlay[y*currentImageWidth_+blob->maxX-1] = c_BEACON_GREEN;
    }
  }
  for (int x = blob->minX; x < blob->maxX; x++) {
    if (x >= 0 && x < currentImageWidth_) {
      visionData->overlay[blob->minY*currentImageWidth_+x] = c_BEACON_GREEN;
    }
    if (blob->maxY-1 >= 0 && blob->maxY-1 < currentImageHeight_) {
      visionData->overlay[(blob->maxY-1)*currentImageWidth_+x] = c_BEACON_GREEN;
    }
  }
#endif        
     

  delete fP;
}

void EllipseFitting::FindPointsRadial(int* nFP, point* fP, Blob* currentBlob, Colour blobColour, int numRays, bool includeImageEdge) {
  return;
  /*
  int singleEdgeThreshold = 9;
  int doubleEdgeThreshold = 6;
  int wrongColourThreshold = 3;*/

  int singleEdgeThreshold = 6;
  int doubleEdgeThreshold = 4;
  int wrongColourThreshold = 4;

  if (blobColour == c_BEACON_YELLOW) {
    singleEdgeThreshold = 7;
    doubleEdgeThreshold = 4;
    wrongColourThreshold = 1;
  }
  
  (*nFP) = 0;
  int centreX = (currentBlob->minX+currentBlob->maxX)/2;
  int centreY = (currentBlob->minY+currentBlob->maxY)/2;

  int currentRay = 0;
  double thetaDeg = 0;
  while (currentRay < numRays) {
    double thetaRad = DEG_TO_RAD(thetaDeg);
    double xPos = centreX;
    double yPos = centreY;
    double xInc = cos(thetaRad);
    double yInc = sin(thetaRad);
    int xPosInt = (int)xPos;
    int yPosInt = (int)yPos;
    int lastXPosInt = (int)xPos;
    int lastYPosInt = (int)yPos;


    unsigned char prevYComponent = visionData->unclassified_[yPosInt*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];
    unsigned char prevUComponent = visionData->unclassified_[(yPosInt+1)*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];
    unsigned char prevVComponent = visionData->unclassified_[(yPosInt+2)*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];


    int edgeFound = 0;
    int edgeDone = 0;
    int numWrongColours = 0;
    while (xPosInt < currentImageWidth_ && yPosInt < currentImageHeight_ && xPosInt >= 0 && yPosInt >= 0) {
      unsigned char currYComponent = visionData->unclassified_[yPosInt*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];
      unsigned char currUComponent = visionData->unclassified_[(yPosInt+1)*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];
      unsigned char currVComponent = visionData->unclassified_[(yPosInt+2)*(currentImageWidth_*RAW_MULTIPLIER)+xPosInt];
//visionData->classified_[yPosInt*currentImageWidth_+xPosInt] = c_ROBOT_BLUE;
#ifdef WIN32
visionData->overlay[yPosInt*currentImageWidth_+xPosInt] = c_ROBOT_BLUE;
#endif
      // special sanity check for yellow/orange problems ..
      if (visionData->classified_[yPosInt*currentImageWidth_+xPosInt] != blobColour &&visionData->classified_[yPosInt*currentImageWidth_+xPosInt] != c_UNKNOWN) {
        numWrongColours++;
        if (numWrongColours > wrongColourThreshold) break;
      }
      // if we're classified orange, DEFINITELY not an edge. if edgeDone!=0, we're just waiting for a ball orange in case our 'edge' wasn't an edge.
      if (visionData->classified_[yPosInt*currentImageWidth_+xPosInt] != blobColour && edgeDone==0) {
        int yChange = ABS(currYComponent-prevYComponent);
        if (yChange > singleEdgeThreshold) {
          fP[*nFP].x = xPosInt;
          fP[*nFP].y = yPosInt;
          (*nFP)++;
          // ok we think we have an edge => edgeDone = 1. but we don't quit out until we don't
          // find another orange for a little while
          edgeDone = 1;
          //break;
        }
        if (yChange > doubleEdgeThreshold) {
          if (edgeFound > 1) {
            fP[(*nFP)].x = xPosInt;
            fP[(*nFP)].y = yPosInt;
            (*nFP)++;
            edgeDone = 1;
            //break;
          }
          edgeFound++;
        }
      } else if (visionData->classified_[yPosInt*currentImageWidth_+xPosInt] == blobColour) {
        edgeFound=0;
        if (edgeDone != 0) {
          (*nFP)--;
          edgeDone=0;
        }
      }
      if (edgeDone != 0)
        edgeDone++;

      if (edgeDone > 5) break;

      prevYComponent = currYComponent;
      prevUComponent = currUComponent;
      prevVComponent = currVComponent;

      xPos+=xInc;
      yPos+=yInc;
      lastXPosInt = xPosInt;
      lastYPosInt = yPosInt;
      xPosInt = (int)xPos;
      yPosInt = (int)yPos;
    }
    // we went off edge of image.
    if (includeImageEdge) {
      if (xPosInt >= currentImageWidth_ || yPosInt >= currentImageHeight_ || xPosInt < 0 || yPosInt < 0) {
        if (edgeDone==0) {
          fP[*nFP].x = lastXPosInt;
          fP[*nFP].y = lastYPosInt;
          (*nFP)++;
        }
      }
    }

    thetaDeg+=360.0/numRays;
    currentRay++;

  }
  double totalX = 0;
  double totalY = 0;
  double cX = 0;
  double cY = 0;
  int i = 0;
  for (i = 0; i < (*nFP); i++) {
    totalX+=fP[i].x;
    totalY+=fP[i].y;
  }
  cX = totalX/(*nFP);
  cY = totalY/(*nFP);
  double radius = 0;
  for (i = 0; i < (*nFP); i++) {
    radius += (cX-fP[i].x)*(cX-fP[i].x) + (cY-fP[i].y)*(cY-fP[i].y);
  }
  radius = radius/(*nFP);
  double sum = 0;
  for (i = 0; i < (*nFP); i++) {
    double r = (cX-fP[i].x)*(cX-fP[i].x) + (cY-fP[i].y)*(cY-fP[i].y);
    sum+=(r-radius)*(r-radius);
  }
  double stdDev = sqrt(sum / (*nFP));
  
  for (i = 0; i < (*nFP); i++) {
    double r = (cX-fP[i].x)*(cX-fP[i].x) + (cY-fP[i].y)*(cY-fP[i].y);
    if ((r+2*stdDev < radius) || (r-2*stdDev > radius)) {
      (*nFP)--;
      for (int k = i; k < (*nFP); k++) {
        fP[k] = fP[k+1];
      }
      i--;
    }
  }
}