#include
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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>
#include <pcl/visualization/pcl_visualizer.h>

int
main(int argc, char** argv)
{
pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ);

// Fill in the cloud data
cloud->width = 150;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);

// Generate the data
for (auto& point : *cloud)
{
    point.x = 100 * rand() / (RAND_MAX + 1.0f);
    point.y = 100 * rand() / (RAND_MAX + 1.0f);
    point.z =1;
}

// Set a few outliers
(*cloud)[0].z = 2.0;
(*cloud)[3].z = -2.0;
(*cloud)[6].z = 4.0;

std::cerr << "Point cloud data: " << cloud->size() << " points" << std::endl;
for (const auto& point : *cloud)
    std::cerr << "    " << point.x << " "
    << point.y << " "
    << point.z << std::endl;

pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);

seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

if (inliers->indices.size() == 0)
{
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    return (-1);
}
//平面的系数  ax + by + cz +d  =0;   此处 因为Z是固定值 输出的平面 为 Z-1 =0
std::cerr << "Model coefficients: " 
    << coefficients->values[0] << " "
    << coefficients->values[1] << " "
    << coefficients->values[2] << " "
    << coefficients->values[3] << std::endl;

//std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
//for (std::size_t i = 0; i < inliers->indices.size(); ++i)
//    for (const auto& idx : inliers->indices)
//        std::cerr << idx << "    " << cloud->points[idx].x << " "
//        << cloud->points[idx].y << " "
//        << cloud->points[idx].z << std::endl;


pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->addCoordinateSystem(1.0);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "3D Viewer");

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color);


while (!viewer->wasStopped())
{
    viewer->spinOnce(100);

/* boost::this_thread::sleep(boost::posix_time::microseconds(100000));
*/ }

return (0);

}