计算点云法向量

用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     static void MyNormalEstimation(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     }