brute-force search

 1 #include <pcl/search/brute_force.h>
 2 #include <pcl/common/common.h>
 3 #include <iostream>
 4 
 5 using namespace std;
 6 using namespace pcl;
 7 
 8 int main()
 9 {
10     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
11 
12     // x+y+z=1平面
13     for (float x = -1.0; x <= 1.0; x += 0.5)    
14     {    
15         for (float y = -1.0; y <= 1.0; y += 0.5)    
16         {    
17             pcl::PointXYZ cloud_;    
18             cloud_.x = x;    
19             cloud_.y = y;    
20             cloud_.z = 1 - x - y;    
21             cloud->push_back(cloud_);    
22         }    
23     }   
24 
25     pcl::search::BruteForce<pcl::PointXYZ> bfsearch;
26     bfsearch.setInputCloud(cloud);
27     std::vector<int> k_indices;
28     std::vector<float> k_distances;
29     pcl::PointXYZ p(-1.0,-1.0,3.0);
30     bfsearch.nearestKSearch(p,2,k_indices,k_distances);
31 
32     return 0;
33 }

View Code

 1 template <typename PointT> int
 2 pcl::search::BruteForce<PointT>::denseKSearch (
 3     const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const
 4 {
 5   // container for first k elements -> O(1) for insertion, since order not required here
 6   std::vector<Entry> result;
 7   result.reserve (k);
 8   std::priority_queue<Entry> queue;
 9  
10     Entry entry;
11     for (entry.index = 0; entry.index < std::min (static_cast<unsigned> (k), static_cast<unsigned> (input_->size ())); ++entry.index)
12     {
13       entry.distance = getDistSqr (input_->points[entry.index], point);
14       result.push_back (entry);
15     }
16 
17     queue = std::priority_queue<Entry> (result.begin (), result.end ());
18 
19     // add the rest
20     for (; entry.index < input_->size (); ++entry.index)
21     {
22       entry.distance = getDistSqr (input_->points[entry.index], point);
23       if (queue.top ().distance > entry.distance)
24       {
25         queue.pop ();
26         queue.push (entry);
27       }      
28     }
29 
30   k_indices.resize (queue.size ());
31   k_distances.resize (queue.size ());
32   size_t idx = queue.size () - 1;
33   while (!queue.empty ())
34   {
35     k_indices [idx] = queue.top ().index;
36     k_distances [idx] = queue.top ().distance;
37     queue.pop ();
38     --idx;
39   }
40   
41   return (static_cast<int> (k_indices.size ()));
42 }

View Code