ellipsefitting.cc

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

      lambda *= FACTOR_UP;
      if ((iteration > MAX_ITERATIONS) ||(lambda > LAMBDA_MAX)) break;
      else goto try_again;
    }
  } 
  GenerateCircle();
  return finalCircle;
}

///////////////////////////////////////////////////////////////////////////////////////

// Calculates the maximum standard deviation for the set of points
double EllipseFitting::MaximumDeviation() {
  int imax,jmax;
  double Xi,Yi,dMax=0.0;
  for (int i=1; i<numFittedPoints; i++) {
    for (int j=0; j<i; j++) {
      Xi = fittedPoints[i].x - fittedPoints[j].x;
      Yi = fittedPoints[i].y - fittedPoints[j].y;
      if (dMax < (Xi*Xi+Yi*Yi)) {
        dMax=Xi*Xi+Yi*Yi;
        imax = i;
        jmax = j;
      }
    }
  }
  dMax = sqrt(dMax);
  return dMax;
}

// Need to determine reasonable values for the radius and the centre at various distances and cap the inputs to these values
// As the algorithm runs generally for all values it must be a bad prediction from the algebraic approximation that is causing the issues.

bool EllipseFitting::Initalise(Circle initialCircle) {
  // Starting with the initial guess provided by the algebraic technique
  if ((initialCircle.radius + initialCircle.radius) <= 0) 
    return false;
  
  newA = 1.0/(initialCircle.radius + initialCircle.radius);
  centreSquared = initialCircle.centreX*initialCircle.centreX + initialCircle.centreY*initialCircle.centreY;
  newF = (centreSquared - initialCircle.radius*initialCircle.radius)*newA;
  
  // Check that the value that we're using is valid
  double temp = -initialCircle.centreX/sqrt(centreSquared); 
  if ((temp < DBL_MIN) || (temp > DBL_MAX))
    return false;

  // Check that we are not generating a divide by zero error
  if (sqrt(centreSquared) <= 0) 
    return false;
 
  newT = acos(-initialCircle.centreX/sqrt(centreSquared));
  if (initialCircle.centreY > 0) newT = 2.0*PI - newT;
  newStandardDeviation = initialCircle.sd;
  newCircle = initialCircle;

  for (int k=0; k<1000; k++) { // Perhaps this 1000 could be changed to numFittedPoints i.e. 1000 was used because there was a 1000 samples in the original experiment
    if ((1.0+4.0*newA*newF) >= LMAEPSILON) {
      dX=(rand()-0.5)*dMax;
      dY=(rand()-0.5)*dMax;
     
      Xshift += dX;
      Yshift += dY;

      newCircle.centreX += dX;
      newCircle.centreY += dY;
      centreSquared = newCircle.centreX*newCircle.centreX + newCircle.centreY*newCircle.centreY;
      newF = (centreSquared - newCircle.radius*newCircle.radius)*newA;
      
      // Check that we are not generating a divide by zero error
      if (sqrt(centreSquared) <= 0) 
        return false;
        
      newT = acos(((double)-newCircle.centreX)/sqrt(centreSquared));
      if (newCircle.centreY > 0) newT = 2.*PI - newT;    
    } else {
      break;
    }
  }
  return true; // True indicates that the initalisation was successful
}

