椭圆拟合理论推导和Matlab实现

1 理论推导

前面一篇文章，解决了最小二乘圆拟合的问题。这篇文章将以类似的方法解决椭圆拟合的问题。

首先搞清楚问题，最小二乘拟合椭圆，即输入为散点集{(xi,yi)∣xi∈R,yi∈R}\{(x_i,y_i)|x_i\in \R,y_i\in \R\}{(xi,yi)∣xi∈R,yi∈R},定义新的向量{X∣X=[x1x2...xn]T∈Rn}\{X|X=[x_1\ x_2...x_n]^T\in \R^n\}{X∣X=[x1 x2...xn]T∈Rn}和{Y∣Y=[y1y2...yn]T∈Rn}\{Y|Y=[y_1\ y_2...y_n]^T\in \R^n\}{Y∣Y=[y1 y2...yn]T∈Rn},求解能够尽可能接近这些点的椭圆。一般斜椭圆具有5个参数，即椭圆中心坐标(x0,y0)(x_0,y_0)(x0,y0),椭圆长径和短径R1,R2R_1,R_2R1,R2以及坐标轴旋转的角度ϕ\phiϕ，只需要求解了这几个参数椭圆就被唯一确定了。那么对于椭圆的求解则至少需要5个独立的方程。即输入的点的个数至少是5个。

