最小二乘法拟合椭圆（椭圆拟合线）

转自：https://blog.csdn.net/weixin_39591047/article/details/87542496

参考文章：
最小二乘法拟合椭圆——MATLAB和Qt-C++实现
https://blog.csdn.net/sinat_21107433/article/details/80877758

以上文章中，C++代码有问题。因此参考如下文章，得到正确的结果。

矩阵求逆-高斯消元法介绍及其实现
https://blog.csdn.net/qithon/article/details/80100029

代码如下：

  1 import android.util.Log;
  2  
  3 public class EclipseFitting {
  4   
  5   /**
  6    * 
  7    * 功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用消元法 
  8    *            解得最小二乘解作为椭圆参数。
  9    *             
 10    *调用形式： EclipseFitting2（arrayx,arrayy,Result）；      
 11    *参数说明： arrayx: arrayx[n] 为第n个点的x坐标 
 12            arrayy: arrayy[n]为第n点的y坐标 
 13            n     : 点的个数 
 14            Result   : Result[0][],椭圆的五个参数，分别为center.x,center.y,2a,2b,xtheta 
 15                  Result[1][],椭圆方程的的五个系数（A,B,C,D,E），X^2+A*XY+B*Y^2+C*X+D*Y+E=0 
 16                  esp: 解精度，通常取1e-6,这个是解方程用的说 
 17    * 
 18    * 
 19    * @param arrayx
 20    * @param arrayy
 21    * @param n
 22    * @param Result
 23    */
 24   
 25   void EclipseFitting2(int [] arrayx, int [] arrayy,int N,double [][] Result)
 26   {
 27        double A = 0.00,B = 0.00,C = 0.00,D = 0.00,E = 0.00;
 28        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;
 29       //平面上任意位置的一个椭圆，其中心坐标为（x0，y0），半长轴a，半短轴b，长轴偏角为θ，方程通式为
 30        //方程为：X^2+A*XY+B*Y^2+C*X+D*Y+E=0
 31  
 32       for(int i = 0; i < N; i++)
 33       {
 34           double xi = arrayx[i], yi = arrayy[i];
 35  
 36           x2y2 += xi*xi*yi*yi;
 37           x1y3 += xi*yi*yi*yi;
 38           x2y1 += xi*xi*yi;
 39           x1y2 += xi*yi*yi;
 40           x1y1 += xi*yi;
 41           yyy4 += yi*yi*yi*yi;
 42           yyy3 += yi*yi*yi;
 43           yyy2 += yi*yi;
 44           xxx2 += xi*xi;
 45           xxx1 += xi;
 46           yyy1 += yi;
 47           x3y1 += xi*xi*xi*yi;
 48           xxx3 += xi*xi*xi;
 49       }
 50       
 51  //5*5 matrix
 52  
 53  
 54       double[][] matrix={{x2y2,x1y3,x2y1,x1y2,x1y1},
 55                                 {x1y3,yyy4,x1y2,yyy3,yyy2},
 56                                 {x2y1,x1y2,xxx2,x1y1,xxx1},
 57                                 {x1y2,yyy3,x1y1,yyy2,yyy1},
 58                                 {x1y1,yyy2,xxx1,yyy1,N}
 59                                };
 60  
 61        double[] matrix2={x3y1,x2y2,xxx3,x2y1,xxx2};
 62        double[] matrix3 = {A,B,C,D,E};
 63  
 64        Log.i("EclipseFitting2","系数矩阵");
 65        LogMatrix(matrix,5);
 66        
 67        
 68       求矩阵matrix的逆，结果为InverseMatrix
 69   
 70 ///      
 71       double[][] mInverseMatrix=InverseMatrix(matrix,5);
 72       
 73       ///求参数A,B,C,D,E
 74       for(int i=0;i<5;i++)
 75       {
 76           for(int j=0;j<5;j++)
 77           {
 78               matrix3[i] += mInverseMatrix[i][j]*(-matrix2[j]);
 79           }
 80       }
 81       A = matrix3[0];
 82       B = matrix3[1];
 83       C = matrix3[2];
 84       D = matrix3[3];
 85       E = matrix3[4];
 86  
 87       ///求拟合结果重要参数
 88       
 89       double Xc = (2*B*C-A*D)/(A*A-4*B);
 90       double Yc = (2*D-A*D)/(A*A-4*B);
 91       double a = Math.sqrt(Math.abs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B-Math.sqrt(A*A+(1-B)*(1-B))+1))));
 92       double b = Math.sqrt(Math.abs(2*(A*C*D-B*C*C-D*D+4*B*E-A*A*E)/((A*A-4*B)*(B+Math.sqrt(A*A+(1-B)*(1-B))+1))));
 93       double theta = Math.atan2(a*a-b*b*B,a*a*B-b*b);
 94       
 95       Result[0][0]=Xc;      Result[0][1]=Yc;
 96       Result[0][2]=a;      Result[0][3]=b;
 97       Result[0][4]=theta*180/3.1415926;
 98       
 99       Result[1][0]=A; Result[1][1]=B; Result[1][2]=C;
