结合师兄写的程序加上自己的修改,为在工程上使用,后又写出C++版,同是将输出一些参数作为结构体输出
椭圆拟合
同时可参考http://mathworld.wolfram.com/Ellipse.html
function [Re, center, vertex]= MyEllipseDirectFit(XY)
% Direct ellipse fit, proposed in article
% A. W. Fitzgibbon, M. Pilu, R. B. Fisher
% "Direct Least Squares Fitting of Ellipses"
% IEEE Trans. PAMI, Vol. 21, pages 476-480 (1999)
%
% Our code is based on a numerically stable version
% of this fit published by R. Halir and J. Flusser
%
% Input: XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2)
%
% Output: A = [a b c d e f]' is the vector of algebraic
% parameters of the fitting ellipse:
% ax^2 + bxy + cy^2 +dx + ey + f = 0
% the vector A is normed, so that ||A||=1
%
% This is a fast non-iterative ellipse fit.
%
% It returns ellipses only, even if points are
% better approximated by a hyperbola.
% It is somewhat biased toward smaller ellipses.
%
centroid = mean(XY); % the centroid of the data set
warning off;
D1 = [(XY(:,1)-centroid(1)).^2, (XY(:,1)-centroid(1)).*(XY(:,2)-centroid(2)),...
(XY(:,2)-centroid(2)).^2];
D2 = [XY(:,1)-centroid(1), XY(:,2)-centroid(2), ones(size(XY,1),1)];
S1 = D1'*D1;
S2 = D1'*D2;
S3 = D2'*D2;
T = -inv(S3)*S2';
M = S1 + S2*T;
M = [M(3,:)./2; -M(2,:); M(1,:)./2];
[evec,eval] = eig(M);
cond = 4*evec(1,:).*evec(3,:)-evec(2,:).^2;
A1 = evec(:,find(cond>0));
A = [A1; T*A1];
A4 = A(4)-2*A(1)*centroid(1)-A(2)*centroid(2);
A5 = A(5)-2*A(3)*centroid(2)-A(2)*centroid(1);
A6 = A(6)+A(1)*centroid(1)^2+A(3)*centroid(2)^2+...
A(2)*centroid(1)*centroid(2)-A(4)*centroid(1)-A(5)*centroid(2);
A(4) = A4; A(5) = A5; A(6) = A6;
% if A(1)<0
% A=-A;
% end
Re=A;
%长轴倾角
%ellipse equation: a*x^2 + b*x*y + c*y^2 +d*x + e*y + 1 = 0;
% Refer to:http://blog.csdn.net/ningyaliuhebei/article/details/46327681
B=Re(2)/Re(6); A=Re(1)/Re(6);C=Re(3)/Re(6); D=Re(4)/Re(6);E=Re(5)/Re(6);
if B==0
if A<C/2
theta1=0;
else
theta1= pi/2;
end
else
if A<C/2
theta1=0.5*atan(B/(A-C));
else
theta1= pi/2 + 0.5*atan(B/(A-C));
end
end
K1= tan(theta1); %长轴斜率
if (abs(K1)<1e-8)
K1=0;
end
%椭圆的中心点
xc=(B*E-2*C*D)/(4*A*C-B^2);
yc=(B*D-2*A*E)/(4*A*C-B^2);
%椭圆的长、短半轴长度
la=sqrt(2*(A*xc^2+C*yc^2+B*xc*yc-1)/(A+C-sqrt((A-C)^2+B^2)));
lb=sqrt(2*(A*xc^2+C*yc^2+B*xc*yc-1)/(A+C+sqrt((A-C)^2+B^2)));
%长、短轴顶点坐标
n1=[1,K1]; n1=n1/norm(n1);
center = [xc, yc];
va1=center+la*n1; %长轴顶点1, 坐标轴正向
va2=center-la*n1; %长轴顶点2,坐标轴负向
if(K1 == 0)
n2=[0,1];
else
n2=[1,-1/K1];
end
n2=n2/norm(n2);
if n2(2) >= 0
vb1=center+lb*n2; %短轴顶点1,坐标轴正向
vb2=center-lb*n2; %短轴顶点2,坐标轴负向
else
vb1=center-lb*n2; %短轴顶点1,坐标轴正向
vb2=center+lb*n2; %短轴顶点2,坐标轴负向
end
center = [xc, yc];
vertex = [va1;va2;vb1;vb2];
end % EllipseDirectFit
长短轴计算
结合上文函数
function [aa,bb]=getab(re)
a=re(1);
b=re(2)/2;
c=re(3);
d=re(4)/2;
f=re(5)/2;
g=re(6);
aa=sqrt((2*(a*f^2+c*d^2+g*b^2-2*b*d*f-a*c*g))/((b^2-a*c)*(sqrt((a-c)^2+4*b^2)-(a+c))));
bb=sqrt((2*(a*f^2+c*d^2+g*b^2-2*b*d*f-a*c*g))/((b^2-a*c)*(-sqrt((a-c)^2+4*b^2)-(a+c))));
if abs(aa)>abs(bb)
temp=aa;
aa=bb;
bb=temp;
end
end
C++版
C++程序基于Opencv库
typedef struct{
double Re[6], center[2], vertex[4][2], abc[3], longShortAxis[2];
//Re椭圆一般式方程参数ax^2+bxy+cy^2+dx+ey+f=0;
//center,椭圆中心坐标
//vertex,椭圆长轴点短轴点坐标
//abc,椭圆方程参数,a(x-u)^2+b(y-v)^2+2c(x-u)(y-v)=1
//longShortAxis,椭圆长轴短轴长度
}structEllipse;
structEllipse MyEllipseDirectFit(std::vector<std::vector<double>> XY, int rows)
{
structEllipse result;
//double eps = 1e-12;
//std::vector<double >centroidx, centroidy;
//for (int i = 0; i < rows; i++){
// centroidx.push_back(XY[i][0]);
// centroidy.push_back(XY[i][1]);
//}
std::cout << *max_element(centroidx.begin(), centroidx.end());
//double centroid[2] = { arrCal.mean(centroidx), arrCal.mean(centroidy) };
//std::vector<std::vector<double>>D1(rows), D2(rows);
cv::Mat M;
M.create(rows, 6, CV_64FC1);
for (int i = 0; i < rows; i++){
M.at<double>(i, 0) = std::pow(XY[i][0], 2);
M.at<double>(i, 1) = XY[i][0] * XY[i][1];
M.at<double>(i, 2) = std::pow(XY[i][1], 2);
M.at<double>(i, 3) = XY[i][0];
M.at<double>(i, 4) = XY[i][1];
M.at<double>(i, 5) = 1;
}
//cv::Mat T = (-(D2.t()*D2).inv()*(D1.t()*D2).t());
//cv::Mat M0 = D1.t()*D1 + (D1.t()*D2)*(-(D2.t()*D2).inv()*(D1.t()*D2).t());
//cv::Mat M ;
//cv::vconcat(M0(cv::Rect(0, 2, 3, 1)) / 2, -M0(cv::Rect(0, 1, 3, 1)), M);//vconcat:矩阵拼接,Rect矩阵裁剪
//cv::vconcat(M, M0(cv::Rect(0, 0, 3, 1)) / 2, M);
cv::SVD svd;
cv::Mat U;
cv::Mat DD;
cv::Mat VT;
svd.compute(M, DD, U, VT);
//cv::Mat Re = VT(cv::Rect(0, 5, 6, 1));
for (int i = 0; i < VT.cols; i++)
result.Re[i] = *VT.ptr<double>(5, i);
double parameter[5];
parameter[0] = result.Re[0] / result.Re[5];
parameter[1] = result.Re[1] / result.Re[5];
parameter[2] = result.Re[2] / result.Re[5];
parameter[3] = result.Re[3] / result.Re[5];
parameter[4] = result.Re[4] / result.Re[5];
double K1 = tan(0.5*atan(parameter[1] / (parameter[0] - parameter[2])));
if (fabs(K1) < 1e-8)
K1 = 0;
//std::cout << std::setprecision(20) <<K1 << std::endl;
result.center[0] = (parameter[1] * parameter[4] - 2 * parameter[2] * parameter[3]) / (4.0 * parameter[0] * parameter[2] - pow(parameter[1], 2));
result.center[1] = (parameter[1] * parameter[3] - 2 * parameter[0] * parameter[4]) / (4.0 * parameter[0] * parameter[2] - pow(parameter[1], 2));
//longShortAxis椭圆长短轴
result.longShortAxis[0] = sqrt(2 * (parameter[0] * pow(result.center[0], 2) + parameter[2] * pow(result.center[1], 2) + parameter[1] * result.center[0] * result.center[1] - 1) / (parameter[0] + parameter[2] - sqrt(pow((parameter[0] - parameter[2]), 2) + pow(parameter[1], 2))));
result.longShortAxis[1] = sqrt(2 * (parameter[0] * pow(result.center[0], 2) + parameter[2] * pow(result.center[1], 2) + parameter[1] * result.center[0] * result.center[1] - 1) / (parameter[0] + parameter[2] + sqrt(pow((parameter[0] - parameter[2]), 2) + pow(parameter[1], 2))));
//std::cout << std::setprecision(20) << la << std::endl << lb << std::endl;
double n1[2];
n1[0] = (1.0 / sqrt(1 + pow(K1, 2)));
n1[1] = (K1 / sqrt(1 + pow(K1, 2)));
result.vertex[0][0] = result.center[0] + result.longShortAxis[0] *n1[0] ;
result.vertex[0][1] = result.center[1] + result.longShortAxis[0] *n1[1] ;
result.vertex[1][0] = result.center[0] - result.longShortAxis[0] * n1[0];
result.vertex[1][1] = result.center[1] - result.longShortAxis[0] * n1[1];
double n2[2];
if (K1 == 0){
n2[0] = 0;
n2[1]= 1.0;
}
else{
n2[0] = (1.0 / sqrt(1 + pow(-1.0 / K1, 2)));
n2[1] = ((-1.0 / K1) / sqrt(1 + pow(-1.0 / K1, 2)));
}
if (n2 >= 0){
result.vertex[2][0] = result.center[0] + result.longShortAxis[1] * n2[0];
result.vertex[2][1] = result.center[1] + result.longShortAxis[1] * n2[1];
result.vertex[3][0] = result.center[0] - result.longShortAxis[1] * n2[0];
result.vertex[3][1] = result.center[1] - result.longShortAxis[1] * n2[1];
}
else{
result.vertex[2][0] = result.center[0] - result.longShortAxis[1] * n2[0];
result.vertex[2][1] = result.center[1] - result.longShortAxis[1] * n2[1];
result.vertex[3][0] = result.center[0] + result.longShortAxis[1] * n2[0];
result.vertex[3][1] = result.center[1] + result.longShortAxis[1] * n2[1];
}
//std::cout << std::setprecision(20) << << std::endl;
//result.Re[1]= *Re.ptr<double>;
//cond=4 * eVectorsMat(cv::Rect(0, 0, 3, 1)).mul(eVectorsMat(cv::Rect(0, 2, 3, 1))) - (eVectorsMat(cv::Rect(0, 1, 3, 1))).mul(eVectorsMat(cv::Rect(0, 1, 3, 1)));
//int j=0;
//for (j = 0; j < cond.cols; j++){
// if (*cond.ptr<double>(0, j)>0)
// break;
//}
//cv::Mat A1 = eVectorsMat(cv::Rect(j, 0, 1, 3));
//cv::Mat A;
//cv::vconcat(A1,T*A1, A);
//std::cout << A1 << std::endl;
//std::cout << cond << std::endl;
//std::cout << eVectorsMat(cv::Rect(0, 0, 3, 1)).mul(eVectorsMat(cv::Rect(0, 2, 3, 1)));
//cv::Mat point;
//point.create(rows, 2, CV_64F);
//for (int i = 0; i < rows; i++)
//for (int j = 0; j < 2; j++)
// point.at<double>(i, j) = XY[i][j];
cv::meanStdDev(point, mean_, stddev2_);
//cv::Mat centroid0 = (cv::Mat_<double>(1,2)<<mean(point(cv::Rect(0, 0, 1, rows)))[0],mean(point(cv::Rect(1, 0, 1, rows)))[0]);
//cv::Mat centroid;
//for (int i = 0; i < rows ; i++){
// centroid.push_back(centroid0);
//}
//std::cout << point << std::endl << centroid << std::endl ;//<< point.rowRange;
//另一种拟合形式中的abc三个参数计算
double re1[6];
re1[0] = (result.Re[0] / result.Re[2]);
re1[1] = (result.Re[1] / result.Re[2]);
re1[2] = (result.Re[2] / result.Re[2]);
re1[3] = (result.Re[3] / result.Re[2]);
re1[4] = (result.Re[4] / result.Re[2]);
re1[5] = (result.Re[5] / result.Re[2]);
double p[5];
p[0] = re1[0];
p[1] = -re1[3] / 2.0;
p[2] = -re1[4] / 2.0;
p[3] = re1[1] / 2.0;
p[4] = re1[5];
result.abc[0] = p[0] / (p[0] * pow(result.center[0], 2) + pow(result.center[1], 2) + 2 * p[3]*result.center[0] * result.center[1] - p[4]);
result.abc[1] = result.abc[0] / p[0];
result.abc[2] = p[3]*result.abc[1];
///计算椭圆长短轴
//double a, b, c, d, g, f;
//a = result.Re[0];
//b = (result.Re[1] / 2);
//c = result.Re[2];
//d = (result.Re[3] / 2);
//f = (result.Re[4] / 2);
//g = result.Re[5];
//result.longShortAxis[0] = sqrt((2 * (a*pow(f, 2) + c*pow(d, 2) + g*pow(b, 2) - 2 * b*d*f - a*c*g)) / ((pow(b, 2) - a*c)*(sqrt(pow((a - c), 2) + 4 * pow(b, 2)) - (a + c))));
//result.longShortAxis[1] = sqrt((2 * (a*pow(f, 2) + c*pow(d, 2) + g*pow(b, 2) - 2 * b*d*f - a*c*g)) / ((pow(b, 2) - a*c)*(-sqrt(pow((a - c), 2) + 4 * pow(b, 2)) - (a + c))));
double temp;
if (fabs(result.longShortAxis[0]) > fabs(result.longShortAxis[1])){
temp = result.longShortAxis[0];
result.longShortAxis[0] = result.longShortAxis[1];
result.longShortAxis[1] = temp;
}
return result;
}