#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/visualization/pcl_visualizer.h> //cout endl

#include <pcl/common/intersections.h>

typedef pcl::PointXYZ PointT;

//求两直线的交点或者俩平面的交线


int main()
{
    //(1)两点线的交点，返回值:交点是否存在。(点，方向)
    //bool pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &point, double sqr_eps=1e-4)
    //line_a：前三个是坐标，后三个是向量
    Eigen::VectorXf line_a(6);
    line_a(0) = 0;
    line_a(1) = 0;
    line_a(2) = 0;

    line_a(3) = 1;
    line_a(4) = 0;
    line_a(5) = 0;
    Eigen::VectorXf line_b(6);
    line_b(0) = 1;
    line_b(1) = 1;
    line_b(2) = 0;

    line_b(3) = 1;
    line_b(4) = 0;
    line_b(5) = 0;
    Eigen::Vector4f point;
    bool flag = pcl::lineWithLineIntersection(line_a, line_b, point, 1e-4);

    //(2)两点线的交点，返回值:交点是否存在。(构造函数：pcl::ModelCoefficients)
    //bool pcl::lineWithLineIntersection(const pcl::ModelCoefficients & line_a, const pcl::ModelCoefficients & line_b, Eigen::Vector4f & point, double sqr_eps = 1e-4)
    pcl::ModelCoefficients line_1;
    pcl::ModelCoefficients line_2;
    //bool flag_1= pcl::lineWithLineIntersection(line_1, line_2, point,1e-4);//参数赋值失败

    //(3)计算两平面的交线
    //bool planeWithPlaneIntersection(const Eigen::Matrix<Scalar, 4, 1> &plane_a,const Eigen::Matrix<Scalar, 4, 1> &plane_b,Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line,double angular_tolerance)
    Eigen::Matrix<float, 4, 1> plane_a;
    Eigen::Matrix<float, 4, 1> plane_b;
    Eigen::Matrix<float, 4, 1> plane_c;
    plane_a(0, 0) = 1;// plane_a << 1, 1, 0, 0;//赋值方法二
    plane_a(1, 0) = 1;
    plane_a(2, 0) = 1;
    plane_a(3, 0) = -6;

    plane_b(0, 0) = 2;
    plane_b(1, 0) = 1;
    plane_b(2, 0) = 2;
    plane_b(3, 0) = -11;

    plane_c(0, 0) = 3;
    plane_c(1, 0) = 1;
    plane_c(2, 0) = 4;
    plane_c(3, 0) = -19;
    Eigen::Matrix<float, Eigen::Dynamic, 1> line;
    bool flag_2 = pcl::planeWithPlaneIntersection(plane_a, plane_b, line, 1e-4);//line只有5个数值，暂时未理解出对应的定义

    //(4)计算两平面的交线，构造函数
    //bool planeWithPlaneIntersection(const Eigen::Vector4f & plane_a,const Eigen::Vector4f & plane_b,Eigen::VectorXf & line,double angular_tolerance = 0.1)
    Eigen::VectorXf line1;
    bool flag_3 = pcl::planeWithPlaneIntersection(plane_a, plane_b, line1, 1e-4);

    //(5)三个面的交点坐标(提前设置点(2,1,3),然后设平面方程的系数，最后再进行验算)
    //bool threePlanesIntersection(const Eigen::Vector4f & plane_a,const Eigen::Vector4f & plane_b,const Eigen::Vector4f & plane_c,Eigen::Vector3f & intersection_point,double determinant_tolerance = 1e-6)
    Eigen::Vector3f intersection_point;
    bool flag_4 = pcl::threePlanesIntersection(plane_a, plane_b, plane_c, intersection_point,1e-6);

    //(6)三个面的交点坐标(构造函数)
    Eigen::Matrix<double, 4, 1> plane_ad;
    Eigen::Matrix<double, 4, 1> plane_bd;
    Eigen::Matrix<double, 4, 1> plane_cd;
    plane_ad(0, 0) = 1;// plane_a << 1, 1, 0, 0;//赋值方法二
    plane_ad(1, 0) = 1;
    plane_ad(2, 0) = 1;
    plane_ad(3, 0) = -6;

    plane_bd(0, 0) = 2;
    plane_bd(1, 0) = 1;
    plane_bd(2, 0) = 2;
    plane_bd(3, 0) = -11;

    plane_cd(0, 0) = 3;
    plane_cd(1, 0) = 1;
    plane_cd(2, 0) = 4;
    plane_cd(3, 0) = -19;
    Eigen::Vector3d intersection_point_d;
    bool flag_5 = pcl::threePlanesIntersection(plane_ad, plane_bd, plane_cd, intersection_point_d, 1e-6);

    system("pause");
    return 0;
}

参考文章：
http://pointclouds.org/documentation/intersections_8h.html
http://pointclouds.org/documentation/intersections_8h_source.html