logo
搜索
我的图书馆

发文章

发文工具

其他工具

留言交流
搜索
分享

最小二乘法用于直线，多项式，圆，椭圆的拟合及程序实现

 战神之家 2019-04-09

最小二乘法是一种优化算法，最小二乘法名字的缘由有两个：一是要将误差最小化，二是将误差最小化的方法是使误差的平方和最小化。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法还可用于曲线拟合，所拟合的曲线可以是线性拟合与非线性拟合。

--------------------一元线性函数--------------------

形式1：借用案例（http://blog.csdn.net/qll125596718/article/details/8248249）首先以一元线性方程参数估计为例，样本回归模型：


残差平方和：


则通过Q最小确定这条直线，即确定，以为变量，把它们看作是Q的函数，就变成了一个求极值的问题，可以通过求导数得到。求Q对两个待估参数的偏导数：


解得：

实例（c++）：
  1. #include <iostream>    
  2. #include <algorithm>    
  3. #include <valarray>    
  4. #include <vector>    
  5. using namespace std;
  7. int main()
  8. {
  9. 	double x[] = { 1, 2, 3, 4, 5, 6 };
  10. 	double y[] = { 3, 5.5, 6.8, 8.8, 11, 12};
  11. 	valarray<double> data_x(x, 6);
  12. 	valarray<double> data_y(y, 6);
  14. 	float A = 0.0;
  15. 	float B = 0.0;
  16. 	float C = 0.0;
  17. 	float D = 0.0;
  18. 	A = (data_x*data_x).sum();
  19. 	B = data_x.sum();
  20. 	C = (data_x*data_y).sum();
  21. 	D = data_y.sum();
  22. 	float tmp = A*data_x.size() - B*B;
  23. 	float k, b;
  24. 	k = (C*data_x.size() - B*D) / tmp;
  25. 	b = (A*D - C*B) / tmp;
  26. 	cout << "y=" << k << "x+" << b << endl;
  27. 	return 0;
  28. }
运行结果：


注：valarray类似vector，也是一个模板类，主要用来对一系列元素进行高速的数字计算，其与vector的主要区别在于以下两点：
1、valarray定义了一组在两个相同长度和相同类型的valarray类对象之间的数字计算；
2、通过重载operater[]，可以返回valarray的相关信息（valarray其中某个元素的引用、特定下标的值或者其某个子集）。

valarray类构造函数:
valarray( );
explicit valarray(size_t _Count);
valarray( const Type& _Val, size_t _Count);
valarray( const Type* _Ptr, size_t _Count);
valarray( const slice_array<Type>& _SliceArray);
valarray( const gslice_array<Type>& _GsliceArray);
valarray( const mask_array<Type>& _MaskArray);
valarray( const indirect_array<Type>& _IndArray);

valarray 类用法:
1. apply 将 valarray 数组的每一个值都用 apply 所接受到的函数进行计算
2. cshift 将 valarray 数组的数据进行循环移动，参数为正者左移为负就右移
3. max 返回 valarray 数组的最大值
4. min 返回 valarray 数组的最小值
5. resize 重新设置 valarray 数组大小，并对其进行初始化
6. shift 将 valarray 数组移动，参数为正者左移，为负者右移，移动后由 0 填充剩余位
7. size 得到数组的大小
8. sum 数组求和

--------------------N元线性函数--------------------

一元线性方程可以看做多元函数的特例，现在用矩阵形式表述多元函数情况下，最小二乘的一般形式：

 设拟合多项式为：

                   

各点到这条曲线的距离之和，即偏差平方和如下：

                

对等式右边求ai偏导数，得到： 

              

             

                            .......

             

把这些等式表示成矩阵的形式，就可以得到下面的矩阵：

                （3）

进行化简计算：

           ， 

