#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>

typedef pcl::PointXYZ PointT;

int
main(int argc, char** argv)
{
    // All the objects needed
    pcl::PCDReader reader;
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    // Read in the cloud data
    reader.read("table_scene_mug_stereo_textured.pcd", *cloud);
    std::cerr << "PointCloud has: " << cloud->points.size() << " data points." << std::endl;
    

    
    
    pcl::PCDWriter writer;
    pcl::ExtractIndices<PointT> extract;//点提取对象
    pcl::ExtractIndices<pcl::Normal> extract_normals;//点提取对象
    

    // Datasets
    pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);//滤波后点云
    
    pcl::PointCloud<PointT>::Ptr cloud_filtered2(new pcl::PointCloud<PointT>);//滤波后点云
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);//点类型点云对象
    //法线类型对象
    pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients), coefficients_cylinder(new pcl::ModelCoefficients);
    //模型系数点云
    pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices), inliers_cylinder(new pcl::PointIndices);



    //建立一个直通滤器来去除杂散的 NaNs
    pcl::PassThrough<PointT> pass;//创建直通滤波器对象
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.5);
    pass.filter(*cloud_filtered);//保存剩余的点到cloud_filtered;
    std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;

    // 法线估计，为后续的法线分割准备数据
    pcl::NormalEstimation<PointT, pcl::Normal> ne;//法线估计对象
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());//以kdtree作为索引方式
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);//存储输出数据集
    //设置搜索时所用的搜索机制，参数tree指向搜索时所用的搜索对象，例如kd-tree, octree等对象。
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud_filtered);//输入数据
    ne.setKSearch(50);//参数
    ne.compute(*cloud_normals);

    //设置分割所用的模型类型、方法和相关参数
    pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg;//点云分割对象,是利用采样一致性算法实现分割类
    seg.setOptimizeCoefficients(true);
    //设置随机采样一致性所构造的几何模型的类型,定义为有条件限制的平面模型
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    //设置相对权重系数distance_weight，该权重与距离成正比，与角度成反比。
    seg.setNormalDistanceWeight(0.1);

    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(100);//设置迭代次数的上限
// 该函数配合用户指定的模型，设置点到模型的距离阈值0.03，如果点到模型的距离不超过这个距离阂值，
    //认为该点为局内点，否则认为是局外点，被剔除。
    seg.setDistanceThreshold(0.03);

    seg.setInputCloud(cloud_filtered);
    //设置输人点云的法线，normals为指向法线的指针。
    seg.setInputNormals(cloud_normals);
// 参数inliers是基于模型分割所得到的点云集合结果，model_ coefficients是得到的模型系数。
    seg.segment(*inliers_plane, *coefficients_plane);
    std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

    // Extract the planar inliers from the input cloud
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers_plane);//对通过setInputCloud()和setIndices()共同指定的输入点云进行聚类分割
    extract.setNegative(false);

    // Write the planar inliers to disk
    pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
    extract.filter(*cloud_plane);
    std::cerr << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
    //writer.write("table_scene_mug_stereo_textured_plane.pcd", *cloud_plane, false);

    // Remove the planar inliers, extract the rest
    extract.setNegative(true);
    extract.filter(*cloud_filtered2);
    extract_normals.setNegative(true);
    extract_normals.setInputCloud(cloud_normals);
    extract_normals.setIndices(inliers_plane);
    extract_normals.filter(*cloud_normals2);

    // Create the segmentation object for cylinder segmentation and set all the parameters
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CYLINDER);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setNormalDistanceWeight(0.1);
    seg.setMaxIterations(10000);
    seg.setDistanceThreshold(0.05);
    seg.setRadiusLimits(0, 0.1);
    seg.setInputCloud(cloud_filtered2);
    seg.setInputNormals(cloud_normals2);

    // Obtain the cylinder inliers and coefficients
    seg.segment(*inliers_cylinder, *coefficients_cylinder);
    std::cerr << "Cylinder coefficients: " << *coefficients_cylinder << std::endl;

    // Write the cylinder inliers to disk
    extract.setInputCloud(cloud_filtered2);
    extract.setIndices(inliers_cylinder);
    extract.setNegative(false);
    pcl::PointCloud<PointT>::Ptr cloud_cylinder(new pcl::PointCloud<PointT>());
    extract.filter(*cloud_cylinder);
    if (cloud_cylinder->points.empty())
        std::cerr << "Can't find the cylindrical component." << std::endl;
    else
    {
        std::cerr << "PointCloud representing the cylindrical component: " << cloud_cylinder->points.size() << " data points." << std::endl;
        writer.write("table_scene_mug_stereo_textured_cylinder.pcd", *cloud_cylinder, false);
    }
    pcl::visualization::CloudViewer viewer("Cloud viewer");
    viewer.showCloud(cloud_filtered);
    while (!viewer.wasStopped())
    {

    }
    system("pause");
    return (0);
}