已知空间N点坐标求圆心坐标，半径

时间：2023-01-25 09:43:35

注意哦 这里是求圆心 不是球心哦

条件：已知空间N点坐标，格式如下 求圆心坐标，半径

-33.386698 -12.312448 -2301.396442
-33.668120 -12.571431 -2300.390996
-33.838611 -12.774933 -2299.691688
-34.079235 -13.616660 -2298.326277
-34.254998 -13.441280 -2298.192657
-34.356542 -13.755525 -2297.796473

........................................................

实现方法：用最小二乘法拟合出 球面方程 和 平面方程，两者相交即为所求圆曲线

球面方程：(x - a)^2  + (y - b)^2 + (z - c)^2 = R^2

展开：     x^2 + y^2 + z ^2 + a^2 +b^2 +c^2 - 2ax - 2by -2cz = R ^2

令 A = 2a  ; B =2b ; C =2c  D=a^2 +b^2 +c^2 -R^2 

 得  x^2 + y^2 + z ^2 - Ax -By - Cz + D =0

即：Ax +By +Cz -D = x^2 +y^2 +z^2

用矩阵表示这N组数据如下形式

已知空间N点坐标求圆心坐标，半径

现在就是求 A B C D 的问题了

具体步骤如下

已知空间N点坐标求圆心坐标，半径

 

平面方程 A' x + B' y + C' z + D' = 0

可化为  A x + B y + C z  -1 =0

用矩阵表示如下

已知空间N点坐标求圆心坐标，半径

 

同样可以求出 A B C

 

这样就有了球面方程 球心坐标 球半径 和 平面方程了

如图所示（网络找的图，意思着看 囧）

 

已知空间N点坐标求圆心坐标，半径

圆心为球心到平面的垂足（也就是 平面外一点到平面的坐标问题）

半径为 sqrt(球半径^2 - 球心到平面的距离^2)

 

设 平面方程为 A X + B Y + C Z +D = 0   ①    球心坐标(a,b,c) 

平面外一点到平面的坐标问题：

则 平面的法向量为 （A ,B ,C ）

