ITK—平面擬合

一、三個點計算平面表達式

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> &parameter)
{
	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;

}