上面公式（3）可以写为:

                   ，

              

  1. #include "stdio.h"
  2. #include "stdlib.h"
  3. #include "math.h"
  4. #include "vector"
  5. using namespace std;
  7. struct point
  8. {
  9. 	double x;
  10. 	double y;
  11. };
  13. typedef vector<double> doubleVector;
  14. vector<point> getFile(char *File);  //获取文件数据
  15. doubleVector getCoeff(vector<point> sample, int n);   //矩阵方程
  17. void main()
  18. {
  19. 	int i, n;
  20. 	char *File = "XY.txt";
  21. 	vector<point> sample;
  22. 	doubleVector  coefficient;
  23. 	sample = getFile(File);
  24. 	printf("拟合多项式阶数n=");
  25.   	  scanf_s("%d", &n);
  26. 	  coefficient = getCoeff(sample, n);
  27. 	printf("\n拟合矩阵的系数为：\n");
  28. 	for (i = 0; i < coefficient.size(); i++)
  29. 		printf("a%d = %lf\n", i, coefficient[i]);
  30. }
  31. //矩阵方程
  32. doubleVector getCoeff(vector<point> sample, int n)
  33. {
  34. 	vector<doubleVector> matFunX;  //公式3左矩阵
  35. 	vector<doubleVector> matFunY;  //公式3右矩阵
  36. 	doubleVector temp;
  37. 	double sum;
  38. 	int i, j, k;
  39. 	//公式3左矩阵
  40. 	for (i = 0; i <= n; i++)
  41. 	{
  42. 		temp.clear();
  43. 		for (j = 0; j <= n; j++)
  44. 		{
  45. 			sum = 0;
  46. 			for (k = 0; k < sample.size(); k++)
  47. 				sum += pow(sample[k].x, j + i);
  48. 			temp.push_back(sum);
  49. 		}
  50. 		matFunX.push_back(temp);
  51. 	}
  52. 	//printf("matFunX.size=%d\n", matFunX.size());
  53. 	//printf("matFunX[3][3]=%f\n", matFunX[3][3]);
  55. 	//公式3右矩阵
  56. 	for (i = 0; i <= n; i++)
  57. 	{
  58. 		temp.clear();
  59. 		sum = 0;
  60. 		for (k = 0; k < sample.size(); k++)
  61. 			sum += sample[k].y*pow(sample[k].x, i);
  62. 		temp.push_back(sum);
  63. 		matFunY.push_back(temp);
  64. 	}
  65. 	printf("matFunY.size=%d\n", matFunY.size());
  66. 	//矩阵行列式变换
  67. 	double num1, num2, ratio;
  68. 	for (i = 0; i < matFunX.size() - 1; i++)
  69. 	{
  70. 		num1 = matFunX[i][i];
  71. 		for (j = i + 1; j < matFunX.size(); j++)
  72. 		{
  73. 			num2 = matFunX[j][i];
  74. 			ratio = num2 / num1;
  75. 			for (k = 0; k < matFunX.size(); k++)
  76. 				matFunX[j][k] = matFunX[j][k] - matFunX[i][k] * ratio;
  77. 			matFunY[j][0] = matFunY[j][0] - matFunY[i][0] * ratio;
  78. 		}
  79. 	}
  80. 	//计算拟合曲线的系数
  81. 	doubleVector coeff(matFunX.size(), 0);
  82. 	for (i = matFunX.size() - 1; i >= 0; i--)
  83. 	{
  84. 		if (i == matFunX.size() - 1)
  85. 			coeff[i] = matFunY[i][0] / matFunX[i][i];
  86. 		else
  87. 		{
  88. 			for (j = i + 1; j < matFunX.size(); j++)
  89. 				matFunY[i][0] = matFunY[i][0] - coeff[j] * matFunX[i][j];
  90. 			coeff[i] = matFunY[i][0] / matFunX[i][i];
  91. 		}
  92. 	}
  93. 	return coeff;
  94. }
  96. //获取文件数据
  97. vector<point> getFile(char *File)
  98. {
  99. 	int i = 1;
  100. 	vector<point> dst;
  102. 	FILE *fp=fopen(File, "r");
  104. 	if (fp == NULL)
  105. 	{
  106. 		printf("Open file error!!!\n");
  107. 		exit(0);
  108. 	}
  110. 	point temp;
  111. 	double num;
  113. 	while (fscanf(fp, "%lf", &num) != EOF)
  114. 	{
  115. 		if (i % 2 == 0)
  116. 		{
  117. 			temp.y = num;
  118. 			dst.push_back(temp);
  119. 		}
  120. 		else
  121. 			temp.x = num;
  122. 		i++;
  123. 	}
  124. 	fclose(fp);
  125. 	return dst;
  126. }

XY.txt内容：

  1. 0    1.0
  2. 0.25 1.28
  3. 0.5  1.65
  4. 0.75 2.12
  5. 1    2.72

另外在http://blog.csdn.net/lsh_2013/article/details/46697625里也有相关程序：

  1. #include <iostream>  
  2. #include <vector>  
  3. #include <cmath>  
  4. using namespace std;  
  6. //最小二乘拟合相关函数定义  
  7. double sum(vector<double> Vnum, int n);  
  8. double MutilSum(vector<double> Vx, vector<double> Vy, int n);  
  9. double RelatePow(vector<double> Vx, int n, int ex);  
  10. double RelateMutiXY(vector<double> Vx, vector<double> Vy, int n, int ex);  
  11. void EMatrix(vector<double> Vx, vector<double> Vy, int n, int ex, double coefficient[]);  
  12. void CalEquation(int exp, double coefficient[]);  
  13. double F(double c[],int l,int m);  
  14. double Em[6][4];  
  16. //主函数，这里将数据拟合成二次曲线  
  17. int main(int argc, char* argv[])  
  18. {  
  19.     double arry1[5]={0,0.25,0,5,0.75};  
  20.     double arry2[5]={1,1.283,1.649,2.212,2.178};  
  21.     double coefficient[5];  
  22.     memset(coefficient,0,sizeof(double)*5);  
  23.     vector<double> vx,vy;  
  24.     for (int i=0; i<5; i++)  
  25.     {  
  26.         vx.push_back(arry1[i]);  
  27.         vy.push_back(arry2[i]);  
  28.     }  
  29.     EMatrix(vx,vy,5,3,coefficient);  
  30.     printf("拟合方程为：y = %lf + %lfx + %lfx^2 \n",coefficient[1],coefficient[2],coefficient[3]);  
  31.     return 0;  
  32. }  
  33. //累加  
  34. double sum(vector<double> Vnum, int n)  
  35. {  
  36.     double dsum=0;  
  37.     for (int i=0; i<n; i++)  
  38.     {  
  39.         dsum+=Vnum[i];  
  40.     }  
  41.     return dsum;  
  42. }  
  43. //乘积和  
  44. double MutilSum(vector<double> Vx, vector<double> Vy, int n)  
  45. {  
  46.     double dMultiSum=0;  
  47.     for (int i=0; i<n; i++)  
  48.     {  
  49.         dMultiSum+=Vx[i]*Vy[i];  
  50.     }  
  51.     return dMultiSum;  
  52. }  
  53. //ex次方和  
  54. double RelatePow(vector<double> Vx, int n, int ex)  
  55. {  
  56.     double ReSum=0;  
  57.     for (int i=0; i<n; i++)  
  58.     {  
  59.         ReSum+=pow(Vx[i],ex);  
  60.     }  
  61.     return ReSum;  
  62. }  
  63. //x的ex次方与y的乘积的累加  
  64. double RelateMutiXY(vector<double> Vx, vector<double> Vy, int n, int ex)  
  65. {  
  66.     double dReMultiSum=0;  
  67.     for (int i=0; i<n; i++)  
  68.     {  
  69.         dReMultiSum+=pow(Vx[i],ex)*Vy[i];  
  70.     }  
  71.     return dReMultiSum;  
  72. }  
  73. //计算方程组的增广矩阵  
  74. void EMatrix(vector<double> Vx, vector<double> Vy, int n, int ex, double coefficient[])  
  75. {  
  76.     for (int i=1; i<=ex; i++)  
  77.     {  
  78.         for (int j=1; j<=ex; j++)  
  79.         {  
  80.             Em[i][j]=RelatePow(Vx,n,i+j-2);  
  81.         }  
  82.         Em[i][ex+1]=RelateMutiXY(Vx,Vy,n,i-1);  
  83.     }  
  84.     Em[1][1]=n;  
  85.     CalEquation(ex,coefficient);  
  86. }  
  87. //求解方程  
  88. void CalEquation(int exp, double coefficient[])  
  89. {  
  90.     for(int k=1;k<exp;k++) //消元过程  
  91.     {  
  92.         for(int i=k+1;i<exp+1;i++)  
  93.         {  
  94.             double p1=0;  
  96.             if(Em[k][k]!=0)  
  97.                 p1=Em[i][k]/Em[k][k];  
  99.             for(int j=k;j<exp+2;j++)   
  100.                 Em[i][j]=Em[i][j]-Em[k][j]*p1;  
  101.         }  
  102.     }  
  103.     coefficient[exp]=Em[exp][exp+1]/Em[exp][exp];  
  104.     for(int l=exp-1;l>=1;l--)   //回代求解  
  105.         coefficient[l]=(Em[l][exp+1]-F(coefficient,l+1,exp))/Em[l][l];  
  106. }  
  107. //供CalEquation函数调用  
  108. double F(double c[],int l,int m)  
  109. {  
  110.     double sum=0;  
  111.     for(int i=l;i<=m;i++)  
  112.         sum+=Em[l-1][i]*c[i];  
  113.     return sum;   
  114. }

