注意哦 这里是求圆心 不是球心哦
条件：已知空间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组数据如下形式
现在就是求 A B C D 的问题了
具体步骤如下
平面方程 A' x + B' y + C' z + D' = 0
可化为 A x + B y + C z -1 =0
用矩阵表示如下
同样可以求出 A B C
这样就有了球面方程 球心坐标 球半径 和 平面方程了
如图所示（网络找的图，意思着看 囧）
圆心为球心到平面的垂足（也就是 平面外一点到平面的坐标问题）
半径为 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);
}