计算点云法向量

1.先mark一个文件操作：遍历（或者迭代遍历）指定目录，boost::filesystem可真好用

for (auto &&it : boost::filesystem::directory_iterator("/your/path")) {
    if (it.path().extension() == ".pcd") {
       std::cout << it.path() << ", " << it.path().filename() << ", " << it.path().stem() << ", " << it.path().extension() << std::endl;
    }
}

for (auto &&it : boost::filesystem::recursive_directory_iterator("/your/path")) {
    if (it.path().extension() == ".pcd") {
       std::cout << it.path() << ", " << it.path().filename() << ", " << it.path().stem() << ", " << it.path().extension() << std::endl;
    }
}
// it.path()                  it.path().filename()    it.path().stem()    it.path().extension() 
// "/path/pairs_12_7.pcd",    "pairs_12_7.pcd",       "pairs_12_7",       ".pcd"

2.用pcl::NormalEstimation简直就是坑爹，计算出的点云法向量有40~50%都是有问题的

    pcl::search::KdTree<PointS>::Ptr kdtree_submap(new pcl::search::KdTree<PointS>);
    kdtree_submap->setInputCloud(cloud_submap);// Make sure the tree searches the surface
    pcl::NormalEstimation<PointS, PointS>::Ptr ne(new pcl::NormalEstimation<PointS, PointS>);
    ne->setInputCloud(cloud_ds_angle_);
    ne->setSearchSurface(cloud_submap);
    ne->setSearchMethod(kdtree_submap);
    ne->setRadiusSearch(search_r_ne);
    ne->compute(*cloud_ds_angle_);

用pca和kdtree自己计算，效果赞赞赞，而且效率与上面的一样

    void my_normal_estimation(const KdTreePtr &kdtree, PCloudTPtr &cloud, double search_r) {
        for (auto &pt : cloud->points) {
            std::vector<int> k_indices;
            std::vector<float> k_sqr_distances;
            if (kdtree->radiusSearch(pt, search_r, k_indices, k_sqr_distances) < 3) {
                continue;
            }
            PCloudTPtr cloud_search(new PCloudT);
            pcl::copyPointCloud(*(kdtree->getInputCloud()), k_indices, *cloud_search);
            pcl::PCA<PointT> pca;
            pca.setInputCloud(cloud_search);
            Eigen::Matrix3f eigen_vector = pca.getEigenVectors();
            Eigen::Vector3f vect_2 = eigen_vector.col(2);// please fit to your own coordinate
            pt.normal_x = vect_2[0];
            pt.normal_y = vect_2[1];
            pt.normal_z = vect_2[2];
        }
    }