// NormalEstimation.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.
 
h>


int main()
{
    //載入點雲模型
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\rabbit.pcd", *cloud)==-1)
    {
        PCL_ERROR("Could not read file\n");
    }

    //計演算法線
    pcl::NormalEstimation
 
<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    //建立kdtree來進行近鄰點集搜尋
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    //為kdtree新增點運資料
    tree->setInputCloud(cloud);
    n.
 
setInputCloud(cloud);
    n.setSearchMethod(tree);
    //點雲法向計算時，需要所搜的近鄰點大小
    n.setKSearch(20);
    //開始進行法向計算
    n.compute(*normals);

    /*圖形顯示模組*/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
    //viewer->initCameraParameters();

    int v1(0), v2(1),v3(2),v4(3);
    viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v1);
    viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v2);
    viewer->createViewPort(0.0, 0.5, 0.5, 1.0, v3);
    viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v4);

    //設定背景顏色
    viewer->setBackgroundColor(5,55, 10, v1);
    //設定點雲顏色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 225, 0);
    //新增座標系
    viewer->addCoordinateSystem(0.5, v1);

    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud",v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud0", v2);
    viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 50, 0.01, "normals",v2);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud1", v3);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud3", v4);

    //設定點雲大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud3", 4);


    //新增點雲法向量的另一種方式；
    //解決來源http://www.pcl-users.org/How-to-add-PointNormal-on-PCLVisualizer-td4037041.html
    //pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    //pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
    //viewer->addPointCloudNormals<pcl::PointNormal>(cloud_with_normals, 50, 0.01, "normals");
    //
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}