设垂足即圆心 (x' y' z')   球心到圆心的连线 与法向量是平行的

可以得到如下 (x' -a )/A = (y' -b)/B = (z' - c)/C  = t ②

由 ②得 x' = At + a; y' = B t + b ;  z' = C t + c;

带入① 可以得到(x' , y' , z')

平面外一点到平面的距离问题

d = abs(Ax' + By' + C z' +D) / sqrt(A^2 + B^2 +C^2)

 

到此为止已经有了足够的理论知识，下面是代码 分别用 OPENCV 和C 实现

 

  1 // 最小二乘法拟合球.cpp : 定义控制台应用程序的入口点。
  2 //
  3 
  4 #include "stdafx.h"
  5 #include <iostream>
  6 #include <fstream>
  7 #include  <cmath>   
  8 usingnamespace std; 
  9 
 10 #include <cv.h>
 11 #include <cxcore.h>
 12 #include <highgui.h>
 13 
 14 #ifdef DEBUG
 15 #pragma comment(lib,"cxcore200d.lib")
 16 #pragma comment(lib,"cv200d.lib")
 17 #pragma comment(lib,"highgui200d.lib")
 18 #else
 19 #pragma comment(lib,"cxcore200.lib")
 20 #pragma comment(lib,"cv200.lib")
 21 #pragma comment(lib,"highgui200.lib")
 22 
 23 #endif
 24 
 25 void fitRound(char*filepath,int n)
 26 {
 27     ifstream file(filepath);
 28 //int n = 0;    //数据组数
 29 
 30 //file>>n;
 31 /*
 32     x ~ NX4    
 33     | x1 y1 z1 -1 |
 34     |. .. ....  .......... |
 35     | .. ......  ..........|
 36     |xn yn  zn -1|
 37 */
 38 int xsize =4*n*sizeof(double);
 39 double*x = (double*)malloc(xsize);
 40 /*
 41         y ~ NX1
 42         |x1^2 + y1^2 +z1^2 |
 43         |.......................................|
 44         |.......................................|
 45         |xn^2+yn^2+zn^2  |
 46 */
 47 int ysize = n *sizeof(double);
 48 double*y = (double*)malloc(ysize);
 49 
 50 /*
 51         z ~NX3
 52         |x1 y1 z1|
 53         |...............|
 54         |...............|
 55         |xn yn zn|
 56 */
 57 int zsize =3* n *sizeof(double);
 58 double*z = (double*)malloc(zsize);
 59 
 60 /*
 61         p  ~ Nx1
 62         | 1 |
 63         | .. |
 64         | .. |
 65         | 1 |
 66 */
 67 int psize = n *sizeof(double);
 68 double*p = (double*)malloc(psize);
 69 
 70 for (int i=0;i<n;i++)
 71     {
 72         file>>*(x+i*4+0)>>*( x+i*4+1)>>*(x +i*4+2);
 73 *(x+i*4+3) =-1;
 74 
 75 *(y +i) =*(x+i*4+0) **(x+i*4+0) +*(x+i*4+1) **(x+i*4+1) +*(x+i*4+2) **(x+i*4+2);
 76 
 77 *(z+i*3+0) =*(x+i*4+0);
 78 *(z+i*3+1) =*(x+i*4+1);
 79 *(z+i*3+2) =*(x+i*4+2);
 80 
 81 *(p+i) =1;
 82     }
 83     
 84 // ---------------------------- 球面方程
 85 //x^2 +y^2 + z^2 - AX - BY - CZ +D  =0
 86 
 87 // x 对应的矩阵
 88     CvMat * MAT_X = cvCreateMat(n,4,CV_64FC1);
 89     memcpy(MAT_X->data.db,x,xsize);
 90 
 91 // y 对应的矩阵
 92     CvMat *MAT_Y = cvCreateMat(n,1,CV_64FC1);
 93     memcpy(MAT_Y->data.db,y,ysize);
 94 
 95 //xT -- x的转置矩阵
 96     CvMat *MAT_XT = cvCreateMat(4,n,CV_64FC1);
 97     cvTranspose(MAT_X,MAT_XT);
 98 
 99 // xT (4xn)  *  x(n*4) = XT_X(4*4)
100     CvMat *MAT_XT_X = cvCreateMat(4,4,CV_64FC1);
101     cvMatMul(MAT_XT,MAT_X,MAT_XT_X);
102 
103 //xT (4xn)  *  y(n*1) = xT_Y(4*1)
104     CvMat *MAT_XT_Y = cvCreateMat(4,1,CV_64FC1);
105     cvMatMul(MAT_XT,MAT_Y,MAT_XT_Y);
106 
107 //MAT_XT_X_INVERT -- MAT_XT_X的逆矩阵
108     CvMat *MAT_XT_X_INVERT = cvCreateMat(4,4,CV_64FC1);
109     cvInvert(MAT_XT_X,MAT_XT_X_INVERT,CV_LU); // 高斯消去法
110 
111 //MAT_ABCD 4X1结果矩阵
112     CvMat *MAT_ABCD = cvCreateMat(4,1,CV_64FC1);
113     cvMatMul(MAT_XT_X_INVERT,MAT_XT_Y,MAT_ABCD);
114 
115 double c_x,c_y,c_z,c_r;
116 double*ptr = MAT_ABCD->data.db;
117     c_x =*ptr /2;
118     c_y =*(ptr+1)/2;
119     c_z =*(ptr+2)/2;
120     c_r =sqrt (c_x *c_x +c_y *c_y +c_z *c_z -*(ptr+3));
121 
122 
123 //-----------------------平面方程
124 //E X + F y +G z  =1
125 
126 // z 对应矩阵
127     CvMat * MAT_Z = cvCreateMat(n,3,CV_64FC1);
128     memcpy(MAT_Z->data.db,z,zsize);
129 
130 // p 对应矩阵
131     CvMat *MAT_P = cvCreateMat(n,1,CV_64FC1);
132     memcpy(MAT_P->data.db,p,psize);
133 
134 //z 的转置矩阵
135     CvMat *MAT_ZT = cvCreateMat(3,n,CV_64FC1);
136     cvTranspose(MAT_Z,MAT_ZT);
137 
138 //zt * z
139     CvMat *MAT_ZT_Z = cvCreateMat(3,3,CV_64FC1);
140     cvMatMul(MAT_ZT,MAT_Z,MAT_ZT_Z);
141 
142 //ZT * P 
143     CvMat * MAT_ZT_P = cvCreateMat(3,1,CV_64FC1);
144     cvMatMul(MAT_ZT,MAT_P,MAT_ZT_P);
145 
146 // MAT_ZT_Z的逆矩阵 
147     CvMat *MAT_ZT_Z_INVERT = cvCreateMat(3,3,CV_64FC1);
148     cvInvert(MAT_ZT_Z,MAT_ZT_Z_INVERT,CV_LU);
149 
150 //MAT_EFG 3X1结果
151     CvMat *MAT_EFG = cvCreateMat(3,1,CV_64FC1);
152     cvMatMul(MAT_ZT_Z_INVERT,MAT_ZT_P,MAT_EFG);
153     
154 double E,F,G;
155     E =* MAT_EFG->data.db;
156     F =*(MAT_EFG->data.db +1);
157     G =*(MAT_EFG->data.db +2 );
158 //圆心坐标 半径
159 double n_x, n_y, n_z, n_r;
160　　 n_x = ((F*F+G*G)*c_x +E *(-F*c_y - G*c_z +1))/ ( E *E + F *F+ G *G);
　　　　 n_y = ((E*E+G*G)*c_y +F* (-E*c_x -G*c_z +1))/ ( E *E + F *F+ G *G);
　　　　 n_z = ((E*E+F*F)*c_z +G*(-E*c_x -F *c_y +1))/( E *E + F *F+ G *G);
162 
163 
164     n_r = sqrt( c_r *c_r - (E *c_x +F *c_y +G *c_z -1 ) *(E *c_x +F *c_y +G *c_z -1 ) /(E *E +F * F +G *G) );
165 
166     printf("%f %f %f %f\n",n_x,n_y,n_z,n_r);
167     file.close();
168 }
169 
170 int _tmain(int argc, _TCHAR* argv[])
171 {
172     
173 
174 for (int i =4 ; i<35;i++)
175     {
176         fitRound("log.txt",i);
177     }
178     getchar();
179 return0;
180 }

 

 

C 实现方式

// 最小二乘法求圆心CC++.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include <iostream>
#include <fstream>
#include  <cmath>   
using namespace std; 

int InverseMatrix(double *matrix,const int &row);
void swap(double &a,double &b);
int mulMatrix(double *matrixA,int m1,int n1,double *matrixB,int m2,int n2,double *matrixC);
int transPoseMatrix(double *matrixA,int m,int n,double *matrixB);
void fitRoud(char * filepath,int n);

int _tmain(int argc, _TCHAR* argv[])
{
	for (int i=4 ;i<35;i++)
	{
		fitRoud("log.txt",i);
	}
	getchar();

	return 0;
}


void swap(double &a,double &b)
{
	double temp=a;
	a=b;
	b=temp;
}

/**********************************************
*函数名：InverseMatrix       
*函数介绍：求逆矩阵（高斯—约当法） 
*输入参数：矩阵首地址（二维数组）matrix，阶数row
*输出参数：matrix原矩阵的逆矩阵
*返回值：成功，0；失败，1
*调用函数：swap(double &a,double &b)
**********************************************/
int InverseMatrix(double *matrix,const int &row)
{
	double *m=new double[row*row];
	double *ptemp,*pt=m;

	int i,j;

	ptemp=matrix;
	for (i=0;i<row;i++)
	{
		for (j=0;j<row;j++)
		{
			*pt=*ptemp;
			ptemp++;
			pt++;
		}
	}

	int k;

	int *is=new int[row],*js=new int[row];

	for (k=0;k<row;k++)
	{
		double max=0;
		//全选主元
		//寻找最大元素
		for (i=k;i<row;i++)
		{
			for (j=k;j<row;j++)
			{
				if (fabs(*(m+i*row+j))>max)
				{
					max=*(m+i*row+j);
					is[k]=i;
					js[k]=j;
				}
			}
		}

		if (0 == max)
		{
			return 1;
		}

		//行交换
		if (is[k]!=k)
		{
			for (i=0;i<row;i++)
			{
				swap(*(m+k*row+i),*(m+is[k]*row+i));
			}
		}

		//列交换
		if (js[k]!=k)
		{
			for (i=0;i<row;i++)
			{
				swap(*(m+i*row+k),*(m+i*row+js[k]));
			}
		}

		*(m+k*row+k)=1/(*(m+k*row+k));

		for (j=0;j<row;j++)
		{
			if (j!=k)
			{
				*(m+k*row+j)*=*((m+k*row+k));
			}
		}

		for (i=0;i<row;i++)
		{
			if (i!=k)
			{
				for (j=0;j<row;j++)
				{
					if(j!=k)
					{
						*(m+i*row+j)-=*(m+i*row+k)**(m+k*row+j);
					}
				}
			}
		}

		for (i=0;i<row;i++)
		{
			if(i!=k)
			{
				*(m+i*row+k)*=-(*(m+k*row+k));
			}
		}
	}

	int r;
	//恢复
	for (r=row-1;r>=0;r--)
	{
		if (js[r]!=r)
		{
			for (j=0;j<row;j++)
			{
				swap(*(m+r*row+j),*(m+js[r]*row+j));
			}
		}
		if (is[r]!=r)
		{
			for (i=0;i<row;i++)
			{
				swap(*(m+i*row+r),*(m+i*row+is[r]));
			}
		}
	}

	ptemp=matrix;
	pt=m;
	for (i=0;i<row;i++)
	{
		for (j=0;j<row;j++)
		{
			*ptemp=*pt;
			ptemp++;
			pt++;
		}
	}
	delete []is;
	delete []js;
	delete []m;

	return 0;
}

/*
*函数名：mulMatrix       
*函数介绍：矩阵相乘
*输入参数 :matrixA ----矩阵首地址
				m1,n1	-----	matrixA 行列数
				matrixB -----矩阵首地址
				m2,n2 ------matrixB 行列数
*				matrixC 结果矩阵 行列数 m1 n2 
*返回值 ： 0 -- 失败 
				1 -- 成功

*/
int mulMatrix(double *matrixA,int m1,int n1,double *matrixB,int m2,int n2,double *matrixC)
{
	/*
	if( n1 !=m2 )
		return 0;
	if( sizeof(matrixC) ! = m1 * n2 * sizeof(double) )
		return 0;
*/
	for (int i=0;i<m1;++i)
	{
		for (int j=0;j<n2;++j)
		{
			*(matrixC + i *n2 +j) =0.0;
			// matrixA - i 行  *  matrixB -- j 列
			for (int k = 0;k<m2;k++)
			{
				*(matrixC + i *n2 +j) +=*(matrixA + i*n1 + k) * *(matrixB +k * n2 + j);
			}
		}
	}
	return 1;
}

/*
*函数名：transPoseMatrix       
*函数介绍：矩阵转置
*输入参数 :matrixA ----源矩阵
				m1,n1	-----	matrixA 行列数
				matrixB -----转置结果
*返回值 ： 0 -- 失败 
				1 -- 成功

*/
int transPoseMatrix(double *matrixA,int m,int n,double *matrixB)
{
	for (int i=0;i<m;++i)
	{
		for (int j=0;j<n;++j)
		{
			*(matrixB + j *m +i) = *(matrixA + i * n + j);
		}
	}
	return 1;
}

void fitRoud(char * filepath,int n)
{
	ifstream file(filepath);
	//int n = 0;    //数据组数

	//file>>n;
	/*
	x ~ NX4	
	| x1 y1 z1 -1 |
	|. .. ....  .......... |
	| .. ......  ..........|
	|xn yn  zn -1|
	*/
	int xsize = 4*n*sizeof(double);
	double *x = (double *)malloc(xsize);
	/*
		y ~ NX1
		|x1^2 + y1^2 +z1^2 |
		|.......................................|
		|.......................................|
		|xn^2+yn^2+zn^2  |
	*/
	int ysize = n * sizeof(double);
	double *y = (double *)malloc(ysize);

	/*
		z ~NX3
		|x1 y1 z1|
		|...............|
		|...............|
		|xn yn zn|
	*/
	int zsize = 3 * n * sizeof(double);
	double *z = (double *)malloc(zsize);

	/*
		p  ~ Nx1
		| 1 |
		| .. |
		| .. |
		| 1 |
	*/
	int psize = n * sizeof(double);
	double *p = (double *)malloc(psize);

	for (int i=0;i<n;i++)
	{
		file>>*(x+i*4+0)>>*( x+i*4+1)>>*(x +i*4 +2);
		*(x+i*4+3) = -1;

		*(y +i) = *(x+i*4+0) * *(x+i*4+0) +*(x+i*4+1) * *(x+i*4+1) + *(x+i*4+2) * *(x+i*4+2);

		*(z+i*3+0) = *(x+i*4+0);
		*(z+i*3+1) =  *(x+i*4+1);
		*(z+i*3+2) =  *(x+i*4+2);

		*(p+i) = 1;
	}

	file.close();
	// ---------------------------- 球面方程
	//x^2 +y^2 + z^2 - AX - BY - CZ +D  =0

	// x 对应的矩阵
	double * MAT_X = (double *)malloc(n * 4 * sizeof(double));
	memcpy(MAT_X,x,xsize);

	// y 对应的矩阵
	double *MAT_Y = (double *)malloc(n * 1 *sizeof(double));
	memcpy(MAT_Y,y,ysize);

	//xT -- x的转置矩阵
	double *MAT_XT =(double *)malloc(4 * n * sizeof(double));
	transPoseMatrix(MAT_X,n,4,MAT_XT);

	// xT (4xn)  *  x(n*4) = XT_X(4*4)
	double *MAT_XT_X = (double *)malloc(4 * 4 * sizeof(double));
	mulMatrix(MAT_XT,4,n,MAT_X,n,4,MAT_XT_X);

	//xT (4xn)  *  y(n*1) = xT_Y(4*1)
	double *MAT_XT_Y = (double *)malloc(4 * 1 * sizeof(double ));
	mulMatrix(MAT_XT,4,n,MAT_Y,n,1,MAT_XT_Y);


	//MAT_XT_X_INVERT -- MAT_XT_X的逆矩阵
	double *MAT_XT_X_INVERT = NULL;
	InverseMatrix(MAT_XT_X,4);
	MAT_XT_X_INVERT = MAT_XT_X;

	//MAT_ABCD 4X1结果矩阵
	double *MAT_ABCD = (double *)malloc(4 * 1 * sizeof(double));
	mulMatrix(MAT_XT_X_INVERT,4,4,MAT_XT_Y,4,1,MAT_ABCD);

	double c_x,c_y,c_z,c_r;
	double *ptr = MAT_ABCD;
	c_x = *ptr /2;
	c_y = *(ptr+1)/2;
	c_z = *(ptr+2)/2;
	c_r =sqrt (c_x *c_x +c_y *c_y +c_z *c_z -*(ptr+3));

	//-----------------------平面方程
	//E X + F y +G z  =1

	// z 对应矩阵
	double * MAT_Z = (double *)malloc(n * 3 * sizeof(double ));
	memcpy(MAT_Z,z,zsize);

	// p 对应矩阵
	double *MAT_P =(double *)malloc(n * 1 * sizeof(double));
	memcpy(MAT_P,p,psize);

	//z 的转置矩阵
	double *MAT_ZT = (double *)malloc(3 * n * sizeof(double));
	transPoseMatrix(MAT_Z,n,3,MAT_ZT);

	//zt * z
	double * MAT_ZT_Z = (double *)malloc(3 * 3 * sizeof(double));
	mulMatrix(MAT_ZT,3,n,MAT_Z,n,3,MAT_ZT_Z);

	//ZT * P 
	double * MAT_ZT_P =(double *)malloc( 3 * 1 * sizeof(double));
	mulMatrix(MAT_ZT,3,n,MAT_P,n,1,MAT_ZT_P);

	// MAT_ZT_Z的逆矩阵 
	double *MAT_ZT_Z_INVERT = (double *)malloc(3 * 3 * sizeof(double));
	InverseMatrix(MAT_ZT_Z,3);
	MAT_ZT_Z_INVERT = MAT_ZT_Z;


	//MAT_EFG 3X1
	double *MAT_EFG = (double *)malloc(3 * 1*sizeof(double));
	mulMatrix(MAT_ZT_Z_INVERT,3,3,MAT_ZT_P,3,1,MAT_EFG);

	double E,F,G;
	E =* MAT_EFG;
	F = *(MAT_EFG+1);
	G = *(MAT_EFG+2 );
	//圆心坐标 半径
	double n_x, n_y, n_z, n_r;
 　　　　n_x = ((F*F+G*G)*c_x +E *(-F*c_y - G*c_z +1))/ ( E *E + F *F+ G *G);
 　　　　n_y = ((E*E+G*G)*c_y +F* (-E*c_x -G*c_z +1))/ ( E *E + F *F+ G *G);
　　　　 n_z = ((E*E+F*F)*c_z +G*(-E*c_x -F *c_y +1))/( E *E + F *F+ G *G);
　　　　　n_r = sqrt( c_r *c_r - (E *c_x +F *c_y +G *c_z -1 ) *(E *c_x +F *c_y +G *c_z -1 ) /(E *E +F * F +G *G) );

	printf("%f %f %f %f\n",n_x,n_y,n_z,n_r);
	free(x);
	free(y);
	free(z);
	free(MAT_X);
	free(MAT_XT);
	free(MAT_XT_X);
	free(MAT_XT_Y);
	free(MAT_Y);
	free(MAT_Z);
	free(MAT_ZT);
	free(MAT_ZT_P);
	free(MAT_ZT_Z);
	free(MAT_P);
	free(MAT_ABCD);
	free(MAT_EFG);


}

 

 

标签：

相关文章