标签:arr p1 ITK int unsigned ++ 拟合 平面 ImgSphSortOrder
一、三个点计算平面表达式
void threepointToPlane()
{
double p1[3] = { 2,5,14 };
double p2[3] = { 0,35,7 };
double p3[3] = { 16,3,9 };
////平面 Ax+By+Cz+D=0
double A = (p3[1] - p1[1])*(p3[2] - p1[2]) - (p2[2] - p1[2])*(p3[1] - p1[1]);
double B = (p3[0] - p1[0])*(p2[2] - p1[2]) - (p2[0] - p1[0])*(p3[2] - p1[2]);
double C = (p2[0] - p1[0])*(p3[1] - p1[1]) - (p3[0] - p1[0])*(p2[1] - p1[1]);
double D = -(A*p1[0] + B*p1[1] + C*p1[2]);
}
二、三维空间点集拟合平面
struct Sphere
{
int id;
double SphereCenter[3];
};
void ImgPlaneFitting(const vector<Sphere> &ImgSphSortOrder,vector<double> ¶meter)
{
vector<double> Pointsx;
vector<double> Pointsy;
vector<double> Pointsz;
//存储标记点各个方向的坐标
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
Pointsx.push_back(ImgSphSortOrder[i].SphereCenter[0]);
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
Pointsy.push_back(ImgSphSortOrder[i].SphereCenter[1]);
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
Pointsz.push_back(ImgSphSortOrder[i].SphereCenter[2]);
}
//计算矩阵arr,arr*A=Z
typedef itk::Matrix<double, 3, 3> MatrixType;
MatrixType arr;
for (unsigned int i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 3; j++)
{
arr[i][j] = 0;
}
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
arr[0][0] += pow( Pointsx[i],2);
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
arr[0][1]+= Pointsx[i]*Pointsy[i];
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
arr[0][2] += Pointsx[i];
}
arr[1][0] = arr[0][1];
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
arr[1][1] += pow(Pointsy[i], 2);
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
arr[1][2] += Pointsy[i];
}
arr[2][0] = arr[0][2];
arr[2][1] = arr[1][2];
arr[2][2] = ImgSphSortOrder.size();
//计算Z
typedef itk::Vector<double, 3> VectorType;
VectorType z;
for (unsigned int i = 0; i < 3; i++)
{
z[i]=0;
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
z[0] += Pointsx[i]*Pointsz[i];
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
z[1] += Pointsy[i] * Pointsz[i];
}
for (unsigned int i = 0; i < ImgSphSortOrder.size(); i++)
{
z[2] += Pointsz[i];
}
//计算arr的逆
MatrixType arrinv(arr.GetInverse());
//计算A
typedef itk::Vector<double, 3> VectorType;
VectorType P;
P = arrinv*z;
//输出Z=a0x+a1y+a2
std::cout << "拟合的平面:z=" << P[0]<<"x+"<< P[1]<<"y+"<<P[2]<<std::endl;
for (unsigned int i = 0; i < 3; i++)
{
parameter.push_back(P[i]);
}
return 0;
}
标签:arr,p1,ITK,int,unsigned,++,拟合,平面,ImgSphSortOrder 来源: https://blog.csdn.net/qq_21760651/article/details/94559701
本站声明: 1. iCode9 技术分享网(下文简称本站)提供的所有内容,仅供技术学习、探讨和分享; 2. 关于本站的所有留言、评论、转载及引用,纯属内容发起人的个人观点,与本站观点和立场无关; 3. 关于本站的所有言论和文字,纯属内容发起人的个人观点,与本站观点和立场无关; 4. 本站文章均是网友提供,不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属;如您发现该文章侵犯了您的权益,可联系我们第一时间进行删除; 5. 本站为非盈利性的个人网站,所有内容不会用来进行牟利,也不会利用任何形式的广告来间接获益,纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。