#include<pcl\point_cloud.h>
#include<pcl\point_types.h>
#include<pcl\visualization\pcl_visualizer.h>
#include<pcl\io\pcd_io.h>
#include<pcl\features\normal_3d.h>
#include<pcl\kdtree\kdtree_flann.h>
#include<pcl\surface\gp3.h>
#include<pcl\io\ply_io.h>

using namespace pcl;
using namespace std;

int main()
{
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    if (io::loadPCDFile("test.pcd",*cloud)==1)return 0;
    cout << io::loadPCDFile("test.pcd", *cloud) << endl;
    NormalEstimation<PointXYZ, Normal> n;
    PointCloud<Normal>::Ptr normals(new PointCloud<Normal>);
    search::KdTree<PointXYZ>::Ptr tree(new  search::KdTree<PointXYZ>);
    tree->setInputCloud(cloud);
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(20);
    n.compute(*normals);

    PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
    concatenateFields(*cloud, *normals,*cloud_with_normals);

    search::KdTree<PointNormal>::Ptr tree2(new search::KdTree<PointNormal>);
    tree2->setInputCloud(cloud_with_normals);

    GreedyProjectionTriangulation<PointNormal> gp3;
    PolygonMesh triangles;

    gp3.setSearchRadius(1.5f);
    gp3.setMu(2.5f);
    gp3.setMaximumNearestNeighbors(100);
    gp3.setMaximumSurfaceAngle(M_PI / 4);
    gp3.setMinimumAngle(M_PI / 18);
    gp3.setMaximumAngle(2 * M_PI / 3);
    gp3.setNormalConsistency(false);

    gp3.setInputCloud(cloud_with_normals);
    gp3.setSearchMethod(tree2);

    gp3.reconstruct(triangles);

    io::savePLYFile("result.pcd", triangles);

    boost::shared_ptr<visualization::PCLVisualizer>viewer(new visualization::PCLVisualizer);
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPolygonMesh(triangles, "my");
    viewer->addCoordinateSystem(1, 0);
    viewer->initCameraParameters();

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    system("pause");
    return 0;
}