100       Result[1][3]=D; Result[1][4]=E;
101  
102   }
103   
104   /**
105     matrix为所求矩阵
106     dim为矩阵的维度
107 */
108  public static double[][] InverseMatrix(double[][] matrix, int dim)
109  {
110       double eps = 1e-6;
111       double[][] mat = new double [dim][ dim * 2];
112       //构造增广矩阵W
113       for(int i = 0;i < dim; i++)
114       {
115          for(int j = 0;j < 2 * dim; j++)
116          {
117               if(j < dim)
118               {
119                   mat[i][j] = matrix[i][j];
120               }
121               else
122               {
123                   mat[i][j] = (j - dim == i) ? 1 : 0;
124               }
125           }
126       }
127  
128       
129       
130     //从上往下消元       
131       for(int i = 0;i < dim; i++)
132       {
133           if(Math.abs(mat[i][i]) < eps)
134           //若mat[i,i]为0，则往下找一个不为0的行，加到第i行
135           {
136               int j;
137               for ( j = i + 1; j < dim; j++)
138               {
139                   if (Math.abs(mat[j][ i]) > eps) break;
140               }
141               if (j == dim) return null;
142               for(int r = i; r < 2 * dim; r++)
143               {
144                   mat[i][ r] += mat[j][ r]; 
145               }
146           }
147           
148           
149           double ep = mat[i][ i];
150           //将mat[i][i]变为1
151           for (int r = i; r < 2 * dim; r++)
152           {
153               mat[i][ r] /= ep;
154           }
155  
156           for(int j = i + 1; j < dim; j++)
157           {
158               double e = -1 * (mat[j][ i] / mat[i][ i]);
159               for(int r = i; r < 2 * dim; r++)
160               {
161                   mat[j][ r] += e * mat[i][ r];
162               }
163           }
164       }
165  
166       LogMatrix(mat,10);      
167  
168 //从下往上消元      
169       for(int i = dim - 1; i >= 0; i--)
170       {
171           for (int j = i - 1; j >= 0; j--)
172           {
173               double e = -1 * (mat[j][ i] / mat[i][ i]);
174               for (int r = i; r < 2 * dim; r++)
175               {
176                   mat[j][ r] += e * mat[i][ r];
177               }
178           }
179       }
180  
181       
182       LogMatrix(mat,10);      
183      
184       
185       double[][] result = new double[dim][ dim];
186       for(int i = 0;i < dim; i++)
187       {
188           for(int r = dim; r < 2 * dim; r++)
189           {
190               result[i][ r - dim] = mat[i][ r];
191           }
192       }
193       
194       return result;
195  }
196  
197  
198 // 原文：https://blog.csdn.net/qithon/article/details/80100029 
199  
200   
201   
202   
203   
204 private static void  LogMatrix(double[][] Matrix, int width){
205     
206     for(int i=0;i<5;i++){
207     String information=" ";
208         
209       for(int j=0;j<width;j++){
210           information+=Matrix[i][j]+",";
211       }
212       
213       
214     Log.i("EclipseFitting2",information);
215       
216     }
217     
218 }
219   
220   
221  
222  // 原文：https://blog.csdn.net/sinat_21107433/article/details/80877758 
223  
224     
225 }