计算点云拟合出的两个平面之间的距离

       在点云应用的过程中，有时候需要知道两个平面之间的距离，比如桌面离地面的距离、箱子到托盘的距离等。

1、计算平面间的夹角

       计算两个平面之间的距离首先要计算两个平面之间的夹角，两个平面必须平行或者近似平行。下面的代码是计算两个平面之间的夹角。

pcl::PointXYZ n1(mdelCoefficients_plane->values[0], mdelCoefficients_plane->values[1], mdelCoefficients_plane->values[2]), 
		      n2(mdelCoefficients_plane1->values[0], mdelCoefficients_plane1->values[1], mdelCoefficients_plane1->values[2]);
double angle = get_angle(n1,n2);
cout << "平面间夹角：" << angle << endl;

double get_angle(pcl::PointXYZ n1, pcl::PointXYZ n2) {
	double cosθ = abs(n1.x * n2.x + n1.y * n2.y + n1.z * n2.z) /(sqrt(n1.x * n1.x + n1.y * n1.y + n1.z * n1.z) * sqrt(n2.x * n2.x + n2.y * n2.y + n2.z * n2.z));
	double angle = std::acos(cosθ);
	angle = angle * 180 / 3.14;
	return angle;
}

2、计算平面之间的距离

       在实际应用中考虑到点云拟合的两个平面很可能不是完全平行的，因此这里通过计算一个点云平面上所有点到另一个平面的距离，然后求平均值来作为该平面到另一个平面的距离。

       在PCL库中可以使用pcl::pointToPlaneDistance来计算点到平面的距离。

Eigen::Vector4f plane_coefficients(mdelCoefficients_plane->values[0], mdelCoefficients_plane->values[1], mdelCoefficients_plane->values[2],
		mdelCoefficients_plane->values[3]);// 平面系数
	double distance = 0;
//遍历点云中的所有点
	for (pcl::PointCloud<pcl::PointXYZ>::iterator it = cloud_p1->begin(); it != cloud_p1->end(); ++it)
	{	
		pcl::PointXYZ n(it->x, it->y, it->z);
		distance += pcl::pointToPlaneDistance(n, plane_coefficients);
	}
	distance /= cloud_p1->size();
	cout << "平面间距离：" << distance << endl;