PCL求三个面的交点原理详细教程（C++版）实现

这篇文章主要跟大家分享如何通过多个相互不平行的点云平面求焦点。首先，是将要求的点云平面计算出来，这篇文章有介绍过，此处不赘述：三维视觉--使用PCL求两个面的交线表达式详解（含C++源码）_低调猫熊的博客-CSDN博客

以下步骤默认已经获取点云平面的法向和常数。

在这里要说明一下，三个面的交点到底求的是什么？我们知道，面在三维空间中就是一个方向（法向）加上一个距离原点的距离（常量），通常用Ax+By+Cz+D=0。这里的(A,B,C)就是法向，也就是该面在坐标系下的朝向，D就是该面距离坐标原点的距离。有了这样的理解，下面的代码就好写了。也就是说，计算三个面的交点，就是求三个平面方程的解。

方程都怼到脸上了，三个面的A/B/C/D都知道了，求个x,y,z就很简单了。当然，上矩阵更通俗点，上面的方程直接表示为：AX+C=0。其中，A为3*3的系数矩阵，由三个面的法向构成。C为3*1的常数矩阵，由三个面的常量构成。X为3*1的矩阵，就是我们要求的(x,y,z)。到这里，代码也就写完了。

首先，构建三个平面法向向量：

	Vector3f normal1(coefficients1->values[0], coefficients2->values[0], coefficients3->values[0]);

	Vector3f normal2(coefficients1->values[1], coefficients2->values[1], coefficients3->values[1]);

	Vector3f normal3(coefficients1->values[2], coefficients2->values[2], coefficients3->values[2]);

 再根据法向向量构建系数矩阵：

    Matrix3f A;
	A << normal1, normal2, normal3;

 构建常数向量：

Vector3f C(coefficients1->values[3], coefficients2->values[3], coefficients3->values[3]);

代入方程：

Vector3f computeIntersection(const Matrix3f& A, const Vector3f& c)
{
	Vector3f x = A.colPivHouseholderQr().solve(-c);

	return x;
}

 到这里，三个面的交点就得到了。

完整代码如下：

Vector3f computeIntersection(const Matrix3f& A, const Vector3f& c)
{
	// 求解方程 Ax = -c
	Vector3f x = A.colPivHouseholderQr().solve(-c);

	return x;
}

int main(){
......

// 已知三个平面的法向量和常量项，获取交点坐标
	Vector3f normal1(coefficients1->values[0], coefficients2->values[0], coefficients3->values[0]);

	Vector3f normal2(coefficients1->values[1], coefficients2->values[1], coefficients3->values[1]);

	Vector3f normal3(coefficients1->values[2], coefficients2->values[2], coefficients3->values[2]);
// 构建系数矩阵A
	Matrix3f A;
	A << normal1, normal2, normal3;
// 构建常数项向量c
	Vector3f C(coefficients1->values[3], coefficients2->values[3], coefficients3->values[3]);
// 计算交点
	Vector3f intersectionPoint = computeIntersection(A, C);
// 输出交点坐标
	std::cout << "Point: (" << intersectionPoint(0)
		<< ", " << intersectionPoint(1)
		<< ", " << intersectionPoint(2) << ")" << std::endl;

......
}

以上就是实现交点获取的方法。

草稿代码可能不是那么严谨，但运行起来问题不大。