首页 > 代码库 > brute-force search
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 }
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 }
声明:以上内容来自用户投稿及互联网公开渠道收集整理发布,本网站不拥有所有权,未作人工编辑处理,也不承担相关法律责任,若内容有误或涉及侵权可进行投诉: 投诉/举报 工作人员会在5个工作日内联系你,一经查实,本站将立刻删除涉嫌侵权内容。