#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;

//求兩直線的交點(diǎn)或者倆平面的交線


int main()
{
    //(1)兩點(diǎn)線的交點(diǎn)菩暗，返回值:交點(diǎn)是否存在俊嗽。(點(diǎn)刑然，方向)
    //bool pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, Eigen::Vector4f &point, double sqr_eps=1e-4)
    //line_a：前三個(gè)是坐標(biāo)，后三個(gè)是向量
    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)兩點(diǎn)線的交點(diǎn)报账，返回值:交點(diǎn)是否存在气破。(構(gòu)造函數(shù)：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);//參數(shù)賦值失敗

    //(3)計(jì)算兩平面的交線
    //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個(gè)數(shù)值寂嘉，暫時(shí)未理解出對(duì)應(yīng)的定義

    //(4)計(jì)算兩平面的交線遏暴，構(gòu)造函數(shù)
    //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)三個(gè)面的交點(diǎn)坐標(biāo)(提前設(shè)置點(diǎn)(2,1,3),然后設(shè)平面方程的系數(shù)，最后再進(jìn)行驗(yàn)算)
    //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)三個(gè)面的交點(diǎn)坐標(biāo)(構(gòu)造函數(shù))
    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