三维视觉--使用PCL求两个面的交线表达式详解（含C++源码）

从这篇文章开始，我将会分享三维视觉的点云处理，包含传统方法和基于深度学习的点云处理。在点云中，很多时候我们需要求两个点云平面的交线，从而进行其他方面的分析，下面进入正题。

首先，一般情况下，计算两个平面的交线会用到初三的数学知识，即：

//平面一表达式
Ax + By + Cz + D = 0;//式1
//平面二表达式
Ex + Fy + Gz + H = 0;//式2
//则交线表达式等于（式1 - 式2）

也就是交线就是两个平面表达式做差，

 这是中学时期学的。在空间中，我们还可以通过获取交线的方向和交线上一点来计算出交线方程。也就是空间中，只要知道一个点和该点的方向，便能确定一条直线了。

下面开始实现：

第一步，先读取点云并对点云做平面拟合操作：

// 加载点云数据
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZRGB>);

    if (pcl::io::loadPLYFile<pcl::PointXYZ>("cloudA.ply", *cloudA) == -1 ||
        pcl::io::loadPLYFile<pcl::PointXYZ>("cloudB.ply", *cloudB) == -1)
    {
        std::cerr << "无法加载点云文件" << std::endl;
        return -1;
    }

	// 创建两个模型对象，分别用于拟合两个面
	pcl::SACSegmentation<pcl::PointXYZRGB> seg1, seg2;
	seg1.setOptimizeCoefficients(true);
	seg1.setModelType(pcl::SACMODEL_PLANE);
	seg1.setMethodType(pcl::SAC_RANSAC);
	seg1.setMaxIterations(1000);
	seg1.setDistanceThreshold(0.001);

	seg2.setOptimizeCoefficients(true);
	seg2.setModelType(pcl::SACMODEL_PLANE);
	seg2.setMethodType(pcl::SAC_RANSAC);
	seg2.setMaxIterations(1000);
	seg2.setDistanceThreshold(0.003);

	pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients);
	pcl::ModelCoefficients::Ptr coefficients2(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
	pcl::PointIndices::Ptr inliers2(new pcl::PointIndices);

	seg1.setInputCloud(cloudA);
	seg1.segment(*inliers1, *coefficients1);

	seg2.setInputCloud(cloudB);
	seg2.segment(*inliers2, *coefficients2);

	std::cout << "第一个平面表达式："
		<< coefficients1->values[0] << " * x + "
		<< coefficients1->values[1] << " * y + "
		<< coefficients1->values[2] << " * z + "
		<< coefficients1->values[3] << " = 0" << std::endl;

	std::cout << "第二个平面表达式："
		<< coefficients2->values[0] << " * x + "
		<< coefficients2->values[1] << " * y + "
		<< coefficients2->values[2] << " * z + "
		<< coefficients2->values[3] << " = 0" << std::endl;

在拟合出平面的同时，我们也可以获得平面的法向，将两个平面的法向叉乘，就可以得到交线的方向了。这时候我们再从平面上寻找到一点带入，交线不就得到了：

// 提取平面法向量
	Eigen::Vector3d normal1(plane1[0], plane1[1], plane1[2]);
	Eigen::Vector3d normal2(plane2[0], plane2[1], plane2[2]);

	// 判断两个平面是否平行
	if (normal1.cross(normal2).norm() < 1e-6)
	{
		std::cout << "平面平行，无交线！" << std::endl;
		return false;
	}

	// 计算平面交线的方向向量
	direction = normal1.cross(normal2);

	// 计算两个平面上的一点
	Eigen::Vector3d pointOnPlane1(-plane1[3] * plane1[0], -plane1[3] * plane1[1], -plane1[3] * plane1[2]);
	Eigen::Vector3d pointOnPlane2(-plane2[3] * plane2[0], -plane2[3] * plane2[1], -plane2[3] * plane2[2]);

	// 计算交线上的一点 
	Eigen::Vector3d v = pointOnPlane2 - pointOnPlane1;
	double t = -(normal1.dot(v) / normal1.dot(direction));

	pointOnLine = pointOnPlane1 + t * direction;

 至此，我们就可以获取两个平面点云的交线表达式了。

完整代码如下：

#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <fstream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/mls.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/pca.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>

using namespace std;


string path1 = "你的路径";

// 计算平面交线的函数
bool computePlaneIntersection(const Eigen::Vector4d& plane1, const Eigen::Vector4d& plane2,
	Eigen::Vector3d& direction, Eigen::Vector3d& pointOnLine)
{
	// 提取平面法向量
	Eigen::Vector3d normal1(plane1[0], plane1[1], plane1[2]);
	Eigen::Vector3d normal2(plane2[0], plane2[1], plane2[2]);

	// 判断两个平面是否平行
	if (normal1.cross(normal2).norm() < 1e-6)
	{
		std::cout << "平面平行，无交线！" << std::endl;
		return false;
	}

	// 计算平面交线的方向向量
	direction = normal1.cross(normal2);

	// 计算两个平面上的一点
	Eigen::Vector3d pointOnPlane1(-plane1[3] * plane1[0], -plane1[3] * plane1[1], -plane1[3] * plane1[2]);
	Eigen::Vector3d pointOnPlane2(-plane2[3] * plane2[0], -plane2[3] * plane2[1], -plane2[3] * plane2[2]);

	// 计算交线上的一点 
	Eigen::Vector3d v = pointOnPlane2 - pointOnPlane1;
	double t = -(normal1.dot(v) / normal1.dot(direction));

	pointOnLine = pointOnPlane1 + t * direction;

	return true;
}


int main()
{
	// 加载点云数据
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZRGB>);

    if (pcl::io::loadPLYFile<pcl::PointXYZ>("cloudA.ply", *cloudA) == -1 ||
        pcl::io::loadPLYFile<pcl::PointXYZ>("cloudB.ply", *cloudB) == -1)
    {
        std::cerr << "无法加载点云文件" << std::endl;
        return -1;
    }

	// 创建两个模型对象，分别用于拟合两个面
	pcl::SACSegmentation<pcl::PointXYZRGB> seg1, seg2;
	seg1.setOptimizeCoefficients(true);
	seg1.setModelType(pcl::SACMODEL_PLANE);
	seg1.setMethodType(pcl::SAC_RANSAC);
	seg1.setMaxIterations(1000);
	seg1.setDistanceThreshold(0.001);

	seg2.setOptimizeCoefficients(true);
	seg2.setModelType(pcl::SACMODEL_PLANE);
	seg2.setMethodType(pcl::SAC_RANSAC);
	seg2.setMaxIterations(1000);
	seg2.setDistanceThreshold(0.003);

	pcl::ModelCoefficients::Ptr coefficients1(new pcl::ModelCoefficients);
	pcl::ModelCoefficients::Ptr coefficients2(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers1(new pcl::PointIndices);
	pcl::PointIndices::Ptr inliers2(new pcl::PointIndices);

	seg1.setInputCloud(cloudA);
	seg1.segment(*inliers1, *coefficients1);

	seg2.setInputCloud(cloudB);
	seg2.segment(*inliers2, *coefficients2);

	std::cout << "第一个平面表达式："
		<< coefficients1->values[0] << " * x + "
		<< coefficients1->values[1] << " * y + "
		<< coefficients1->values[2] << " * z + "
		<< coefficients1->values[3] << " = 0" << std::endl;

	std::cout << "第二个平面表达式："
		<< coefficients2->values[0] << " * x + "
		<< coefficients2->values[1] << " * y + "
		<< coefficients2->values[2] << " * z + "
		<< coefficients2->values[3] << " = 0" << std::endl;

	// 创建两个点云对象，保存拟合的平面点云 
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane1(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane2(new pcl::PointCloud<pcl::PointXYZRGB>);

	for (size_t i = 0; i < inliers1->indices.size(); ++i)
	{
		plane1->points.push_back(cloudGreen->points[inliers1->indices[i]]);
	}

	for (size_t i = 0; i < inliers2->indices.size(); ++i)
	{
		plane2->points.push_back(cloudRed->points[inliers2->indices[i]]);
	}

	// 设置点云属性  
	plane1->width = plane1->points.size();
	plane1->height = 1;
	plane1->is_dense = true;

	plane2->width = plane2->points.size();
	plane2->height = 1;
	plane2->is_dense = true;

	pcl::io::savePLYFileBinary(path1 + "plane1.ply", *plane1);
	pcl::io::savePLYFileBinary(path1 + "plane2.ply", *plane2);

	// 已知平面表达式 
	Eigen::Vector4d plane1_expression(coefficients1->values[0], coefficients1->values[1], coefficients1->values[2], coefficients1->values[3]);   // 平面1: ax + by + cz + d = 0
	Eigen::Vector4d plane2_expression(coefficients2->values[0], coefficients2->values[1], coefficients2->values[2], coefficients2->values[3]); // 平面2: ax + by + cz + d = 0

	Eigen::Vector3d direction, pointOnLine;

	if (computePlaneIntersection(plane1_expression, plane2_expression, direction, pointOnLine))
	{
		std::cout << "交线表达式：" << std::endl;
		std::cout << "x = " << pointOnLine[0] << " + t * " << direction[0] << std::endl;
		std::cout << "y = " << pointOnLine[1] << " + t * " << direction[1] << std::endl;
		std::cout << "z = " << pointOnLine[2] << " + t * " << direction[2] << std::endl;

		
	}
	else
	{
		std::cout << "无法计算交线！" << std::endl;
	}
	return 0;
}

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