# 最小二乘法拟合椭圆——MATLAB和Qt-C++实现

## 2.MATLAB实现

function [ p ] = Ellipse_FittingLS( XY )
N = size(XY,1);
x = XY(:,1);
y = XY(:,2);

sum_X2Y2=0;
sum_X1Y3=0;
sum_X2Y1=0;
sum_X1Y2=0;
sum_X1Y1=0;
sum_Y4=0;
sum_Y3=0;
sum_Y2=0;
sum_X2=0;
sum_X1=0;
sum_Y1=0;
sum_X3Y1=0;
sum_X3=0;

for i = 1:N
sum_X2Y2 = sum_X2Y2+x(i)*x(i)*y(i)*y(i);
sum_X1Y3 = sum_X1Y3+x(i)*y(i)*y(i)*y(i);
sum_X2Y1 = sum_X2Y1+x(i)*x(i)*y(i);
sum_X1Y2 = sum_X1Y2+x(i)*y(i)*y(i);
sum_X1Y1 = sum_X1Y1+x(i)*y(i);
sum_Y4 = sum_Y4+y(i)*y(i)*y(i)*y(i);
sum_Y3 = sum_Y3+y(i)*y(i)*y(i);
sum_Y2 = sum_Y2+y(i)*y(i);
sum_X2 = sum_X2+x(i)*x(i);
sum_X1 = sum_X1+x(i);
sum_Y1 = sum_Y1+y(i);
sum_X3Y1 = sum_X3Y1+x(i)*x(i)*x(i)*y(i);
sum_X3 = sum_X3+x(i)*x(i)*x(i);
end

M1 = [sum_X2Y2,sum_X1Y3,sum_X2Y1,sum_X1Y2,sum_X1Y1;
sum_X1Y3,sum_Y4,sum_X1Y2,sum_Y3,sum_Y2;
sum_X2Y1,sum_X1Y2,sum_X2,sum_X1Y1,sum_X1;
sum_X1Y2,sum_Y3,sum_X1Y1,sum_Y2,sum_Y1;
sum_X1Y1,sum_Y2,sum_X1,sum_Y1,N
];
M2 = [sum_X3Y1;sum_X2Y2;sum_X3;sum_X2Y1;sum_X2];
M3 = inv(M1);
M4 = M3*M2;
A = M4(1);
B = M4(2);
C = M4(3);
D = M4(4);
E = M4(5);

Xc = (2*B*C-A*D)/(A*A-4*B);
Yc = (2*D-A*D)/(A*A-4*B);
a = sqrt(abs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B-sqrt(A*A+(1-B)*(1-B))+1))));
b = sqrt(abs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B+sqrt(A*A+(1-B)*(1-B))+1))));
theta = atan(sqrt(abs((a*a-b*b*B)/(a*a*B-b*b))));
p = [Xc,Yc,a,b,theta];
end



## 3.C++和Qt实现

Jungle将拟合椭圆的方法及椭圆相关参数封装到一个类EllipseFitting中。类中用到了Qt自带的数据结构QList，也可以用数组或者STL中的数据结构，如vetor等。

#ifndef ELLIPSEFITTING_H
#define ELLIPSEFITTING_H

#include <QList>
#include <iostream>
#include <QDebug>
#include "math.h"

class EllipseFitting
{
public:
EllipseFitting(QList<Data*>);
~EllipseFitting();

///拟合椭圆
void fittingData();
///返回拟合结果
QList<double> getResultList();

private:

///原始数据集合
QList<Data *> PrimiDataSet;
///拟合结果列表
QList<double>resultList;
///拟合椭圆的重要参数：椭圆中心坐标（Xc,Yc),长半轴a，短半轴b,偏移角theta
double Xc;
double Yc;
double a;
double b;
double theta;
};

#endif // ELLIPSEFITTING_H

#include "ellipsefitting.h"
#define n 5

EllipseFitting::EllipseFitting(QList<Data*> idataset)
{
this->Xc = 0.00;
this->Yc = 0.00;
this->a = 0.00;
this->b = 0.00;
this->theta = 0.00;
this->resultList.clear();
for(int i=0;i<idataset.size();i++)
this->PrimiDataSet<<idataset[i];
this->fittingData();
}

EllipseFitting::~EllipseFitting()
{

}

QList<double> EllipseFitting::getResultList()
{
return this->resultList;
}

