6参数最小二乘拟合椭圆

//最小二乘平差椭圆拟合
class Ellispefitting_4AC_BB
{
public:
	Ellispefitting_4AC_BB();
	~Ellispefitting_4AC_BB();

	bool fitting(vector<P2d> input, Parameter_ell &Final_ellispe_par);//椭圆拟合总函数，包含椭圆拟合流程
	void Cal_LSadj(vector<P2d> input, Parameter_ell &Final_ellispe_par);//最小二乘法平差
	void SetX(vector<P2d> input);
	void SetY0(vector<P2d> input);
	MatrixXd Dt_Y = MatrixXd::Zero(6, 1);//dltY
private:
	MatrixXd Y0 = MatrixXd::Zero(6, 1);//初始A,B,C,D,E,F
	MatrixXd X = MatrixXd::Zero(pointsize, 6);
	double b20 = 0;
	int pointsize = 0;//点的数量


	//void Cal_Y0(vector<P2d> input);//计算初始A,B,C,D,E,F

	//void Cal_LSadj(vector<P2d> input,Parameter_ell &Final_ellispe_par);//最小二乘法平差
};


bool Ellispefitting_4AC_BB::fitting(vector<P2d> input, Parameter_ell &Final_ellispe_par)
{
	double A, B, C, D, E, F, xc, yc;
	pointsize = input.size();
	//Cal_Y0(input);//计算初始A,B,C,D,E,F
	//SetX(input);
	SetY0(input);
	SetX(input);
	Cal_LSadj(input, Final_ellispe_par);//最小二乘法平差
	vector<double>adf;
	for (int i = 0; i < 6; i++)adf.push_back(Dt_Y(i));
	A = Y0(0);
	B = Y0(1);
	C = Y0(2);
	D = Y0(3);
	E = Y0(4);
	F = Y0(5);


	Parameter_ell tra1;
	
	tra1.xc = (B*E - 2 * C*D) / (4 * A*C - B*B);
	tra1.yc = (B*D - 2 * A*E) / (4 * A*C - B*B);
	xc = tra1.xc;
	yc = tra1.yc;
	tra1.ae = sqrt(2 * (A*xc*xc + C*yc*yc + B*xc*yc - F) / (A + C + sqrt((A - C)*(A - C) + B*B)));
	tra1.be = sqrt(2 * (A*xc*xc + C*yc*yc + B*xc*yc - F) / (A + C - sqrt((A - C)*(A - C) + B*B)));
	tra1.angle = 0.5 * atan(B / (A - C));

	Final_ellispe_par = tra1;
	return true;
}

void Ellispefitting_4AC_BB::Cal_LSadj(vector<P2d> input, Parameter_ell &Final_ellispe_par)
{
	MatrixXd L0 = MatrixXd::Zero(pointsize, 1);//初始L阵

	int kk = 0;
	
	while (true)
	{
		L0 = X*(Y0);
		
		for (int i = 0; i < input.size(); i++)
		{
			L0(i) =  - L0(i);
		}
		Dt_Y = (X.transpose()*X).inverse()*X.transpose()*L0;//平差求解公式
		cout << "Y0:" << endl << Y0 << endl;
		cout << "Dt_Y:" << endl << Dt_Y << endl;
		//cout << "L0:" << endl << L0 << endl;

		Y0 = Y0 + Dt_Y;
		
		double A, B, C, D, E, F, xc, yc;
		A = Y0(0);
		B = Y0(1);
		C = Y0(2);
		D = Y0(3);
		E = Y0(4);
		F = Y0(5);

		Parameter_ell tra1;
		tra1.xc = (B*E - 2 * C*D) / (4 * A*C - B*B);
		tra1.yc = (B*D - 2 * A*E) / (4 * A*C - B*B);
		xc = tra1.xc;
		yc = tra1.yc;
		tra1.ae = 2 * (A*xc*xc + C*yc*yc + B*xc*yc - F) / (A + C + sqrt((A - C)*(A - C) + B*B));
		tra1.be = 2 * (A*xc*xc + C*yc*yc + B*xc*yc - F) / (A + C - sqrt((A - C)*(A - C) + B*B));
		tra1.angle = 0.5 * atan(B / (A-C));

		Final_ellispe_par = tra1;

		vector<double>adc, wds;
		for (int i = 0; i < 6; i++)
		{
			adc.push_back(Y0(i));
			wds.push_back(Dt_Y(i));
		}
		kk++;
		if (abs(Dt_Y(0)) < 1e-6&&abs(Dt_Y(1)) < 1e-6&&abs(Dt_Y(2)) < 1e-6&&abs(Dt_Y(3)) < 1e-6 || kk>100)break;


	}
}
void Ellispefitting_4AC_BB::SetX(vector<P2d> input)
{
	double xi = 0, yi = 0;
	X.resize(pointsize, 6);
	for (int i = 0; i < input.size(); i++)
	{
		xi = input[i].x;
		yi = input[i].y;
		X(i, 0) = xi*xi;
		X(i, 1) = xi*yi;
		X(i, 2) = yi*yi;
		X(i, 3) = xi;
		X(i, 4) = yi;
		X(i, 5) = 1;
	}
}
void Ellispefitting_4AC_BB::SetY0(vector<P2d> input)
{
	double cx = 0, cy = 0, a = 0, b = 0, minx = input[0].x, maxx = input[0].x, miny = input[0].y, maxy = input[0].y;
	for (int i = 0; i < input.size(); i++)
	{
		cx += input[i].x / input.size();
		cy += input[i].y / input.size();
		if (input[i].x < minx)minx = input[i].x;
		if (input[i].x > maxx)maxx = input[i].x;
		if (input[i].y < miny)miny = input[i].y;
		if (input[i].y > maxy)maxy = input[i].y;
	}
	if ((maxx - minx) <= (maxy - miny))
	{
		a = (maxx - minx) / 2;
		b = (maxx - minx) / 2;
	}
	else if ((maxx - minx) > (maxy - miny))
	{
		a = (maxy - miny) / 2;
		b = (maxy - miny) / 2;
	}
	Y0(0) = b*b;
	Y0(1) = 0;
	Y0(2) = a*a;
	Y0(3) = -2 * b*b*cx;
	Y0(4) = -2 * a*a*cy;
	Y0(5) = b*b*cx*cx + a*a*cy*cy - a*a*b*b;
	
}