bool EllipseFitting::ComputeMoments() {
  // computing moments

  H11=H12=H13=H22=H23=H33=F1=F2=F3=0.0;
 
  for (int l=0; l<numFittedPoints; l++) {
    Xi = fittedPoints[l].x + Xshift;
    Yi = fittedPoints[l].y + Yshift;
    Zi = Xi*Xi + Yi*Yi;
    Ui = Xi*cos(oldT) + Yi*sin(oldT);
    Vi =-Xi*sin(oldT) + Yi*cos(oldT);
     
    // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
    if ((1.0 + 4.0*oldA*oldF) <= 0) 
      return false; 

    H = sqrt(1.0 + 4.0*oldA*oldF);
    ADF = oldA*Zi + H*Ui + oldF;

    // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
    if ((1.0 + 4.0*oldA*ADF) <= 0) 
      return false; 
    
    Gi = 2.0*ADF/(sqrt(1.0 + 4.0*oldA*ADF) + 1.0);
    FACT = 2.0/(sqrt(1.0 + 4.0*oldA*ADF) + 1.0)*(1.0 - oldA*Gi/sqrt(1.0 + 4.0*oldA*ADF));
    DGDAi = FACT*(Zi + 2.0*oldF*Ui/H) - Gi*Gi/sqrt(1.0 + 4.0*oldA*ADF);
    DGDFi = FACT*(2.0*oldA*Ui/H + 1.0);
    DGDTi = FACT*H*Vi; 
     
    H11 += DGDAi*DGDAi;
    H12 += DGDAi*DGDFi;
    H13 += DGDAi*DGDTi;
    H22 += DGDFi*DGDFi;
    H23 += DGDFi*DGDTi;
    H33 += DGDTi*DGDTi;
     
    F1 += Gi*DGDAi;
    F2 += Gi*DGDFi;
    F3 += Gi*DGDTi;
  }
  return true;
}

bool EllipseFitting::CholeskyDecomposition() {
    // Cholesky decomposition

    // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
    if ((H11 + lambda) <= 0) 
      return false; 

    G11 = sqrt(H11 + lambda);
    G12 = H12/G11;
    G13 = H13/G11;

    // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
    if ((H22 + lambda - G12*G12) <= 0) 
      return false; 

    G22 = sqrt(H22 + lambda - G12*G12);
    G23 = (H23 - G12*G13)/G22;

    // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
    if ((H33 + lambda - G13*G13 - G23*G23) <= 0) 
      return false; 

    G33 = sqrt(H33 + lambda - G13*G13 - G23*G23);

    D1 = F1/G11;
    D2 = (F2 - G12*D1)/G22;
    D3 = (F3 - G13*D1 - G23*D2)/G33;
    
    // Calculates the change in the three parameters
    dT = D3/G33;
    dF = (D2 - G23*dT)/G22;
    dA = (D1 - G12*dF - G13*dT)/G11;
      
    // Updating the parameters by subtracting the difference from the old parameter
    newA = oldA - dA;
    newF = oldF - dF;
    newT = oldT - dT;
    return true;
}


bool EllipseFitting::UpdateCircleParameters() {
  sumGSquared = 0.0;
   
  for (int i=0; i < numFittedPoints; i++) {
      Xi = fittedPoints[i].x + Xshift;
      Yi = fittedPoints[i].y + Yshift;
      Zi = Xi*Xi + Yi*Yi;
      Ui = Xi*cos(newT) + Yi*sin(newT);
  
      // Check that we are not generating a negative square root error - note the equal to check is for the divide which is down further 
      if ((1.0 + 4.0*newA*newF) <= 0) 
        return false; 

      ADF = newA*Zi + (sqrt(1.0 + 4.0*newA*newF))*Ui + newF;
      denominator = sqrt(4.0*newA*ADF + 1.0) + 1.0;
      
      if (denominator <= 0)
        return false;
      
      Gi = 2.0*ADF/(denominator);

      sumGSquared = sumGSquared+Gi*Gi;
  }

  newStandardDeviation = sqrt(sumGSquared/numFittedPoints);

  H = sqrt(1.0+4.0*newA*newF);

  double denominator = newA + newA;
  newCircle.centreX = -H*cos(newT)/(denominator) - Xshift;
  newCircle.centreY = -H*sin(newT)/(denominator) - Yshift;
  newCircle.radius = 1.0/fabs(denominator);
  newCircle.sd = newStandardDeviation;
  return true;
}

void EllipseFitting::GenerateCircle() {
  H = sqrt(1.0 + 4.0*oldA*oldF);
  finalCircle.centreX = -H*cos(oldT)/(oldA+oldA) - Xshift;
  finalCircle.centreY = -H*sin(oldT)/(oldA+oldA) - Yshift;
  finalCircle.radius = 1.0/fabs(oldA+oldA);
  finalCircle.sd = oldStandardDeviation;
  return;
}

