注意哦 这里是求圆心 不是球心哦
条件：已知空间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 =1125 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 * z139 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 %fn",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 %fn",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);
}
原文链接: https://www.cnblogs.com/xiaomaLV2/archive/2011/08/30/2159908.html
欢迎关注
微信关注下方公众号，第一时间获取干货硬货；公众号内回复【pdf】免费获取数百本计算机经典书籍
原创文章受到原创版权保护。转载请注明出处：https://www.ccppcoding.com/archives/31461
非原创文章文中已经注明原地址，如有侵权，联系删除
关注公众号【高性能架构探索】，第一时间获取最新文章
转载文章受原作者版权保护。转载请注明原作者出处！
