标签：style blog class code tar color
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 }
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 }
brute-force search,布布扣,bubuko.com
标签：style blog class code tar color
原文地址：http://www.cnblogs.com/uestc-wxp/p/3710268.html
