有很多类似的算法,我在网上找到一个按照几何去解的代码,这次我自己写的是一个安装代数来解的代码。
知识来源是上一篇解n元一次方程组中的文章。我扩展到了三维。方法很简单,就是安装那个文章上的形式写出方程组,解出方程组。
POINT7D AssembleAlgorithm::computeCenterPointImprove(vector<POINT7D> dataList)
{
POINT7D r;
POINT7D *p1;
p1 = new POINT7D[3];
p1[0].x=dataList[0].x;
p1[1].x=dataList[dataList.size()/2].x;
p1[2].x=dataList[dataList.size() - 1].x;
p1[0].y=dataList[0].y;
p1[1].y=dataList[dataList.size()/2].y;
p1[2].y=dataList[dataList.size() - 1].y;
p1[0].z=dataList[0].z;
p1[1].z=dataList[dataList.size()/2].z;
p1[2].z=dataList[dataList.size() - 1].z;
AdjustLSM MObj;
// int nRow=8, nCol=12;
double** d = MObj.CreateMatrix(3, 4);
d[0][0] = p1[0].x - p1[1].x;
d[0][1] = p1[0].y - p1[1].y;
d[0][2] = p1[0].z - p1[1].z;
d[1][0] = p1[0].x - p1[2].x;
d[1][1] = p1[0].y - p1[2].y;
d[1][2] = p1[0].z - p1[2].z;
d[2][0] = p1[1].x - p1[2].x;
d[2][1] = p1[1].y - p1[2].y;
d[2][2] = p1[1].z - p1[2].z;
//u
d[0][3] = (p1[0].x*p1[0].x - p1[1].x*p1[1].x +
p1[0].y*p1[0].y - p1[1].y*p1[1].y +
p1[0].z*p1[0].z - p1[1].z*p1[1].z)/2.0;
d[1][3] = (p1[0].x*p1[0].x - p1[2].x*p1[2].x +
p1[0].y*p1[0].y - p1[2].y*p1[2].y +
p1[0].z*p1[0].z - p1[2].z*p1[2].z)/2.0;
d[2][3] = (p1[1].x*p1[1].x - p1[2].x*p1[2].x +
p1[1].y*p1[1].y - p1[2].y*p1[2].y +
p1[1].z*p1[1].z - p1[2].z*p1[2].z)/2.0;
double* X = this->solveEquationSet(d, 3);
这样你觉得就结束了吗?nonono
因为是3维空间上的点,方程组的约束其实只在二维空间上能找到唯一的点,在三维上可以找到一条直线。
所以还要把解出来的投射到这个平面上。
//点x不一定在这个平面,所以需要投影到平面上去
float t1,t2,t3;
POINT7D* p =new POINT7D;
<span style="white-space:pre"> </span>//t1,t2,t3是法向量,有其他很多方法可以求,大家可以百度一下,我这里是调用了其他的一个函数
this->planeFitting(t1, t2, t3, p, dataList);
//求出单位向量
float sum_fenmu = sqrt(t1*t1 + t2*t2 + t3*t3);
t1 /= sum_fenmu;
t2 /= sum_fenmu;
t3 /= sum_fenmu;
POINT7D tmp;
tmp.x = X[0] - p1[0].x;
tmp.y = X[1] - p1[0].y;
tmp.z = X[2] - p1[0].z;
float tmp_muli;
tmp_muli = tmp.x*t1 + tmp.y*t2 + tmp.z*t3;
tmp.x = tmp_muli*t1;
tmp.y = tmp_muli*t2;
tmp.z = tmp_muli*t3;
r.x = X[0] - tmp.x;
r.y = X[1] - tmp.y;
r.z = X[2] - tmp.z;
return r;
}
这个只是我的程序中的函数我没有把他独立出来,不过有一些基础的人应该改一些东西就能独立使用了。