//最小二乘平差椭圆拟合
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;
}
12-20
1287