pcl計算樣點法向並顯示

利用最小二乘法估計樣點表面法向，並顯示

#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 (int argc, char** argv)
{
        //加載點雲模型
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        // pcl::PCLPointCloud2 cloud_blob;
        if(pcl::io::loadPCDFile<pcl::PointXYZ> ("foot.pcd", *cloud) == -1){
                
                PCL_ERROR("Could not read file \n");
        }
        //* the data should be available in cloud

        // Normal estimation*
        //法向計算
        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);
        //* normals should not contain the point normals + surface curvatures

        // Concatenate the XYZ and normal fields*
        //將點雲數據與法向信息拼接
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
        pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);

        /*圖形顯示模塊*/
        //顯示設置
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

        //設置背景色
        viewer->setBackgroundColor (0, 0, 0.7);

        //設置點雲顏色，該處為單一顏色設置
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);

        //添加需要顯示的點雲數據
        viewer->addPointCloud<pcl::PointXYZ> (cloud, single_color, "sample cloud");

        //設置點顯示大小
        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

        //添加需要顯示的點雲法向。cloud為原始點雲模型，normal為法向信息，10表示需要顯示法向的點雲間隔，即每10個點顯示一次法向，５表示法向長度。
        viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 10, 5, "normals");

        //--------------------
        while (!viewer->wasStopped ())
                {
                        viewer->spinOnce (100);
                        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
                }

        // Finish
        return (0);
}