memset相关介绍：

http://baike.baidu.com/link?url=p7JreiRCj9yPs3r3WAfsXgynjvtGrWoQ_exF9tFGK6fsVP7V6tdm-_13QhCZxqPrfRi0wH0EihhRL_-qVvrewq

http://c./cpp/html/157.html

--------------------拟合圆的方程--------------------






  1. /*
  2. 最小二乘法拟合圆，拟合出的圆以圆心坐标和半径的形式表示
  3.  */
  4. typedef complex<int> POINT;
  5. bool FitCircle(const std::vector<POINT> &points, double &er_x, double &er_y, double &radius)
  6. {
  7.      cent_x = 0.0f;
  8.      cent_y = 0.0f;
  9.      radius = 0.0f;
  10.      if (points.size() < 3)
  11.      {
  12.          return false;
  13.      }
  15.      double sum_x = 0.0f, sum_y = 0.0f;
  16.      double sum_x2 = 0.0f, sum_y2 = 0.0f;
  17.      double sum_x3 = 0.0f, sum_y3 = 0.0f;
  18.      double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;
  20.      int N = points.size();
  21.      for (int i = 0; i < N; i++)
  22.      {
  23.          double x = points[i].real();
  24.          double y = points[i].imag();
  25.          double x2 = x * x;
  26.          double y2 = y * y;
  27.          sum_x += x;
  28.          sum_y += y;
  29.          sum_x2 += x2;
  30.          sum_y2 += y2;
  31.          sum_x3 += x2 * x;
  32.          sum_y3 += y2 * y;
  33.          sum_xy += x * y;
  34.          sum_x1y2 += x * y2;
  35.          sum_x2y1 += x2 * y;
  36.      }
  38.      double C, D, E, G, H;
  39.      double a, b, c;
  41.      C = N * sum_x2 - sum_x * sum_x;
  42.      D = N * sum_xy - sum_x * sum_y;
  43.      E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
  44.      G = N * sum_y2 - sum_y * sum_y;
  45.      H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
  46.      a = (H * D - E * G) / (C * G - D * D);
  47.      b = (H * C - E * D) / (D * D - G * C);
  48.      c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;
  50.      cent_x = a / (-2);
  51.      cent_y = b / (-2);
  52.      radius = sqrt(a * a + b * b - 4 * c) / 2;
  53.      return true;
  54. }

--------------------拟合椭圆方程--------------------

//LSEllipse.h

  1. /*************************************************************************
  2. 	功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用奇异值分解法
  3. 	           解得最小二乘解作为椭圆参数。
  4. 	调用形式： cvFitEllipse2f（arrayx,arrayy,box）；    
  5. 	参数说明： arrayx: arrayx[n],每个值为x轴一个点
  6. 	           arrayx: arrayy[n],每个值为y轴一个点
  7. 			   n     : 点的个数
  8. 			   box   : box[5],椭圆的五个参数，分别为center.x,center.y,2a,2b,xtheta
  9. 			   esp: 解精度，通常取1e-6,这个是解方程用的说
  10. ***************************************************************************/
  11. #include<cstdlib>
  12. #include<float.h>
  13. #include<vector>
  14. using namespace std;
  16. class LSEllipse
  17. {
  18. public:
  19. 	LSEllipse(void);
  20. 	~LSEllipse(void);
  21. 	vector<double> getEllipseparGauss(vector<CPoint> vec_point);
  22. 	void cvFitEllipse2f( int *arrayx, int *arrayy,int n,float *box );
  23. private:
  24. 	int SVD(float *a,int m,int n,float b[],float x[],float esp);
  25. 	int gmiv(float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka);
  26.     int ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka);
  27. 	int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);
  28. };
