KdTree
Posted larry-xia
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了KdTree相关的知识,希望对你有一定的参考价值。
1 #include <pcl/point_cloud.h> 2 #include <pcl/kdtree/kdtree_flann.h> 3 4 #include <iostream> 5 #include <vector> 6 #include <ctime> 7 8 int 9 main (int argc, char** argv) 10 11 srand (time (NULL)); 12 13 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); 14 15 // Generate pointcloud data 16 cloud->width = 1000; 17 cloud->height = 1; 18 cloud->points.resize (cloud->width * cloud->height); 19 20 for (size_t i = 0; i < cloud->points.size (); ++i) 21 22 cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f); 23 cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f); 24 cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f); 25 26 27 pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; 28 29 kdtree.setInputCloud (cloud); 30 31 pcl::PointXYZ searchPoint; 32 33 searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f); 34 searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f); 35 searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f); 36 37 // K nearest neighbor search 38 39 int K = 10; 40 41 std::vector<int> pointIdxNKNSearch(K); 42 std::vector<float> pointNKNSquaredDistance(K); 43 44 std::cout << "K nearest neighbor search at (" << searchPoint.x 45 << " " << searchPoint.y 46 << " " << searchPoint.z 47 << ") with K=" << K << std::endl; 48 49 if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ) 50 51 for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i) 52 std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x 53 << " " << cloud->points[ pointIdxNKNSearch[i] ].y 54 << " " << cloud->points[ pointIdxNKNSearch[i] ].z 55 << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl; 56 57 58 // Neighbors within radius search 59 60 std::vector<int> pointIdxRadiusSearch; 61 std::vector<float> pointRadiusSquaredDistance; 62 63 float radius = 256.0f * rand () / (RAND_MAX + 1.0f); 64 65 std::cout << "Neighbors within radius search at (" << searchPoint.x 66 << " " << searchPoint.y 67 << " " << searchPoint.z 68 << ") with radius=" << radius << std::endl; 69 70 71 if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ) 72 73 for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i) 74 std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x 75 << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 76 << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 77 << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl; 78 79 80 81 return 0; 82
以上是关于KdTree的主要内容,如果未能解决你的问题,请参考以下文章