|
參考：http://blog.sina.com.cn/s/blog_648868460100h2b8.html（這個博主還有很多關(guān)于工程測量的知識和空間幾何計算的知識）
已知空間三點的坐標(biāo)為（x1,y1,z1）,(x2,y2,z2),(x3,y3,z3),求這三個點所確定的空間圓的圓心坐標(biāo)和半徑。
分析可得約束條件：1、三點共面2、三點到空間圓心坐標(biāo)的距離相等。
從約束條件可得，4個自由項4個方程可解，可以列出線性代數(shù)方程組，即可用消元法求解；
即以下的（1）（2）（3）（4）四個方程組成的線性代數(shù)方程組
共面約束：
三點到空間圓心坐標(biāo)的距離相等約束:
(1) (2)(3)聯(lián)解可得（5）（6）同時消去R
通過（4）（5）（6）獲得關(guān)于圓心空間坐標(biāo)的線性代數(shù)方程組
利用opencv2.1實現(xiàn)的代碼如下：
CvScalar xyz1,CvScalar xyz2,CvScalar xyz3 分別為空間三點坐標(biāo)
//從空間圓上三點獲得所在圓的空間中心坐標(biāo)
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++)//通過計算三點到中心的距離測試結(jié)果是否正確
{
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;//返回中心坐標(biāo)矩陣
}
|