然后开始我们正式的推导，我们在中学时代就已经知道，一般的圆锥曲线的隐式方程为：
Ax2+Bxy+Cy2+Dx+Ey+1=0Ax^2+Bxy+Cy^2+Dx+Ey+1=0 Ax2+Bxy+Cy2+Dx+Ey+1=0
其与我们想要参数之间的转换关系是（这个地方的推导比较复杂，不是重点）：
{x0=BE−2CD4AC−B2y0=BD−2AE4AC−B2R1=2(Ax02+Cy02+Bx0y0−1)A+C+(A−C)2+B2R2=2(Ax02+Cy02+Bx0y0−1)A+C−(A−C)2+B2ϕ=12arctan(BA−C)\begin{cases} \begin{aligned} x_0&=\frac{BE-2CD}{4AC-B^2}\\ y_0&=\frac{BD-2AE}{4AC-B^2}\\ R_1&=\sqrt{\frac{2(Ax_0^2+Cy_0^2+Bx_0y_0-1)}{A+C+\sqrt{(A-C)^2+B^2}}}\\ R_2&=\sqrt{\frac{2(Ax_0^2+Cy_0^2+Bx_0y_0-1)}{A+C-\sqrt{(A-C)^2+B^2}}}\\ \phi &= \frac{1}{2}arctan(\frac{B}{A-C}) \end{aligned} \end{cases} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧x0y0R1R2ϕ=4AC−B2BE−2CD=4AC−B2BD−2AE=A+C+(A−C)2+B22(Ax02+Cy02+Bx0y0−1)=A+C−(A−C)2+B22(Ax02+Cy02+Bx0y0−1)=21arctan(A−CB)
值得一提的是，上式中的长径和短径并一定满足R1>R2R_1>R_2R1>R2,称呼为旋转前X轴截距的一半和y轴截距的一半其实更为准确。这样做的目的是为了不失一般性；同样为了不失去一般性，设ϕ∈[0,2π]\phi \in [0,2\pi]ϕ∈[0,2π]。因此在Matlab中使用反三角函数时，采用atan2() 二不是 atan()。

为了计算的方便，凭借我们朴素的数学直觉，我们对于隐式方程进行x2x^2x2项系数归一处理：
x2+axy+by2+cx+dy+e=0x^2+axy+by^2+cx+dy+e=0 x2+axy+by2+cx+dy+e=0

对于每个点，都是关于未知数向量[abcde]T[a\ b\ c\ d\ e]^T[a b c d e]T的线性方程，写成矩阵形式为：
[xyy2xy1]×[abcde]=−x2\begin{bmatrix} xy&y^2&x&y&1 \end{bmatrix}\times \begin{bmatrix} a\\b\\c\\d\\e \end{bmatrix}=-x^2 [xyy2xy1]×⎣⎢⎢⎢⎢⎡abcde⎦⎥⎥⎥⎥⎤=−x2
这样对于每个点，我们都能得到一个线性方程。对于nnn个点，我们将得到的方程写成矩阵的形式：
[x1y1y12x1y11x2y2y22x2y21⋮⋮⋮⋮⋮xnynyn2xnyn1]×[abcde]=−[x12x22⋮xn2]\begin{bmatrix} x_1y_1&y_1^2&x_1&y_1&1\\ x_2y_2&y_2^2&x_2&y_2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots\\ x_ny_n&y_n^2&x_n&y_n&1 \end{bmatrix}\times \begin{bmatrix} a\\b\\c\\d\\e \end{bmatrix}= -\begin{bmatrix} x_1^2\\x_2^2\\\vdots\\x_n^2 \end{bmatrix} ⎣⎢⎢⎢⎡x1y1x2y2⋮xnyny12y22⋮yn2x1x2⋮xny1y2⋮yn11⋮1⎦⎥⎥⎥⎤×⎣⎢⎢⎢⎢⎡abcde⎦⎥⎥⎥⎥⎤=−⎣⎢⎢⎢⎡x12x22⋮xn2⎦⎥⎥⎥⎤
这个问题中，未知数的个数为5，独立方程的个数为nnn。矩阵论中给出了形如AX=bAX=bAX=b的方程组在独立方程个数多于未知数个数时的最佳二乘解为X=A+bX=A^+bX=A+b，其中A+A^+A+表示的是矩阵AAA的M-P广义逆。

对于我们这个问题我们需要求下面这个矩阵的广义逆:
A=[x1y1y12x1y11x2y2y22x2y21⋮⋮⋮⋮⋮xnynyn2xnyn1]A=\begin{bmatrix} x_1y_1&y_1^2&x_1&y_1&1\\ x_2y_2&y_2^2&x_2&y_2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots\\ x_ny_n&y_n^2&x_n&y_n&1 \end{bmatrix} A=⎣⎢⎢⎢⎡x1y1x2y2⋮xnyny12y22⋮yn2x1x2⋮xny1y2⋮yn11⋮1⎦⎥⎥⎥⎤
对于拟合问题，矩阵AAA是一个列满秩矩阵。矩阵论中给出了列满秩矩阵的广义逆计算公式为：
A+=(AHA)−AHA^+=(A^HA)^-A^H A+=(AHA)−AH
对于实矩阵，AHA^HAH表示其对称矩阵，则未知数构成的向量的解为：
[abcde]=(AHA)−AH[−x12−x22⋮−xn2]=B−C\begin{bmatrix} a\\b\\c\\d\\e \end{bmatrix}=(A^HA)^-A^H \begin{bmatrix} -x_1^2\\-x_2^2\\\vdots\\-x_n^2\\ \end{bmatrix}=B^-C ⎣⎢⎢⎢⎢⎡abcde⎦⎥⎥⎥⎥⎤=(AHA)−AH⎣⎢⎢⎢⎡−x12−x22⋮−xn2⎦⎥⎥⎥⎤=B−C
计算BBB矩阵:
B=AHA=[x1y1x2y2⋯xnyny12y22⋯yn2x1x2⋯xny1y2⋯yn11⋯1][x1y1y12x1y11x2y2y22x2y21⋮⋮⋮⋮⋮xnynyn2xnyn1]=[∑i=1nxi2yi2∑i=1nxiyi3∑i=1nxi2yi∑i=1nxiyi2∑i=1nxiyi∑i=1nxiyi3∑i=1nyi4∑i=1nxiyi2∑i=1nyi3∑i=1nyi2∑i=1nxi2yi∑i=1nxiyi2∑i=1nxi2∑i=1nxiyi∑i=1nxi∑i=nnxiyi2∑i=1nyi3∑i=1nxiyi∑i=1nyi2∑i=1nyi∑i=1nxiyi∑i=1nyi2∑i=1nxi∑i=1nyi∑i=1n1]B=A^HA = \begin{bmatrix} x_1y_1&x_2y_2&\cdots&x_ny_n\\ y_1^2&y_2^2&\cdots&y_n^2\\ x_1&x_2&\cdots&x_n\\ y_1&y_2&\cdots&y_n\\ 1&1&\cdots&1 \end{bmatrix} \begin{bmatrix} x_1y_1&y_1^2&x_1&y_1&1\\ x_2y_2&y_2^2&x_2&y_2&1\\ \vdots&\vdots&\vdots&\vdots&\vdots\\ x_ny_n&y_n^2&x_n&y_n&1 \end{bmatrix}\\= \begin{bmatrix} \sum_{i=1}^nx_i^2y_i^2&\sum_{i=1}^nx_iy_i^3&\sum_{i=1}^nx_i^2y_i&\sum_{i=1}^nx_iy_i^2&\sum_{i=1}^nx_iy_i\\ \sum_{i=1}^nx_iy_i^3&\sum_{i=1}^ny_i^4&\sum_{i=1}^nx_iy_i^2&\sum_{i=1}^ny_i^3&\sum_{i=1}^ny_i^2\\ \sum_{i=1}^nx_i^2y_i&\sum_{i=1}^nx_iy_i^2&\sum_{i=1}^nx_i^2&\sum_{i=1}^nx_iy_i&\sum_{i=1}^nx_i\\ \sum_{i=n}^nx_iy_i^2&\sum_{i=1}^ny_i^3&\sum_{i=1}^nx_iy_i&\sum_{i=1}^ny_i^2&\sum_{i=1}^ny_i\\ \sum_{i=1}^nx_iy_i&\sum_{i=1}^ny_i^2&\sum_{i=1}^nx_i &\sum_{i=1}^ny_i &\sum_{i=1}^n1 \end{bmatrix} B=AHA=⎣⎢⎢⎢⎢⎡x1y1y12x1y11x2y2y22x2y21⋯⋯⋯⋯⋯xnynyn2xnyn1⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡x1y1x2y2⋮xnyny12y22⋮yn2x1x2⋮xny1y2⋮yn11⋮1⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡∑i=1nxi2yi2∑i=1nxiyi3∑i=1nxi2yi∑i=nnxiyi2∑i=1nxiyi∑i=1nxiyi3∑i=1nyi4∑i=1nxiyi2∑i=1nyi3∑i=1nyi2∑i=1nxi2yi∑i=1nxiyi2∑i=1nxi2∑i=1nxiyi∑i=1nxi∑i=1nxiyi2∑i=1nyi3∑i=1nxiyi∑i=1nyi2∑i=1nyi∑i=1nxiyi∑i=1nyi2∑i=1nxi∑i=1nyi∑i=1n1⎦⎥⎥⎥⎥⎤
计算CCC矩阵：
C=AH×[−x12−x22⋮−xn2]=[x1y1x2y2⋯xnyny12y22⋯yn2x1x2⋯xny1y2⋯yn11⋯1][−x12−x22⋮−xn2]=−[∑i=1nxi3yi∑i=1nxi2yi2∑i=1nxi3∑i=1nxi2yi∑i=1nxi2]C=A^H\times \begin{bmatrix} -x_1^2\\ -x_2^2\\ \vdots\\ -x_n^2 \end{bmatrix}= \begin{bmatrix} x_1y_1&x_2y_2&\cdots&x_ny_n\\ y_1^2&y_2^2&\cdots&y_n^2\\ x_1&x_2&\cdots&x_n\\ y_1&y_2&\cdots&y_n\\ 1&1&\cdots&1 \end{bmatrix} \begin{bmatrix} -x_1^2\\ -x_2^2\\ \vdots\\ -x_n^2 \end{bmatrix} =-\begin{bmatrix} \sum_{i=1}^nx_i^3y_i\\ \sum_{i=1}^nx_i^2y_i^2\\ \sum_{i=1}^nx_i^3\\ \sum_{i=1}^nx_i^2y_i\\ \sum_{i=1}^nx_i^2 \end{bmatrix} C=AH×⎣⎢⎢⎢⎡−x12−x22⋮−xn2⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡x1y1y12x1y11x2y2y22x2y21⋯⋯⋯⋯⋯xnynyn2xnyn1⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎡−x12−x22⋮−xn2⎦⎥⎥⎥⎤=−⎣⎢⎢⎢⎢⎡∑i=1nxi3yi∑i=1nxi2yi2∑i=1nxi3∑i=1nxi2yi∑i=1nxi2⎦⎥⎥⎥⎥⎤

细心的读者可能会发现，如果样本的值本身比较大，那么对齐进行平方、立方等处理后进行求和，很可能会导致程序中的变量对应的寄存器存储不了这么庞大的数，因此在程序中需要把xi,yix_i,y_ixi,yi变换到xi∈[−1,1],yi∈[−1,1]x_i \in[-1,1],y_i\in[-1,1]xi∈[−1,1],yi∈[−1,1]。具体做法是每个元素除以其构成的向量中的最大绝对值。最后计算完后再进行逆变换。

至此我们在具备矩阵论知识的基础上，推导了最佳拟合椭圆的解析解。

2 Matlab 实现

[Talk is cheap;Show me the code.]

在前文理论的支撑下，啪的一下就写完了，很快啊！

最小二乘椭圆拟合函数：

%%020406081012141618202224262830323436384042444648505254565860626466687072747678%%
%@func:Fit the scatters by least-square ellipse;
%@inpt:Coordinate set (x,y);
%@oupt:The structure contains the center of the ellipse, the long and short dia-
%      meters and the rotation angle;
%@note:
%@date:2021/01/23 by xiyin,li in hust;
function [ellipse] = ellipseFit(x,y)
xlength = length(x);
xmax = max(abs(x));
ymax = max(abs(y));
x = x./xmax;
y = y./ymax;%%归一化处理
if(xlength ~= length(y) | xlength  < 5)warning('the length of x and y must be same and >= 5');
elsexigema.one = xlength;  xigema.x = sum(x);     xigema.y = sum(y);xigema.xx =sum(x.*x);  xigema.xy = sum(x.*y); xigema.yy = sum(y.*y);xigema.xxx = sum(x.^3); xigema.xxy = sum(x.*x.*y);xigema.xyy = sum(x.*y.*y); xigema.yyy = sum(y.^3);xigema.xxxy = sum(x.^3.*y);xigema.xxyy = sum(x.*x.*y.*y);xigema.xyyy = sum(x.*y.^3);xigema.yyyy = sum(y.^4);B = [xigema.xxyy xigema.xyyy xigema.xxy xigema.xyy xigema.xy;xigema.xyyy xigema.yyyy xigema.xyy xigema.yyy xigema.yy;xigema.xxy xigema.xyy xigema.xx xigema.xy xigema.x;xigema.xyy xigema.yyy xigema.xy xigema.yy xigema.y;xigema.xy xigema.yy xigema.x xigema.y xigema.one];if(rank(B)<5)warning("Please check input set again")elseC = [xigema.xxxy;xigema.xxyy;xigema.xxx;xigema.xxy;xigema.xx];D = -inv(B)*C;%标准方程参数(注意反归一化)norm.a = 1/(xmax*xmax*D(5));norm.b = D(1)/(xmax*ymax*D(5));norm.c = D(2)/(ymax*ymax*D(5));norm.d = D(3)/(xmax*D(5));norm.e = D(4)/(ymax*D(5));%参数方程参数ellipse.x0  =(norm.b*norm.e-2*norm.c*norm.d)/(4*norm.a*norm.c-norm.b^2);ellipse.y0 = (norm.b*norm.d-2*norm.a*norm.e)/(4*norm.a*norm.c-norm.b^2);ellipse.a = sqrt((2*(norm.a*ellipse.x0^2+norm.c*ellipse.y0^2+norm.b*ellipse.x0*ellipse.y0-1)).../(norm.a+norm.c+sqrt((norm.a-norm.c)^2+norm.b^2)));ellipse.b = sqrt((2*(norm.a*ellipse.x0^2+norm.c*ellipse.y0^2+norm.b*ellipse.x0*ellipse.y0-1)).../(norm.a+norm.c-sqrt((norm.a-norm.c)^2+norm.b^2)));ellipse.phi = 0.5*atan2(norm.b,(norm.a-norm.c));%绘制图像theta =  linspace(0.1*pi,1.9*pi,100)plot(x*xmax,y*ymax,'o')hold onx = ellipse.a * cos(theta);y = ellipse.b * sin(theta);Ex = (x*cos(ellipse.phi)-y*sin(ellipse.phi) + ellipse.x0);Ey = (x*sin(ellipse.phi)+y*cos(ellipse.phi) + ellipse.y0);plot(Ex,Ey,'r','linewidth',1.6)axis equalend
end
end

使用如下的例子进行测试(对于斜椭圆和其他形式的椭圆已经做过了测试)：

clc,clear;
close all
Sensor012 = csvread('CircleSensor012.csv');
Sensor.num1.x = Sensor012(4,:);
Sensor.num1.y = Sensor012(5,:);
Sensor.num1.z = Sensor012(6,:);
figure(1)
ellipse = ellipseFit(Sensor.num1.x,Sensor.num1.z)
figure(2)
t = linspace(0,2*pi,180);
x = 5000*sin(t-0.1*pi);
y = 3000*cos(t+pi/4);
ellipse = ellipseFit(x,y)

结果为：

3 总结

本文通过M-P广义逆求取了最小二乘椭圆。
本文通过Matlab进行了实现，实现过程与推导过程完全一致。
今天Markdown比昨天顺手多了，公式等号对齐怎么这么难用。
后面将进行双曲线拟合以及其他一些二次型曲线拟合（如果用不到估计就不会写了）。