ellipsefitting.h

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

// File: EllipseFitting.h
// Date: 30/03/03

#ifndef _EllipseFitting_h_DEFINED
#define _EllipseFitting_h_DEFINED

#include "../Common/Common.h"
#include "VisionData.h"

#define MAX_ITERATIONS 5
#define MAX_PARLIMIT 208
#define FACTOR_UP 10
#define FACTOR_DOWN 0.04
#define EPSILON 0.5 
#define LMAEPSILON 0.00000005
#define MIN_POINT_DIST_FROM_EDGE 8
#define LAMBDA_MIN 0.0000000000000000000000000000000000000000000000000001
#define LAMBDA_MAX 1000.0

//0.000001

class EllipseFitting {

  public:
    EllipseFitting();
    ~EllipseFitting();
    void SetVisionData(VisionData* vd) {visionData = vd;}
    Circle FitEllipseLMA(VisionData* vd, Blob* ballBlob, int direction);
    Circle FitEllipseLMF(VisionData* vd, Blob* ballBlob, int direction);
    Circle ThreePointFit(VisionData*,point,point,point);
    

    void EnlargeBlobRadial(Blob* blob, Colour c, int numRays);

    int numFittedPoints; //  Stores the number of points found so far
    point* fittedPoints; // Stores the points which have been found so far
    int meanX, meanY; 
  private:
    VisionData* visionData;
    void FindPointsRadial(int* nFP, point* fP, Blob* currentBlob, Colour blobColour, int numRays, bool includeImageEdge);
    void FindPoints(Blob* currentBlob, Colour blobColour, int direction);
    Circle AlgebraicCircleFit();
    Circle GeometricCircleFitLMA(Circle initialCircle);
    Circle GeometricCircleFitLMF(Circle initialCircle);
    Circle InvalidCircle();
    double Sigma (Circle circle);
    double Distance (Circle One, Circle Two);
    double sumGSquared;
    double denominator;

    // Private data from Geometric Circle Fit
    double MaximumDeviation();
    bool Initalise(Circle);
    bool CholeskyDecomposition();
    bool ComputeMoments();
    bool UpdateCircleParameters();
    void GenerateCircle();


    double UpdateOldParameters();

    double lambda,oldA,oldF,oldT,newA,newF,newT,H,oldStandardDeviation,newStandardDeviation,centreSquared;
    double Xi,Yi,Zi,Ui,Vi,Gi,ADF,FACT,DGDAi,DGDFi,DGDTi;
    double H11,H12,H13,H22,H23,H33,F1,F2,F3,dA,dF,dT;
    double G11,G22,G33,G12,G13,G23,D1,D2,D3;
    double Xshift,Yshift,dX,dY,dMax;
    Circle finalCircle,oldCircle,newCircle;


    // Three point fit 
    void Swap(point p1, point p2);
    bool GetCenter(point p1, point p2, point p3, point* center);
    double GetRadius(point p1, point p2, point p3);
};

#endif