double EllipseFitting::UpdateOldParameters() {
      dX=(rand()-0.5)*dMax;
      dY=(rand()-0.5)*dMax;
     
      Xshift += dX;
      Yshift += dY;
     
      H = sqrt(1.+4.*oldA*oldF);
      double aTemp = -H*cos(oldT)/(oldA+oldA) + dX;
      double bTemp = -H*sin(oldT)/(oldA+oldA) + dY;
      double rTemp = 1.0/fabs(oldA+oldA);
      oldA = 1.0/(rTemp + rTemp);
      centreSquared = aTemp*aTemp + bTemp*bTemp;
      oldF = (centreSquared - rTemp*rTemp)*oldA;
      oldT = acos(-aTemp/sqrt(centreSquared));
      return bTemp;
}

 
// The following method performs a geometric circle fit using the Levenberg-Marquard (LMF) algorithm and the points
// provided in the fitted points array
Circle EllipseFitting::GeometricCircleFitLMF(Circle initialCircle) {
  double dx,dy,radius,u,v;
	double UUl,VVl,Nl,F1,F2,F3,dX,dY,dR;
	double G11,G22,G33,G12,G13,G23,D1,D2,D3;
  double meanU, meanV, meanUSquared, meanVSquared, meanUV, meanR;
  double sumU, sumV, sumUSquared, sumVSquared,sumUV, sumR;
	Circle oldCircle,newCircle;

  // starting with the given initial guess as determined by the algebraic approximation
	newCircle = initialCircle;

  // initializing the lambda and iteration counters
	double lambda = 1.0;
	int iteration = 0;

  while(true) {
    oldCircle = newCircle; // Updates the pointer to point to the newest circle which has been determined
    if (iteration > MAX_ITERATIONS) {
      oldCircle.centreX = oldCircle.centreX + meanX;
      oldCircle.centreY = oldCircle.centreY + meanY;
      oldCircle.isDefined = true;
      return oldCircle;
    }
    // computing moments  
    sumU=sumV=sumUSquared=sumVSquared=sumUV=sumR=0;

    // Iterate over the array, summing the difference between the original points and the lastest approximation 
    for (int i=0; i < numFittedPoints; i++) {
      dx = fittedPoints[i].x - oldCircle.centreX;
      dy = fittedPoints[i].y - oldCircle.centreY;
      radius = sqrt(dx*dx + dy*dy);
      
      // Check that the radius is defined
      if (radius <= 0) 
        return InvalidCircle();  

      u = dx/radius;
      v = dy/radius;  
      sumU = sumU + u;
      sumV = sumV + v;
      sumUSquared = sumUSquared + SQUARE(u);
      sumVSquared = sumVSquared + SQUARE(v);
      sumUV = sumUV + u*v;
      sumR = sumR + radius;
    }
    meanU = sumU / numFittedPoints;
    meanV = sumV / numFittedPoints;
    meanUSquared = sumUSquared / numFittedPoints;
    meanVSquared = sumVSquared  / numFittedPoints;
    meanUV =  sumUV / numFittedPoints;
    meanR = sumR  / numFittedPoints;

    // computing matrices
    F1 = oldCircle.centreX + oldCircle.radius*meanU;
    F2 = oldCircle.centreY + oldCircle.radius*meanV;
    F3 = oldCircle.radius - meanR;

    while (true) {

      UUl = meanUSquared + lambda;
      VVl = meanVSquared + lambda;
      Nl = 1.0 + lambda;

      //	Cholesly decomposition
      G11 = sqrt(UUl);
      
      // Check that G11 is defined
      if (G11 <= 0)
        return InvalidCircle();  
      
      G12 = meanUV/G11;
      G13 = meanU/G11;

      if ((VVl - G12*G12) < 0) 
        return InvalidCircle();  

      G22 = sqrt(VVl - G12*G12);
      G23 = (meanV - G12*G13)/G22;

      if ((Nl - G13*G13 - G23*G23) < 0) 
        return InvalidCircle();  

      G33 = sqrt(Nl - G13*G13 - G23*G23);

      D1 = F1/G11;
      D2 = (F2 - G12*D1)/G22;
      D3 = (F3 - G13*D1 - G23*D2)/G33;

      dR = D3/G33;
      dY = (D2 - G23*dR)/G22;
      dX = (D1 - G12*dY - G13*dR)/G11;

      // updating the parameters