pcl邊界識別

#include <iostream>
#include <vector>
#include <ctime>
#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/features/eigen.h>
#include <pcl/features/feature.h>
#include <pcl/features/normal_3d.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/features/boundary.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;

int main(int argc, char **argv)
{
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        // if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/yxg/pcl/pcd/mid.pcd",*cloud) == -1)
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1],*cloud) == -1)
        {
                PCL_ERROR("COULD NOT READ FILE mid.pcl \n");
                return (-1);
        }
        
        std::cout << "points sieze is:"<< cloud->size()<<std::endl;
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
        pcl::PointCloud<pcl::Boundary> boundaries;
        pcl::BoundaryEstimation<pcl::PointXYZ,pcl::Normal,pcl::Boundary> est;
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
        /*
        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;  //建立一個快速k近鄰查詢,查詢的時候若該點在點雲中，則第一個近鄰點是其自己
        kdtree.setInputCloud(cloud);
        int k =2;
        float everagedistance =0;
        for (int i =0; i < cloud->size()/2;i++)
        {
                vector<int> nnh ;
                vector<float> squaredistance;
                //  pcl::PointXYZ p;
                //   p = cloud->points[i];
                kdtree.nearestKSearch(cloud->points[i],k,nnh,squaredistance);
                everagedistance += sqrt(squaredistance[1]);
                //   cout<<everagedistance<<endl;
        }

        everagedistance = everagedistance/(cloud->size()/2);
        cout<<"everage distance is : "<<everagedistance<<endl;
        
*/


        
        pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> normEst;  //其中pcl::PointXYZ表示輸入類型數據，pcl::Normal表示輸出類型,且pcl::Normal前三項是法向，最後一項是曲率
        normEst.setInputCloud(cloud);
        normEst.setSearchMethod(tree);
        // normEst.setRadiusSearch(2);  //法向估計的半徑
        normEst.setKSearch(9);  //法向估計的點數
        normEst.compute(*normals);
        cout<<"normal size is "<< normals->size()<<endl;
        
       //normal_est.setViewPoint(0,0,0); //這個應該會使法向一致
        est.setInputCloud(cloud);
        est.setInputNormals(normals);
        //  est.setAngleThreshold(90);
        //   est.setSearchMethod (pcl::search::KdTree<pcl::PointXYZ>::Ptr (new pcl::search::KdTree<pcl::PointXYZ>));
        est.setSearchMethod (tree);
        est.setKSearch(20);  //通常這裏的數值越高，最終邊界識別的精度越好
        //  est.setRadiusSearch(everagedistance);  //搜索半徑
        est.compute (boundaries);

        //  pcl::PointCloud<pcl::PointXYZ> boundPoints;
        pcl::PointCloud<pcl::PointXYZ>::Ptr boundPoints (new               pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ> noBoundPoints;
        int countBoundaries = 0;
        for (int i=0; i<cloud->size(); i++){
                uint8_t x = (boundaries.points[i].boundary_point);
        int a = static_cast<int>(x); //該函數的功能是強制類型轉換
        if ( a == 1)
                {
                        //  boundPoints.push_back(cloud->points[i]);
                        ( *boundPoints).push_back(cloud->points[i]);
                        countBoundaries++;
                }
                else
                        noBoundPoints.push_back(cloud->points[i]);
                
    }
        std::cout<<"boudary size is：" <<countBoundaries <<std::endl;
        //  pcl::io::savePCDFileASCII("boudary.pcd",boundPoints);

        pcl::io::savePCDFileASCII("boudary.pcd", *boundPoints);
        pcl::io::savePCDFileASCII("NoBoundpoints.pcd",noBoundPoints);
        pcl::visualization::CloudViewer viewer ("test");
        viewer.showCloud(boundPoints);
        while (!viewer.wasStopped())
        {
        }
        return 0;
}