//LSEllipse.cpp 
  1. #include "LSEllipse.h"
  2. #include <cmath>
  4. LSEllipse::LSEllipse(void)
  5. {
  6. }
  8. LSEllipse::~LSEllipse(void)
  9. {
  10. }
  11. //列主元高斯消去法
  12. //A为系数矩阵，x为解向量，若成功，返回true，否则返回false，并将x清空。
  14. bool RGauss(const vector<vector<double> > & A, vector<double> & x)
  15. {
  16.     x.clear();
  17.     int n = A.size();
  18.     int m = A[0].size();
  19.     x.resize(n);
  20.     //复制系数矩阵，防止修改原矩阵
  21.     vector<vector<double> > Atemp(n);
  22.     for (int i = 0; i < n; i++)
  23.     {
  24.         vector<double> temp(m);
  25.         for (int j = 0; j < m; j++)
  26.         {
  27.             temp[j] = A[i][j];
  28.         }
  29.         Atemp[i] = temp;
  30.         temp.clear();
  31.     }
  32.     for (int k = 0; k < n; k++)
  33.     {
  34.         //选主元
  35.         double max = -1;
  36.         int l = -1;
  37.         for (int i = k; i < n; i++)
  38.         {
  39.             if (abs(Atemp[i][k]) > max)
  40.             {
  41.                 max = abs(Atemp[i][k]);
  42.                 l = i;
  43.             }
  44.         }
  45.         if (l != k)
  46.         {
  47.             //交换系数矩阵的l行和k行
  48.             for (int i = 0; i < m; i++)
  49.             {
  50.                 double temp = Atemp[l][i];
  51.                 Atemp[l][i] = Atemp[k][i];
  52.                 Atemp[k][i] = temp;
  53.             }
  54.         }
  55.         //消元
  56.         for (int i = k+1; i < n; i++)
  57.         {
  58.             double l = Atemp[i][k]/Atemp[k][k];
  59.             for (int j = k; j < m; j++)
  60.             {
  61.                 Atemp[i][j] = Atemp[i][j] - l*Atemp[k][j];
  62.             }
  63.         }
  64.     }
  65.     //回代
  66.     x[n-1] = Atemp[n-1][m-1]/Atemp[n-1][m-2];
  67.     for (int k = n-2; k >= 0; k--)
  68.     {
  69.         double s = 0.0;
  70.         for (int j = k+1; j < n; j++)
  71.         {
  72.             s += Atemp[k][j]*x[j];
  73.         }
  74.         x[k] = (Atemp[k][m-1] - s)/Atemp[k][k];
  75.     }
  76.     return true;
  77. }
  79. vector<double>  LSEllipse::getEllipseparGauss(vector<CPoint> vec_point)
  80. {
  81. 	vector<double> vec_result;
  82. 	double x3y1 = 0,x1y3= 0,x2y2= 0,yyy4= 0, xxx3= 0,xxx2= 0,x2y1= 0,yyy3= 0,x1y2= 0 ,yyy2= 0,x1y1= 0,xxx1= 0,yyy1= 0;
  83. 	int N = vec_point.size();
  84. 	for (int m_i = 0;m_i < N ;++m_i )
  85. 	{
  86. 		double xi = vec_point[m_i].x ;
  87. 		double yi = vec_point[m_i].y;
  88. 		x3y1 += xi*xi*xi*yi ;
  89. 		x1y3 += xi*yi*yi*yi;
  90. 		x2y2 += xi*xi*yi*yi; ;
  91. 		yyy4 +=yi*yi*yi*yi;
  92. 		xxx3 += xi*xi*xi ;
  93. 		xxx2 += xi*xi ;
  94. 		x2y1 += xi*xi*yi;
  96. 		x1y2 += xi*yi*yi;
  97. 		yyy2 += yi*yi;
  98. 		x1y1 += xi*yi;
  99. 		xxx1 += xi;
  100. 		yyy1 += yi;
  101. 		yyy3 += yi*yi*yi;
  102. 	}
  103. 	double resul[5];
  104. 	resul[0] = -(x3y1);
  105. 	resul[1] = -(x2y2);
  106. 	resul[2] = -(xxx3);
  107. 	resul[3] = -(x2y1);
  108. 	resul[4] = -(xxx2);
  109. 	long double Bb[5],Cc[5],Dd[5],Ee[5],Aa[5];
  110. 	Bb[0] = x1y3, Cc[0] = x2y1, Dd[0] = x1y2, Ee[0] = x1y1, Aa[0] = x2y2;
  111. 	Bb[1] = yyy4, Cc[1] = x1y2, Dd[1] = yyy3, Ee[1] = yyy2, Aa[1] = x1y3;
  112. 	Bb[2] = x1y2, Cc[2] = xxx2, Dd[2] = x1y1, Ee[2] = xxx1, Aa[2] = x2y1;
  113. 	Bb[3] = yyy3, Cc[3]= x1y1, Dd[3] = yyy2, Ee[3] = yyy1, Aa[3] = x1y2;
  114. 	Bb[4]= yyy2, Cc[4]= xxx1, Dd[4] = yyy1, Ee[4] = N, Aa[4]= x1y1;
  116.     vector<vector<double>>Ma(5);
  117. 	vector<double>Md(5);
  118. 	for(int i=0;i<5;i++)
  119. 	{
  120. 		Ma[i].push_back(Aa[i]);
  121. 		Ma[i].push_back(Bb[i]);
  122.         Ma[i].push_back(Cc[i]);
  123.         Ma[i].push_back(Dd[i]);
  124. 		Ma[i].push_back(Ee[i]);
  125. 		Ma[i].push_back(resul[i]);
  126. 	}
  128. 	RGauss(Ma,Md);
  129. 	long double A=Md[0];
  130. 	long double B=Md[1];
  131. 	long double C=Md[2];
  132. 	long double D=Md[3];
  133. 	long double E=Md[4];
  134. 	double XC=(2*B*C-A*D)/(A*A-4*B);
  135. 	double YC=(2*D-A*C)/(A*A-4*B);
  136. 	long double fenzi=2*(A*C*D-B*C*C-D*D+4*E*B-A*A*E);
  137. 	long double fenmu=(A*A-4*B)*(B-sqrt(A*A+(1-B)*(1-B))+1);
  138. 	long double fenmu2=(A*A-4*B)*(B+sqrt(A*A+(1-B)*(1-B))+1);
  139. 	double XA=sqrt(fabs(fenzi/fenmu));
  140. 	double XB=sqrt(fabs(fenzi/fenmu2));
  141. 	double Xtheta=0.5*atan(A/(1-B))*180/3.1415926;
  142. 	if(B<1)
  143. 		Xtheta+=90;
  144. 	vec_result.push_back(XC);
  145. 	vec_result.push_back(YC);
  146. 	vec_result.push_back(XA);
  147. 	vec_result.push_back(XB);
  148. 	vec_result.push_back(Xtheta);
  149. 	return vec_result;
  150. }
  152. void  LSEllipse::cvFitEllipse2f(  int *arrayx, int *arrayy,int n,float *box )
  153. {   
  154. 	float cx=0,cy=0;
  155.     double rp[5], t;
  156. 	float *A1=new float[n*5];
  157. 	float *A2=new float[2*2];
  158. 	float *A3=new float[n*3];
  159. 	float *B1=new float[n],*B2=new float[2],*B3=new float[n];
  160.     const double min_eps = 1e-6;
  161.     int i;
  162.     for( i = 0; i < n; i++ )
  163.     {
  165.         cx += arrayx[i]*1.0;
  166.         cy += arrayy[i]*1.0;
  168.     }
  169.     cx /= n;
  170.     cy /= n;
  171.     for( i = 0; i < n; i++ )
  172.     {
  173. 		int step=i*5;
  174. 		float px,py;
  175.         px = arrayx[i]*1.0;
  176.         py = arrayy[i]*1.0;
  177.         px -= cx;
  178.         py -= cy;
  179.         B1[i] = 10000.0;
  180.         A1[step] = -px * px;
  181.         A1[step + 1] = -py * py;
  182.         A1[step + 2] = -px * py;
  183.         A1[step + 3] = px;
  184.         A1[step + 4] = py;
  185.     }
  186. 	float *x1=new float[5];
  187. 	//解出Ax^2+By^2+Cxy+Dx+Ey=10000的最小二乘解！
  188. 	SVD(A1,n,5,B1,x1,min_eps);
  189. 	A2[0]=2*x1[0],A2[1]=A2[2]=x1[2],A2[3]=2*x1[1];
  190. 	B2[0]=x1[3],B2[1]=x1[4];
  191. 	float *x2=new float[2];
  192. 	//标准化，将一次项消掉，求出center.x和center.y;
  193. 	SVD(A2,2,2,B2,x2,min_eps);
  194.     rp[0]=x2[0],rp[1]=x2[1];
  195.     for( i = 0; i < n; i++ )
  196.     {
  197.         float px,py;
  198.         px = arrayx[i]*1.0;
  199.         py = arrayy[i]*1.0;
  200.         px -= cx;
  201.         py -= cy;
  202.         B3[i] = 1.0;
  203. 		int step=i*3;
  204.         A3[step] = (px - rp[0]) * (px - rp[0]);
  205.         A3[step+1] = (py - rp[1]) * (py - rp[1]);
  206.         A3[step+2] = (px - rp[0]) * (py - rp[1]);
  208.     }
  209. 	//求出A(x-center.x)^2+B(y-center.y)^2+C(x-center.x)(y-center.y)的最小二乘解
  210. 	SVD(A3,n,3,B3,x1,min_eps);
  212.     rp[4] = -0.5 * atan2(x1[2], x1[1] - x1[0]);
  213.     t = sin(-2.0 * rp[4]);
  214.     if( fabs(t) > fabs(x1[2])*min_eps )
  215.         t = x1[2]/t;
  216.     else
  217.         t = x1[1] - x1[0];
  218.     rp[2] = fabs(x1[0] + x1[1] - t);
  219.     if( rp[2] > min_eps )
  220.         rp[2] = sqrt(2.0 / rp[2]);
  221.     rp[3] = fabs(x1[0] + x1[1] + t);
  222.     if( rp[3] > min_eps )
  223.         rp[3] = sqrt(2.0 / rp[3]);
  225.     box[0] = (float)rp[0] + cx;
  226.     box[1]= (float)rp[1] + cy;
  227.     box[2]= (float)(rp[2]*2);
  228.     box[3] = (float)(rp[3]*2);
  229.     if( box[2] > box[3] )
  230.     {
  231. 		double tmp=box[2];
  232. 		box[2]=box[3];
  233. 		box[3]=tmp;
  234.     }
  235. 	box[4] = (float)(90 + rp[4]*180/3.1415926);
  236.     if( box[4] < -180 )
  237.         box[4] += 360;
  238.     if( box[4] > 360 )
  239.         box[4] -= 360;
  240. 	delete []A1;
  241. 	delete []A2;
  242. 	delete []A3;
  243.     delete []B1;
  244. 	delete []B2;
  245. 	delete []B3;
  246. 	delete []x1;
  247. 	delete []x2;
  249. }
  251. int LSEllipse::SVD(float *a,int m,int n,float b[],float x[],float esp)
  252. {  
  253. 	float *aa;
  254. 	float *u;
  255. 	float *v;
  256.     aa=new float[n*m];
  257. 	u=new  float[m*m];
  258.     v=new  float[n*n];
  260.    int ka;
  261.    int  flag;
  262.    if(m>n)
  263.    { 
  264. 	ka=m+1;
  265.    }else
  266.    {
  267. 	   ka=n+1;
  268.    }
  270.    flag=gmiv(a,m,n,b,x,aa,esp,u,v,ka);
  274. 	delete []aa;
  275. 	delete []u;
  276. 	delete []v;
  278. 	return(flag);
  279. }
  285. int LSEllipse::gmiv( float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka)  
  286. { 
  287. 	int i,j;
  288.     i=ginv(a,m,n,aa,eps,u,v,ka);
  290.     if (i<0) return(-1);
  291.     for (i=0; i<=n-1; i++)
  292.       { x[i]=0.0;
  293.         for (j=0; j<=m-1; j++)
  294.           x[i]=x[i]+aa[i*m+j]*b[j];
  295.       }
  296.     return(1);
  297.   }
  300. int LSEllipse::ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka)
  301.   { 
  303.  //  int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);
  305. 	int i,j,k,l,t,p,q,f;
  306.     i=muav(a,m,n,u,v,eps,ka);
  307.     if (i<0) return(-1);
  308.     j=n;
  309.     if (m<n) j=m;
  310.     j=j-1;
  311.     k=0;
  312.     while ((k<=j)&&(a[k*n+k]!=0.0)) k=k+1;
  313.     k=k-1;
  314.     for (i=0; i<=n-1; i++)
  315.     for (j=0; j<=m-1; j++)
  316.       { t=i*m+j; aa[t]=0.0;
  317.         for (l=0; l<=k; l++)
  318.           { f=l*n+i; p=j*m+l; q=l*n+l;
  319.             aa[t]=aa[t]+v[f]*u[p]/a[q];
  320.           }
  321.       }
  322.     return(1);
  323.   }
  330. int LSEllipse::muav(float a[],int m,int n,float u[],float v[],float eps,int ka)
  331.   { int i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks;
  332.     float d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];
  333.     float *s,*e,*w;
  334.     //void ppp();
  335.    // void sss();
  336.      void ppp(float a[],float e[],float s[],float v[],int m,int n);
  337.      void sss(float fg[],float cs[]);
  339. 	s=(float *) malloc(ka*sizeof(float));
  340.     e=(float *) malloc(ka*sizeof(float));
  341.     w=(float *) malloc(ka*sizeof(float));
  342.     it=60; k=n;
  343.     if (m-1<n) k=m-1;
  344.     l=m;
  345.     if (n-2<m) l=n-2;
  346.     if (l<0) l=0;
  347.     ll=k;
  348.     if (l>k) ll=l;
  349.     if (ll>=1)
  350.       { for (kk=1; kk<=ll; kk++)
  351.           { if (kk<=k)
  352.               { d=0.0;
  353.                 for (i=kk; i<=m; i++)
  354.                   { ix=(i-1)*n+kk-1; d=d+a[ix]*a[ix];}
  355.                 s[kk-1]=(float)sqrt(d);
  356.                 if (s[kk-1]!=0.0)
  357.                   { ix=(kk-1)*n+kk-1;
  358.                     if (a[ix]!=0.0)
  359.                       { s[kk-1]=(float)fabs(s[kk-1]);
  360.                         if (a[ix]<0.0) s[kk-1]=-s[kk-1];
  361.                       }
  362.                     for (i=kk; i<=m; i++)
  363.                       { iy=(i-1)*n+kk-1;
  364.                         a[iy]=a[iy]/s[kk-1];
  365.                       }
  366.                     a[ix]=1.0f+a[ix];
  367.                   }
  368.                 s[kk-1]=-s[kk-1];
  369.               }
  370.             if (n>=kk+1)
  371.               { for (j=kk+1; j<=n; j++)
  372.                   { if ((kk<=k)&&(s[kk-1]!=0.0))
  373.                       { d=0.0;
  374.                         for (i=kk; i<=m; i++)
  375.                           { ix=(i-1)*n+kk-1;
  376.                             iy=(i-1)*n+j-1;
  377.                             d=d+a[ix]*a[iy];
  378.                           }
  379.                         d=-d/a[(kk-1)*n+kk-1];
  380.                         for (i=kk; i<=m; i++)
  381.                           { ix=(i-1)*n+j-1;
  382.                             iy=(i-1)*n+kk-1;
  383.                             a[ix]=a[ix]+d*a[iy];
  384.                           }
  385.                       }
  386.                     e[j-1]=a[(kk-1)*n+j-1];
  387.                   }
  388.               }
  389.             if (kk<=k)
  390.               { for (i=kk; i<=m; i++)
  391.                   { ix=(i-1)*m+kk-1; iy=(i-1)*n+kk-1;
  392.                     u[ix]=a[iy];
  393.                   }
  394.               }
  395.             if (kk<=l)
  396.               { d=0.0;
  397.                 for (i=kk+1; i<=n; i++)
  398.                   d=d+e[i-1]*e[i-1];
  399.                 e[kk-1]=(float)sqrt(d);
  400.                 if (e[kk-1]!=0.0)
  401.                   { if (e[kk]!=0.0)
  402.                       { e[kk-1]=(float)fabs(e[kk-1]);
  403.                         if (e[kk]<0.0) e[kk-1]=-e[kk-1];
  404.                       }
  405.                     for (i=kk+1; i<=n; i++)
  406.                       e[i-1]=e[i-1]/e[kk-1];
  407.                     e[kk]=1.0f+e[kk];
  408.                   }
  409.                 e[kk-1]=-e[kk-1];
  410.                 if ((kk+1<=m)&&(e[kk-1]!=0.0))
  411.                   { for (i=kk+1; i<=m; i++) w[i-1]=0.0;
  412.                     for (j=kk+1; j<=n; j++)
  413.                       for (i=kk+1; i<=m; i++)
  414.                         w[i-1]=w[i-1]+e[j-1]*a[(i-1)*n+j-1];
  415.                     for (j=kk+1; j<=n; j++)
  416.                       for (i=kk+1; i<=m; i++)
  417.                         { ix=(i-1)*n+j-1;
  418.                           a[ix]=a[ix]-w[i-1]*e[j-1]/e[kk];
  419.                         }
  420.                   }
  421.                 for (i=kk+1; i<=n; i++)
  422.                   v[(i-1)*n+kk-1]=e[i-1];
  423.               }
  424.           }
  425.       }
  426.     mm=n;
  427.     if (m+1<n) mm=m+1;
  428.     if (k<n) s[k]=a[k*n+k];
  429.     if (m<mm) s[mm-1]=0.0;
  430.     if (l+1<mm) e[l]=a[l*n+mm-1];
  431.     e[mm-1]=0.0;
  432.     nn=m;
  433.     if (m>n) nn=n;
  434.     if (nn>=k+1)
  435.       { for (j=k+1; j<=nn; j++)
  436.           { for (i=1; i<=m; i++)
  437.               u[(i-1)*m+j-1]=0.0;
  438.             u[(j-1)*m+j-1]=1.0;
  439.           }
  440.       }
  441.     if (k>=1)
  442.       { for (ll=1; ll<=k; ll++)
  443.           { kk=k-ll+1; iz=(kk-1)*m+kk-1;
  444.             if (s[kk-1]!=0.0)
  445.               { if (nn>=kk+1)
  446.                   for (j=kk+1; j<=nn; j++)
  447.                     { d=0.0;
  448.                       for (i=kk; i<=m; i++)
  449.                         { ix=(i-1)*m+kk-1;
  450.                           iy=(i-1)*m+j-1;
  451.                           d=d+u[ix]*u[iy]/u[iz];
  452.                         }
  453.                       d=-d;
  454.                       for (i=kk; i<=m; i++)
  455.                         { ix=(i-1)*m+j-1;
  456.                           iy=(i-1)*m+kk-1;
  457.                           u[ix]=u[ix]+d*u[iy];
  458.                         }
  459.                     }
  460.                   for (i=kk; i<=m; i++)
  461.                     { ix=(i-1)*m+kk-1; u[ix]=-u[ix];}
  462.                   u[iz]=1.0f+u[iz];
  463.                   if (kk-1>=1)
  464.                     for (i=1; i<=kk-1; i++)
  465.                       u[(i-1)*m+kk-1]=0.0;
  466.               }
  467.             else
  468.               { for (i=1; i<=m; i++)
  469.                   u[(i-1)*m+kk-1]=0.0;
  470.                 u[(kk-1)*m+kk-1]=1.0;
  471.               }
  472.           }
  473.       }
  474.     for (ll=1; ll<=n; ll++)
  475.       { kk=n-ll+1; iz=kk*n+kk-1;
  476.         if ((kk<=l)&&(e[kk-1]!=0.0))
  477.           { for (j=kk+1; j<=n; j++)
  478.               { d=0.0;
  479.                 for (i=kk+1; i<=n; i++)
  480.                   { ix=(i-1)*n+kk-1; iy=(i-1)*n+j-1;
  481.                     d=d+v[ix]*v[iy]/v[iz];
  482.                   }
  483.                 d=-d;
  484.                 for (i=kk+1; i<=n; i++)
  485.                   { ix=(i-1)*n+j-1; iy=(i-1)*n+kk-1;
  486.                     v[ix]=v[ix]+d*v[iy];
  487.                   }
  488.               }
  489.           }
  490.         for (i=1; i<=n; i++)
  491.           v[(i-1)*n+kk-1]=0.0;
  492.         v[iz-n]=1.0;
  493.       }
  494.     for (i=1; i<=m; i++)
  495.     for (j=1; j<=n; j++)
  496.       a[(i-1)*n+j-1]=0.0;
  497.     m1=mm; it=60;
  498.     while (1==1)
  499.       { if (mm==0)
  500.           { ppp(a,e,s,v,m,n);
  501.             free(s); free(e); free(w); return(1);
  502.           }
  503.         if (it==0)
  504.           { ppp(a,e,s,v,m,n);
  505.             free(s); free(e); free(w); return(-1);
  506.           }
  507.         kk=mm-1;
  508. 	while ((kk!=0)&&(fabs(e[kk-1])!=0.0))
  509.           { d=(float)(fabs(s[kk-1])+fabs(s[kk]));
  510.             dd=(float)fabs(e[kk-1]);
  511.             if (dd>eps*d) kk=kk-1;
  512.             else e[kk-1]=0.0;
  513.           }
  514.         if (kk==mm-1)
  515.           { kk=kk+1;
  516.             if (s[kk-1]<0.0)
  517.               { s[kk-1]=-s[kk-1];
  518.                 for (i=1; i<=n; i++)
  519.                   { ix=(i-1)*n+kk-1; v[ix]=-v[ix];}
  520.               }
  521.             while ((kk!=m1)&&(s[kk-1]<s[kk]))
  522.               { d=s[kk-1]; s[kk-1]=s[kk]; s[kk]=d;
  523.                 if (kk<n)
  524.                   for (i=1; i<=n; i++)
  525.                     { ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;
  526.                       d=v[ix]; v[ix]=v[iy]; v[iy]=d;
  527.                     }
  528.                 if (kk<m)
  529.                   for (i=1; i<=m; i++)
  530.                     { ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;
  531.                       d=u[ix]; u[ix]=u[iy]; u[iy]=d;
  532.                     }
  533.                 kk=kk+1;
  534.               }
  535.             it=60;
  536.             mm=mm-1;
  537.           }
  538.         else
  539.           { ks=mm;
  540.             while ((ks>kk)&&(fabs(s[ks-1])!=0.0))
  541.               { d=0.0;
  542.                 if (ks!=mm) d=d+(float)fabs(e[ks-1]);
  543.                 if (ks!=kk+1) d=d+(float)fabs(e[ks-2]);
  544.                 dd=(float)fabs(s[ks-1]);
  545.                 if (dd>eps*d) ks=ks-1;
  546.                 else s[ks-1]=0.0;
  547.               }
  548.             if (ks==kk)
  549.               { kk=kk+1;
  550.                 d=(float)fabs(s[mm-1]);
  551.                 t=(float)fabs(s[mm-2]);
  552.                 if (t>d) d=t;
  553.                 t=(float)fabs(e[mm-2]);
  554.                 if (t>d) d=t;
  555.                 t=(float)fabs(s[kk-1]);
  556.                 if (t>d) d=t;
  557.                 t=(float)fabs(e[kk-1]);
  558.                 if (t>d) d=t;
  559.                 sm=s[mm-1]/d; sm1=s[mm-2]/d;
  560.                 em1=e[mm-2]/d;
  561.                 sk=s[kk-1]/d; ek=e[kk-1]/d;
  562.                 b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0f;
  563.                 c=sm*em1; c=c*c; shh=0.0;
  564.                 if ((b!=0.0)||(c!=0.0))
  565.                   { shh=(float)sqrt(b*b+c);
  566.                     if (b<0.0) shh=-shh;
  567.                     shh=c/(b+shh);
  568.                   }
  569.                 fg[0]=(sk+sm)*(sk-sm)-shh;
  570.                 fg[1]=sk*ek;
  571.                 for (i=kk; i<=mm-1; i++)
  572.                   { sss(fg,cs);
  573.                     if (i!=kk) e[i-2]=fg[0];
  574.                     fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
  575.                     e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
  576.                     fg[1]=cs[1]*s[i];
  577.                     s[i]=cs[0]*s[i];
  578.                     if ((cs[0]!=1.0)||(cs[1]!=0.0))
  579.                       for (j=1; j<=n; j++)
  580.                         { ix=(j-1)*n+i-1;
  581.                           iy=(j-1)*n+i;
  582.                           d=cs[0]*v[ix]+cs[1]*v[iy];
  583.                           v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
  584.                           v[ix]=d;
  585.                         }
  586.                     sss(fg,cs);
  587.                     s[i-1]=fg[0];
  588.                     fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
  589.                     s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
  590.                     fg[1]=cs[1]*e[i];
  591.                     e[i]=cs[0]*e[i];
  592.                     if (i<m)
  593.                       if ((cs[0]!=1.0)||(cs[1]!=0.0))
  594.                         for (j=1; j<=m; j++)
  595.                           { ix=(j-1)*m+i-1;
  596.                             iy=(j-1)*m+i;
  597.                             d=cs[0]*u[ix]+cs[1]*u[iy];
  598.                             u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
  599.                             u[ix]=d;
  600.                           }
  601.                   }
  602.                 e[mm-2]=fg[0];
  603.                 it=it-1;
  604.               }
  605.             else
  606.               { if (ks==mm)
  607.                   { kk=kk+1;
  608.                     fg[1]=e[mm-2]; e[mm-2]=0.0;
  609.                     for (ll=kk; ll<=mm-1; ll++)
  610.                       { i=mm+kk-ll-1;
  611.                         fg[0]=s[i-1];
  612.                         sss(fg,cs);
  613.                         s[i-1]=fg[0];
  614.                         if (i!=kk)
  615.                           { fg[1]=-cs[1]*e[i-2];
  616.                             e[i-2]=cs[0]*e[i-2];
  617.                           }
  618.                         if ((cs[0]!=1.0)||(cs[1]!=0.0))
  619.                           for (j=1; j<=n; j++)
  620.                             { ix=(j-1)*n+i-1;
  621.                               iy=(j-1)*n+mm-1;
  622.                               d=cs[0]*v[ix]+cs[1]*v[iy];
  623.                               v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
  624.                               v[ix]=d;
  625.                             }
  626.                       }
  627.                   }
  628.                 else
  629.                   { kk=ks+1;
  630.                     fg[1]=e[kk-2];
  631.                     e[kk-2]=0.0;
  632.                     for (i=kk; i<=mm; i++)
  633.                       { fg[0]=s[i-1];
  634.                         sss(fg,cs);
  635.                         s[i-1]=fg[0];
  636.                         fg[1]=-cs[1]*e[i-1];
  637.                         e[i-1]=cs[0]*e[i-1];
  638.                         if ((cs[0]!=1.0)||(cs[1]!=0.0))
  639.                           for (j=1; j<=m; j++)
  640.                             { ix=(j-1)*m+i-1;
  641.                               iy=(j-1)*m+kk-2;
  642.                               d=cs[0]*u[ix]+cs[1]*u[iy];
  643.                               u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
  644.                               u[ix]=d;
  645.                             }
  646.                       }
  647.                   }
  648.               }
  649.           }
  650.       }
  652.    	free(s);free(e);free(w); 
  653. 	  return(1);
  656.   }
  659. void ppp(float a[],float e[],float s[],float v[],int m,int n) 
  660. { int i,j,p,q;
  661.     float d;
  662.     if (m>=n) i=n;
  663.     else i=m;
  664.     for (j=1; j<=i-1; j++)
  665.       { a[(j-1)*n+j-1]=s[j-1];
  666.         a[(j-1)*n+j]=e[j-1];
  667.       }
  668.     a[(i-1)*n+i-1]=s[i-1];
  669.     if (m<n) a[(i-1)*n+i]=e[i-1];
  670.     for (i=1; i<=n-1; i++)
  671.     for (j=i+1; j<=n; j++)
  672.       { p=(i-1)*n+j-1; q=(j-1)*n+i-1;
  673.         d=v[p]; v[p]=v[q]; v[q]=d;
  674.       }
  675.     return;
  676.   }
  679.   void sss(float fg[],float cs[])
  680.  { float r,d;
  681.     if ((fabs(fg[0])+fabs(fg[1]))==0.0)
  682.       { cs[0]=1.0; cs[1]=0.0; d=0.0;}
  683.     else 
  684.       { d=(float)sqrt(fg[0]*fg[0]+fg[1]*fg[1]);
  685.         if (fabs(fg[0])>fabs(fg[1]))
  686.           { d=(float)fabs(d);
  687.             if (fg[0]<0.0) d=-d;
  688.           }
  689.         if (fabs(fg[1])>=fabs(fg[0]))
  690.           { d=(float)fabs(d);
  691.             if (fg[1]<0.0) d=-d;
  692.           }
  693.         cs[0]=fg[0]/d; cs[1]=fg[1]/d;
  694.       }
  695.     r=1.0;
  696.     if (fabs(fg[0])>fabs(fg[1])) r=cs[1];
  697.     else
  698.       if (cs[0]!=0.0) r=1.0f/cs[0];
  699.     fg[0]=d; fg[1]=r;
  700.     return;
  701.   }

参考:

线性，非线性多项式：

http://blog.csdn.net/qll125596718/article/details/8248249

http://blog.csdn.net/poxiaozhuimeng/article/details/41117947

http://blog.csdn.net/ouyangying123/article/details/53996403

http://blog.csdn.net/jairuschan/article/details/7517773/

http://blog.csdn.net/zang141588761/article/details/50523036

http://www.cnblogs.com/gnuhpc/archive/2012/12/09/2809997.html

http://download.csdn.net/download/biaobiao11/9755119

圆拟合：

http://blog.sina.com.cn/s/blog_b27f71160101gxun.html  

http://www.cnblogs.com/dotLive/archive/2007/04/06/524633.html

http://blog.csdn.net/andylao62/article/details/24522365

http://blog.csdn.net/liyuanbhu/article/details/50889951

http://blog.csdn.net/liyuanbhu/article/details/50890587

椭圆拟合：

http://doc./u013708970/archive/121532.html

    本站是提供个人知识管理的网络存储空间，所有内容均由用户发布，不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息，谨防诈骗。如发现有害或侵权内容，请点击一键举报。
    转藏 分享 献花（0）

    来自： 战神之家 > 《待分类》

    举报/认领

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多