迭代最小二乘拟合椭圆

迭代最小二乘拟合椭圆

//二维点坐标
struct P2d
{
	double x = 0.0;//横坐标
	double y = 0.0;//纵坐标
	double angle = 0.0;//角度
	int number = 0;//序号
	int pp = 0;//分段
	double di2 = 0;//点到圆心的距离平方
	double Dhxy = 0;//点的共焦双曲线距离
};
//椭圆参数
struct Parameter_ell
{
	double xc = 0.0;
	double yc = 0.0;
	double ae = 0.0;
	double be = 0.0;
	double angle = 0.0;
};

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

	bool fitting(vector<P2d> input, Parameter_ell &Final_ellispe_par);//椭圆拟合总函数，包含椭圆拟合流程
	void Cal_LSadj(vector<P2d> input, Parameter_ell &Final_ellispe_par);//最小二乘法平差
	void Cal_Y0(vector<P2d> input);//计算初始A,B,C,D,E,F

	void SetX(vector<P2d> input);
	void SetY0(vector<P2d> input);
	MatrixXd Dt_Y = MatrixXd::Zero(5, 1);//dltY
private:
	MatrixXd Y0 = MatrixXd::Zero(5, 1);//初始A,B,C,D,E,F
	MatrixXd X = MatrixXd::Zero(pointsize, 5);
	int pointsize = 0;//点的数量
};


//计算初始A,B,C,D,E,F
void Ellispefitting_error_adjustment::Cal_Y0(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) = 0;
	Y0(1) = a*a / (b*b);
	Y0(2) = -2 * cx;
	Y0(3) = (-2 * a*a*cy) / (b*b);
	Y0(4) = cx*cx + (a*a*cy*cy)*(b*b) - a*a;

}
//椭圆拟合总函数，包含椭圆拟合流程
bool Ellispefitting_error_adjustment::fitting(vector<P2d> input, Parameter_ell &Final_ellispe_par)
{
	double A, B, C, D, E, F;
	pointsize = input.size();
	//Cal_Y0(input);//计算初始A,B,C,D,E,F
	SetX(input);
	SetY0(input);
	Cal_LSadj( input, Final_ellispe_par);//最小二乘法平差
	vector<double>adf;
	for (int i = 0; i < 5; i++)adf.push_back(Dt_Y(i));
	A = Y0(0);
	B = Y0(1);
	C = Y0(2);
	D = Y0(3);
	E = Y0(4);
	

	Parameter_ell tra1;
	tra1.xc = (2 * B * C - A * D) / (A * A - 4 * B);
	tra1.yc = (2 * D - A * C) / (A * A - 4 * B);
	double qwd = 2 * (A * C * D - B * C * C - D * D + 4 * B * E - A * A * E);
	double asd = (A * A - 4 * B) * (B + 1 - sqrt(A * A + (1 - B) * (1 - B)));
	double wifb = (A * A - 4 * B) * (B + 1 + sqrt(A * A + (1 - B) * (1 - B)));
	tra1.ae = sqrt(abs(2 * (A * C * D - B * C * C - D * D + 4 * B * E - A * A * E)
		/ ((A * A - 4 * B) * (B + 1 - sqrt(A * A + (1 - B) * (1 - B))))));
	tra1.be = sqrt(abs(2 * (A * C * D - B * C * C - D * D + 4 * B * E - A * A * E)
		/ ((A * A - 4 * B) * (B + 1 + sqrt(A * A + (1 - B) * (1 - B))))));
	tra1.angle = -0.5 * atan(A / (B - 1));

	Final_ellispe_par = tra1;
	return true;
}
//最小二乘法平差，迭代的求解方式
void Ellispefitting_error_adjustment::Cal_LSadj(vector<P2d> input, Parameter_ell &Final_ellispe_par)
{
	
	MatrixXd L0 = MatrixXd::Zero(pointsize, 1);//初始L阵
	
	int kk = 0;
	//for (int i = 0; i < 6; i++)Y0(i) = 0;
	while (true)
	{	//MatrixXd L = MatrixXd::Zero(pointsize, 1);//L阵
		L0 = X*(Y0);
		for (int i = 0; i < input.size(); i++)
		{
			L0(i) = -input[i].x*input[i].x - L0(i);
		}
		Dt_Y = (X.transpose()*X).inverse()*X.transpose()*L0;//平差求解公式
		Y0 = Y0+Dt_Y;

		MatrixXd df = X*Y0;
		vector<double> vd;
		for (int i = 0; i < input.size(); i++)
		{
			vd.push_back(input[i].x*input[i].x + df(i));
		}

		double A, B, C, D, E;
		A = Y0(0);
		B = Y0(1);
		C = Y0(2);
		D = Y0(3);
		E = Y0(4);

		vector<double>adc, wds;
		for (int i = 0; i < 5; 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_error_adjustment::SetX(vector<P2d> input)
{
	double xi = 0, yi = 0;
	X.resize(pointsize,5);
	for (int i = 0; i < input.size(); i++)
	{
		xi = input[i].x;
		yi = input[i].y;
		X(i, 0) = xi*yi;
		X(i, 1) = yi*yi;
		X(i, 2) = xi;
		X(i, 3) = yi;
		X(i, 4) = 1;
	}
}

void Ellispefitting_error_adjustment::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) = 0;
	Y0(1) = a*a / (b*b);
	Y0(2) = -2 * cx;
	Y0(3) = (-2 * a*a*cy) / (b*b);
	Y0(4) = cx*cx + (a*a*cy*cy)*(b*b) - a*a;

}