#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/model_outlier_removal.h>
#include <pcl/segmentation/sac_segmentation.h>


int main()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    // 人为填充点云数据
    cloud->width = 50 ;
    cloud->height = 1 ;
    cloud->points.resize (cloud->width * cloud->height);

    for (auto & point: *cloud)
    {
        point.x = 1024 * rand () / (RAND_MAX + 1.0f);
        point.y = 1024 * rand () / (RAND_MAX + 1.0f);
        point.z = 1.0;
    }

    // 制造 3 个 outlier
    (*cloud)[0].z = 2.0;
    (*cloud)[3].z = -2.0;
    (*cloud)[6].z = 4.0;

    // 进行平面分割，这应该都很简单了
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    pcl::SACSegmentation<pcl::PointXYZ> seg ;
    seg.setModelType(pcl::SACMODEL_PLANE) ;
    seg.setMethodType(pcl::SAC_RANSAC) ;
    seg.setDistanceThreshold(0.01) ;
    seg.setInputCloud(cloud) ;
    seg.segment(*inliers, *coefficients) ;

    // 看看得到的平面的系数
    std::cout << "Model coefficients: " << coefficients->values[0] << " "
                                        << coefficients->values[1] << " "
                                        << coefficients->values[2] << " "
                                        << coefficients->values[3] << std::endl;

    // 开始对点云进行平面滤波
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_filtered (new pcl::PointCloud<pcl::PointXYZ>) ;

    pcl::ModelOutlierRemoval<pcl::PointXYZ> plane_filter;
    plane_filter.setModelCoefficients (*coefficients);  // 把得到的平面的系数丢进去
    plane_filter.setThreshold (2);  // 距离平面 2 米之内的点不被认为是 outlier  这里的阈值根据需要设置
    plane_filter.setModelType (pcl::SACMODEL_PLANE);  // 设置模型的类型，当然是平面模型啦
    plane_filter.setInputCloud (cloud);  // 把原始点云丢进去
    plane_filter.setNegative(true) ;  // 想得到 outlier 就设置为 true    想得到 inlier 就设置为 false
    plane_filter.filter (*cloud_plane_filtered);  // 这就是最终得到的滤波之后的点云啦

    std::cout << "cloud_plane_filtered->points.size = " << cloud_plane_filtered->points.size() << std::endl;

    // 以下仅仅只是可视化部分，帮助理解
    pcl::visualization::PCLVisualizer viewer ;
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue (cloud_plane_filtered, 0, 255, 0);
    viewer.addPointCloud(cloud_plane_filtered, blue, "cloud") ;
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "cloud");
    viewer.addCoordinateSystem (2.0);

    while (!viewer.wasStopped())
    {
        viewer.spinOnce() ;
    }

    return 0;
}