PCL 交点及交线计算【两直线的交点 + 两平面的交线 + 三个平面的交点】（Common_Intersections）

PCL专栏目录及须知-CSDN博客

注：本示例包含计算两直线的交点、两平面的交线及三平面的交点。

本示例是common中的单个函数，因此不做可视化展示。

1.计算两直线的交点

1.1 原理

（1）假设三维空间有两直线，那么计算直线A和直线B之间，在三维空间内的交点。

（2）该函数中有个距离容差的设置，即在两直线在三维空间中不一定完全相交，但二者在一定的相交范围内。

具体原理参考：计算两直线交点原理

如下图：

直线A和直线B在距离容差范围内相交。

1.2 代码

#include <pcl/common/intersections.h>


 /****************计算两直线的交点<intersections.h>********************/
    Eigen::VectorXf line_a(6);                                  // 直线A
    line_a(0) = 0; line_a(1) = 0; line_a(2) = 0;                // 直线A的起始点XYZ值
    line_a(3) = 1; line_a(4) = 1; line_a(5) = 1;                // 直线A的方向

    Eigen::VectorXf line_b(6);                                  // 直线B
    line_b(0) = 0.5; line_b(1) = 0.7; line_b(2) = 0.8;          // 直线B的起始点XYZ值
    line_b(3) = -0.5; line_b(4) = 1; line_b(5) = -0.6;          // 直线B的方向

    Eigen::Vector4f point;                                      // 交点

    double sqr_eps_line = 0.1;                                  // 得到真解允许的最大平方距离

    // 返回值为true即交点存在，返回值为false为交点不存在
    bool bLine = pcl::lineWithLineIntersection(line_a, line_b, point, sqr_eps_line);          // 计算两直线的交点

1.3 结果展示

2.计算两平面的交线

2.1 原理

（1）假设三维空间中有两平面A和平面B，若两平面相交，那么计算得到两平面的交线。

（2）该函数中有个角度容差的设置，即在两平面在三维空间中不一定完全相交，但二者在一定的相交范围内。

具体原理参考：两平面计算交线

如下图：

平面A和平面B在角度容差范围内有交线。

2.2 代码

#include <pcl/common/intersections.h>

    /****************计算两平面的交线<intersections.h>********************/
    Eigen::Matrix<float, 4, 1> plane_a;         // 平面A
    Eigen::Matrix<float, 4, 1> plane_b;         // 平面B
    plane_a(0, 0) = 1; plane_a(1, 0) = 1; plane_a(2, 0) = 1; plane_a(3, 0) = -6;        // 平面A的Ax + By + Cz + D = 0;的参数值
    plane_b(0, 0) = 2; plane_b(1, 0) = 1; plane_b(2, 0) = 2; plane_b(3, 0) = -11;       // 平面B的Ax + By + Cz + D = 0;的参数值

    Eigen::Matrix<float, Eigen::Dynamic, 1> line;                     // 交线（六个参数：坐标XYZ值，朝向XYZ值）

    double angular_tolerance = 10.0;                                  // 得到真解允许的角度公差

    // 返回值为true即交线存在，返回值为false为交线不存在
    bool bPlane = pcl::planeWithPlaneIntersection(plane_a, plane_b, line, angular_tolerance);          // 计算两平面的交线

2.3 结果展示

3.计算三个平面的交点

3.1 原理

（1）假设三维空间中有三个平面A、平面B、平面C，三个平面相交于某一点。（两平面相交的结果是一条交线，两个交线相交的结果是一个交点，因此最后平面相交于某一点）

（2）该函数中有个名为determinant_tolerance容差的设置，即在三平面在三维空间中不一定完全相交，但三者在一定的相交范围内。

关于determinant_tolerance参数：如果使用几乎平行的平面，可以降低determinant_tolerance值。

具体原理：

平面方程的参数方程形式为：Ax + By + Cz + D = 0;

平面A：1x + 2y +3z  + 4 = 0; 

平面B：2x + 3y +4z  + 5 = 0; 

平面C：3x + 5y +6z  + 7 = 0; 

联立三个方程，解得xyz的值，即交点的位置。

如下图（三个平面的图画得不好，将就的看）：

3.2 代码

#include <pcl/common/intersections.h>

    /****************计算三个平面的交点<intersections.h>********************/
    Eigen::Matrix<float, 4, 1> plane_a0;         // 平面A
    Eigen::Matrix<float, 4, 1> plane_b0;         // 平面B
    Eigen::Matrix<float, 4, 1> plane_c0;         // 平面C
    plane_a0(0, 0) = 1; plane_a0(1, 0) = 2; plane_a0(2, 0) = 3; plane_a0(3, 0) = -6;        // 平面A的Ax + By + Cz + D = 0;的参数值
    plane_b0(0, 0) = 2; plane_b0(1, 0) = 1; plane_b0(2, 0) = 5; plane_b0(3, 0) = -11;       // 平面B的Ax + By + Cz + D = 0;的参数值
    plane_c0(0, 0) = 3; plane_c0(1, 0) = 6; plane_c0(2, 0) = 3; plane_c0(3, 0) = -19;       // 平面B的Ax + By + Cz + D = 0;的参数值

    Eigen::Vector3f intersection_point;                                    // 交点

    double determinant_tolerance = 0.2;                                    // 如果使用几乎平行的平面，可以降低determinant_tolerance值

    bool bThreePlane = pcl::threePlanesIntersection(plane_a0, plane_b0, plane_c0, intersection_point, determinant_tolerance);            // 计算三平面的交线

3.3 结果展示