PCL近邻搜索相关的类

首先PCL定义了搜索的基类pcl::search::Search<PointInT>

template<typename PointT>
    class Search

其子类包括：KD树，八叉树，FLANN快速搜索，暴力搜索（brute force），有序点云搜索。

The pcl_search library provides methods for searching for nearest neighbors using different data structures, including:

  • kd-trees (via libpcl_kdtree);
  • octrees (via libpcl_octree);
  • brute force;
  • specialized search for organized datasets.

search类都定义了两种最常见的近邻搜索模式：k近邻搜索，球状固定距离半径近邻搜索。

virtual int nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const = 0;

virtual int  radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices, std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;

还有一种比较有用的方式是：圆柱固定距离半径搜索，即在XOY平面的投影距离，目前没有看到PCL中的专门的实现方式，可以通过一种折中的方法解决octree。

还有就是通过累积地图实现的投影到XOY平面内的简单搜索。

Weinmann, M., et al. (2015). "Semantic point cloud interpretation based on optimal neighborhoods, relevant features and efficient classifiers." ISPRS Journal of Photogrammetry and Remote Sensing 105: 286-304.

在feature模块中大量使用了近邻搜索的东西。近邻搜索是很多点云计算的基础功能。

示例

如下调用了点云KD树近邻搜索实现了8个基于特征值的点云特征计算：

  1     QString filePly = dlg.txtPath->text();
  2     std::wstring pszRoomFile1 = filePly.toStdWString();
  3     char buffer[256];
  4     size_t ret = wcstombs(buffer, pszRoomFile1.c_str(), sizeof(buffer));
  5     const char * pszShapeFile = buffer;
  6     char * file_name = (char*)pszShapeFile;
  7     pcl::PointCloud<pcl::PointXYZ>::Ptr input_(new pcl::PointCloud<pcl::PointXYZ>);
  8     /*------------读取PLY文件-------------*/
  9     if (pcl::io::loadPLYFile<pcl::PointXYZ>(file_name, *input_) == -1) //* load the file 
 10     {
 11         PCL_ERROR("Couldn't read file test_pcd.pcd \n");
 12         QMessageBox::information(NULL, "Title", "Content", QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
 13     }
 14     double search_radius_ = dlg.sb_scale2->value();
 15     int k_=10;
 16     double search_parameter_ = 0.0;
 17     /*------------构造索引-------------*/
 18     boost::shared_ptr <std::vector<int> > indices_;
 19     if (!indices_)
 20     {
 21         indices_.reset(new std::vector<int>);
 22         try
 23         {
 24             indices_->resize(input_->points.size());
 25         }
 26         catch (const std::bad_alloc&)
 27         {
 28             PCL_ERROR("[initCompute] Failed to allocate %lu indices.\n", input_->points.size());
 29         }
 30         for (size_t i = 0; i < indices_->size(); ++i) { (*indices_)[i] = static_cast<int>(i); }
 31     }
 32     std::vector<int> nn_indices(k_);
 33     std::vector<float> nn_dists(k_);
 34     /*---------------构造KD树-------------*/
 35     //pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
 36     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
 37     tree->setInputCloud(input_);
 38     SearchMethodSurface search_method_surface_;
 39     if (search_radius_ != 0.0)
 40     {
 41         search_parameter_ = search_radius_;
 42         int (pcl::search::Search<pcl::PointXYZ>::*radiusSearchSurface)(const PointCloudIn &cloud, int index, double radius,
 43             std::vector<int> &k_indices, std::vector<float> &k_distances,
 44             unsigned int max_nn) const = &pcl::search::Search<pcl::PointXYZ>::radiusSearch;
 45         search_method_surface_ = boost::bind(radiusSearchSurface, boost::ref(tree), _1, _2, _3, _4, _5, 0);
 46     }
 47     else
 48     {
 49         search_parameter_ = k_;
 50         int (pcl::search::Search<pcl::PointXYZ>::*nearestKSearchSurface)(const PointCloudIn &cloud, int index, int k, std::vector<int> &k_indices,
 51             std::vector<float> &k_distances) const = &pcl::search::Search<pcl::PointXYZ>::nearestKSearch;
 52         search_method_surface_ = boost::bind(nearestKSearchSurface, boost::ref(tree), _1, _2, _3, _4, _5);
 53     }
 54     pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
 55     PointCloudOut& output = *cloud_normals;
 56     AxPointCloudOut output_pts;
 57     if (input_->size() < 3)
 58     {
 59         return;
 60     }
 61     else/*--------------计算特征值和特征向量-------------*/
 62     {
 63         output_pts.resize(input_->size());
 64         //output_pts.reserve(input_->size(), (new AxPoint()));
 65 #ifdef _OPENMP
 66 #pragma omp parallel for shared (output_pts) private (nn_indices, nn_dists) num_threads(threads_)
 67 #endif
 68         // Iterating over the entire index vector
 69         for (int idx = 0; idx < static_cast<int> (indices_->size()); ++idx)
 70         {
 71             if (search_method_surface_(*input_, (*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
 72             {
 73                 output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN();
 74 
 75                 output.is_dense = false;
 76                 continue;
 77             }
 78 
 79             EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
 80             // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
 81             Eigen::Vector4f xyz_centroid;
 82 
 83             if (pcl::computeMeanAndCovarianceMatrix(*input_, nn_indices, covariance_matrix, xyz_centroid) == 0)
 84             {
 85                 continue;
 86             }
 87 
 88             EIGEN_ALIGN16 Eigen::Matrix3f eigen_vector3;
 89             EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
 90             //计算特征值和特征向量
 91             pcl::eigen33(covariance_matrix, eigen_vector3, eigen_values);
 92             double eig_val1 = eigen_values[0];
 93             double eig_val2 = eigen_values[1];
 94             double eig_val3 = eigen_values[2];
 95             
 96             output_pts[idx].x = input_->at((*indices_)[idx]).x;
 97             output_pts[idx].y = input_->at((*indices_)[idx]).y;
 98             output_pts[idx].z = input_->at((*indices_)[idx]).z;
 99             output_pts[idx].eig_val1 = eig_val1;
100             output_pts[idx].eig_val2 = eig_val2;
101             output_pts[idx].eig_val3 = eig_val3;
102         }
103         QString savefilePly = filePly.replace(".ply",".txt");
104         std::wstring psaveFile1 = savefilePly.toStdWString();
105         char buffer[256];
106         size_t ret = wcstombs(buffer, psaveFile1.c_str(), sizeof(buffer));
107         const char * psavetxtFile = buffer;
108         char * file_name_2 = (char*)psavetxtFile;
109         FILE*  saveFeaturePointCloud = fopen(file_name_2, "w");
110         for (int i = 0; i < output_pts.size(); i++)
111         {
112             float x = output_pts[i].x;
113             float y = output_pts[i].y;
114             float z = output_pts[i].z;
115 
116             //注意：eig_val1最小
117             float eig_val1 = output_pts[i].eig_val1;
118             float eig_val2 = output_pts[i].eig_val2;
119             float eig_val3 = output_pts[i].eig_val3;
120             float eig_sum = eig_val1 + eig_val2 + eig_val3;
121 
122             float e1 = 0, e2 = 0, e3 = 0;
123             float Linearity = 0;
124             float Planarity = 0;
125             float Scattering = 0;
126             float Omnivariance = 0;
127             float Anisotropy = 0;
128             float EigenEntropy = 0;
129             float changeOfcurvature = 0;
130             if (eig_sum != 0)
131             {
132                 e1 = eig_val3 / eig_sum;
133                 e2 = eig_val2 / eig_sum;
134                 e3 = eig_val1 / eig_sum;
135                 Linearity = (e1 - e2) / e1;
136                 Planarity = (e2 - e3) / e1;
137                 Scattering = e3 / e1;
138                 Omnivariance = pow(e1*e2*e3, 1 / 3);
139                 Anisotropy = (e1 - e3) / e1;
140                 EigenEntropy = -(e1*log(e1) + e2*log(e2) + e3*log(e3));
141                 //计算曲率变化
142                 changeOfcurvature = fabsf(e1 / (e1 + e2 + e3));
143             }
144             else
145                 changeOfcurvature = 0;
146             //x,y,z,e1,e2,e3,
147             //Linearity,Planarity,Scattering,Omnivariance,Anisotropy,
148             //Eigenentropy,Sum of eigenvalues,Change of curvature
149             fprintf(saveFeaturePointCloud, "%f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", x, y, z, eig_val1, eig_val2, eig_val3,
150                 Linearity, Planarity, Scattering, Omnivariance, Anisotropy, EigenEntropy, eig_sum, changeOfcurvature);
151         }
152         fclose(saveFeaturePointCloud);
153     }
View Code

 

posted @ 2018-11-20 16:57  太一吾鱼水  阅读(2402)  评论(0)  编辑  收藏  举报