参考:http://blog.sina.com.cn/s/blog_648868460100h2b8.html(这个博主还有很多关于工程测量的知识和空间几何计算的知识)
已知空间三点的坐标为(x1,y1,z1),(x2,y2,z2),(x3,y3,z3),求这三个点所确定的空间圆的圆心坐标和半径。
分析可得约束条件:1、三点共面2、三点到空间圆心坐标的距离相等。
从约束条件可得,4个*项4个方程可解,可以列出线性代数方程组,即可用消元法求解;
即以下的(1)(2)(3)(4)四个方程组成的线性代数方程组
共面约束:
三点到空间圆心坐标的距离相等约束:
(1) (2)(3)联解可得(5)(6)同时消去R
通过(4)(5)(6)获得关于圆心空间坐标的线性代数方程组
利用opencv2.1实现的代码如下:
CvScalar xyz1,CvScalar xyz2,CvScalar xyz3 分别为空间三点坐标
//从空间圆上三点获得所在圆的空间中心坐标
CvMat * GETIrisCenter(CvScalar xyz1,CvScalar xyz2,CvScalar xyz3)
{
double ABCD1[4];
double ABCD2[4];
double ABCD3[4];
ABCD1[0]=xyz1.val[1]*xyz2.val[2]-xyz1.val[1]*xyz3.val[2]-xyz1.val[2]*xyz2.val[1]+xyz1.val[2]*xyz3.val[1]+xyz2.val[1]*xyz3.val[2]-xyz3.val[1]*xyz2.val[2];
ABCD1[1]=-xyz1.val[0]*xyz2.val[2]+xyz1.val[0]*xyz3.val[2]+xyz1.val[2]*xyz2.val[0]-xyz1.val[2]*xyz3.val[0]-xyz2.val[0]*xyz3.val[2]+xyz3.val[0]*xyz2.val[2];
ABCD1[2]=xyz1.val[0]*xyz2.val[1]-xyz1.val[0]*xyz3.val[1]-xyz1.val[1]*xyz2.val[0]+xyz1.val[1]*xyz3.val[0]+xyz2.val[0]*xyz3.val[1]-xyz3.val[0]*xyz2.val[1];
ABCD1[3]=-xyz1.val[0]*xyz2.val[1]*xyz3.val[2]+xyz1.val[0]*xyz3.val[1]*xyz2.val[2]+xyz2.val[0]*xyz1.val[1]*xyz3.val[2]-xyz3.val[0]*xyz1.val[1]*xyz2.val[2]-xyz2.val[0]*xyz3.val[1]*xyz1.val[2]+xyz3.val[0]*xyz2.val[1]*xyz1.val[2];
ABCD2[0]=(xyz2.val[0]-xyz1.val[0])*2;
ABCD2[1]=(xyz2.val[1]-xyz1.val[1])*2;
ABCD2[2]=(xyz2.val[2]-xyz1.val[2])*2;
ABCD2[3]=xyz1.val[0]*xyz1.val[0]+xyz1.val[1]*xyz1.val[1]+xyz1.val[2]*xyz1.val[2]-(xyz2.val[0]*xyz2.val[0]+xyz2.val[1]*xyz2.val[1]+xyz2.val[2]*xyz2.val[2]);
ABCD3[0]=(xyz3.val[0]-xyz1.val[0])*2;
ABCD3[1]=(xyz3.val[1]-xyz1.val[1])*2;
ABCD3[2]=(xyz3.val[2]-xyz1.val[2])*2;
ABCD3[3]=xyz1.val[0]*xyz1.val[0]+xyz1.val[1]*xyz1.val[1]+xyz1.val[2]*xyz1.val[2]-(xyz3.val[0]*xyz3.val[0]+xyz3.val[1]*xyz3.val[1]+xyz3.val[2]*xyz3.val[2]);
CvMat *ABC_mat=cvCreateMat(3,3,CV_64F);
CvMat *D_mat=cvCreateMat(3,1,CV_64F);
CvMat *Solve_mat=cvCreateMat(3,1,CV_64F);
for(int i=0;i<3;i++)
{
cvSetReal2D(ABC_mat,0,i,ABCD1[i]) ;
cvSetReal2D(ABC_mat,1,i,ABCD2[i]) ;
cvSetReal2D(ABC_mat,2,i,ABCD3[i]) ;
}
cvSetReal2D(D_mat,0,0,ABCD1[3]) ;
cvSetReal2D(D_mat,1,0,ABCD2[3]) ;
cvSetReal2D(D_mat,2,0,ABCD3[3]) ;
CvMat *ABC_mat_Invert=cvCreateMat(3,3,CV_64F);
double InvertNum=cvInvert(ABC_mat,ABC_mat_Invert,CV_LU);//求逆
cvGEMM(ABC_mat_Invert,D_mat,-1,NULL,0,Solve_mat,0);//矩阵相乘
double r1=0;
double r2=0;
double r3=0;
for(int i=0;i<3;i++)//通过计算三点到中心的距离测试结果是否正确
{
r1=(cvGetReal2D(Solve_mat,i,0)-xyz1.val[i])*(cvGetReal2D(Solve_mat,i,0)-xyz1.val[i])+r1;
r2=(cvGetReal2D(Solve_mat,i,0)-xyz2.val[i])*(cvGetReal2D(Solve_mat,i,0)-xyz2.val[i])+r2;
r3=(cvGetReal2D(Solve_mat,i,0)-xyz3.val[i])*(cvGetReal2D(Solve_mat,i,0)-xyz3.val[i])+r3;
}
cvReleaseMat(&ABC_mat);
cvReleaseMat(&D_mat);
cvReleaseMat(&ABC_mat_Invert);
return Solve_mat;//返回中心坐标矩阵
}