迭代最小二乘拟合椭圆
//二维点坐标
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;
}