计算点云法向量

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

 1 for (const auto& it : boost::filesystem::directory_iterator("/your/path")) {
 2     if (it.path().extension() == ".pcd") {
 3        std::cout << it.path() << ", " << it.path().filename() << ", " << it.path().stem() << ", " << it.path().extension() << std::endl;
 4     }
 5 }
 6 
 7 for (const auto& it : boost::filesystem::recursive_directory_iterator("/your/path")) {
 8     if (it.path().extension() == ".pcd") {
 9        std::cout << it.path() << ", " << it.path().filename() << ", " << it.path().stem() << ", " << it.path().extension() << std::endl;
10     }
11 }
// 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%都是有问题的

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

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

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