void EllipseFitting::fittingData()
{
long double A = 0.00,B = 0.00,C = 0.00,D = 0.00,E = 0.00;
long double x2y2=0.0,x1y3=0.0,x2y1=0.0,x1y2=0.0,x1y1=0.0,yyy4=0.0,yyy3=0.0,yyy2=0.0,xxx2=0.0,xxx1=0.0,yyy1=0.0,x3y1=0.0,xxx3=0.0;
int N = PrimiDataSet.size();
for(int i = 0; i < PrimiDataSet.size(); i++)
{
long double xi = PrimiDataSet[i]->x, yi = PrimiDataSet[i]->y;

x2y2 += xi*xi*yi*yi;
x1y3 += xi*yi*yi*yi;
x2y1 += xi*xi*yi;
x1y2 += xi*yi*yi;
x1y1 += xi*yi;
yyy4 += yi*yi*yi*yi;
yyy3 += yi*yi*yi;
yyy2 += yi*yi;
xxx2 += xi*xi;
xxx1 += xi;
yyy1 += yi;
x3y1 += xi*xi*xi*yi;
xxx3 += xi*xi*xi;
}
long double matrix[5][5]={{x2y2,x1y3,x2y1,x1y2,x1y1},
{x1y3,yyy4,x1y2,yyy3,yyy2},
{x2y1,x1y2,xxx2,x1y1,xxx1},
{x1y2,yyy3,x1y1,yyy2,yyy1},
{x1y1,yyy2,xxx1,yyy1,N}
};

long double matrix2[5][1]={x3y1,x2y2,xxx3,x2y1,xxx2};
long double matrix3[5][1] = {A,B,C,D,E};

////求矩阵matrix的逆，结果为InverseMatrix
///单位矩阵
long double E_Matrix[n][n];
long double mik;
long double m = 2*n;
for(int i = 0; i < n; i++)
{
for(int j = 0; j < n; j++)
{
if(i == j)
E_Matrix[i][j] = 1.00;
else
E_Matrix[i][j] = 0.00;
}
}
long double CalcuMatrix[n][2*n];
for(int i = 0; i < n; i++)
{
for(int j = 0; j < n; j++)
{
CalcuMatrix[i][j] = matrix[i][j];
}
for(int k = n; k < m; k++)
{
CalcuMatrix[i][k] = E_Matrix[i][k-n];
}
}

for(int i = 1; i <= n-1; i++)
{
for(int j = i+1; j <= n; j++)
{
mik = CalcuMatrix[j-1][i-1]/CalcuMatrix[i-1][i-1];
for(int k = i+1;k <= m; k++)
{
CalcuMatrix[j-1][k-1] -= mik*CalcuMatrix[i-1][k-1];
}
}
}
for(int i=1;i<=n;i++)
{
long double temp = CalcuMatrix[i-1][i-1];
for(int j=1;j<=m;j++)
{
CalcuMatrix[i-1][j-1] = CalcuMatrix[i-1][j-1]/temp;
}
}
for(int k=n-1;k>=1;k--)
{
for(int i=k;i>=1;i--)
{
mik = CalcuMatrix[i-1][k];
for(int j=k+1;j<=m;j++)
{
CalcuMatrix[i-1][j-1] -= mik*CalcuMatrix[k][j-1];
}
}
}
long double InverseMatrix[n][n];
for(int i=0;i<n;i++)
{
for(int j=0;j<n;j++)
{
InverseMatrix[i][j] = CalcuMatrix[i][j+n];
}
}
for(int i=0;i<5;i++)
{
for(int j=0;j<5;j++)
{
if(fabs(InverseMatrix[i][j]) < 0.0000001)
InverseMatrix[i][j] = 0.00;
}
}
///求参数A,B,C,D,E
for(int i=0;i<5;i++)
{
for(int j=0;j<5;j++)
{
matrix3[i][0] += InverseMatrix[i][j]*(-matrix2[j][0]);
}
}
A = matrix3[0][0];
B = matrix3[1][0];
C = matrix3[2][0];
D = matrix3[3][0];
E = matrix3[4][0];

///求拟合结果重要参数
Xc = (2*B*C-A*D)/(A*A-4*B);
Yc = (2*D-A*D)/(A*A-4*B);
a = sqrt(fabs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B-sqrt(A*A+(1-B)*(1-B))+1))));
b = sqrt(fabs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B+sqrt(A*A+(1-B)*(1-B))+1))));
theta = atan2(a*a-b*b*B,a*a*B-b*b);
qDebug()<<"("<<Xc<<","<<Yc<<")"<<"\na="<<a<<"  b="<<b<<"  1/2(a+b)="<<0.5*(a+b);
qDebug()<<"theta="<<theta<<"  Arccos(b/a)="<<((acos(b/a)*180)/3.1415926);
this->resultList<<Xc<<Yc<<a;
//this->resultList<<Xc<<Yc<<a<<b<<theta;
}