网站链接结构有哪些,17种新型商业模式,住房和城建设网站,wordpress做的好看【PCL】#xff08;十三#xff09;使用KdTree查找点
以下代码实现使用KdTree来查找特定点的K个最近邻点#xff0c;以及查找指定的某个半径内的所有邻点。
kdtree_search.cpp#xff1a;
#include pcl/point_cloud.h
#include pcl/kdtree/kdtree_flann.h十三使用KdTree查找点
以下代码实现使用KdTree来查找特定点的K个最近邻点以及查找指定的某个半径内的所有邻点。
kdtree_search.cpp
#include pcl/point_cloud.h
#include pcl/kdtree/kdtree_flann.h#include iostream
#include vector
#include ctimeint main ()
{srand (time (NULL)); // 把当前的时间作为种子使程序每次运行产生不同的随机数pcl::PointCloudpcl::PointXYZ::Ptr cloud (new pcl::PointCloudpcl::PointXYZ);cloud-width 1000;cloud-height 1;cloud-points.resize (cloud-width * cloud-height);for (std::size_t i 0; i cloud-size (); i){(*cloud)[i].x 1024.0f * rand () / (RAND_MAX 1.0f);(*cloud)[i].y 1024.0f * rand () / (RAND_MAX 1.0f);(*cloud)[i].z 1024.0f * rand () / (RAND_MAX 1.0f);}// 创建kdtree对象并将随机创建的云cloud设置为输入pcl::KdTreeFLANNpcl::PointXYZ kdtree;kdtree.setInputCloud (cloud);// 创建一个查询点pcl::PointXYZ searchPoint;searchPoint.x 1024.0f * rand () / (RAND_MAX 1.0f); // [0,1024)searchPoint.y 1024.0f * rand () / (RAND_MAX 1.0f);searchPoint.z 1024.0f * rand () / (RAND_MAX 1.0f);/* K 近邻点查找 */int K 10; // 邻点数量 std::vectorint pointIdxKNNSearch(K); // 存储K个邻点在目标点云cloud的索引std::vectorfloat pointKNNSquaredDistance(K); // 存储K个邻点与查询点searchPoint的距离std::cout K nearest neighbor search at ( searchPoint.x searchPoint.y searchPoint.z ) with K K std::endl;// 如果nearestKSearch返回了0个以上的邻点则打印出这些点的坐标和离查询点的距离。if ( kdtree.nearestKSearch (searchPoint, K, pointIdxKNNSearch, pointKNNSquaredDistance) 0 ){for (std::size_t i 0; i pointIdxKNNSearch.size (); i)std::cout (*cloud)[ pointIdxKNNSearch[i] ].x (*cloud)[ pointIdxKNNSearch[i] ].y (*cloud)[ pointIdxKNNSearch[i] ].z (squared distance: pointKNNSquaredDistance[i] ) std::endl;}/* 指定半径内邻点查找 */std::vectorint pointIdxRadiusSearch;std::vectorfloat pointRadiusSquaredDistance;float radius 256.0f * rand () / (RAND_MAX 1.0f); // 随机指定一个半径 [0,256)std::cout Neighbors within radius search at ( searchPoint.x searchPoint.y searchPoint.z ) with radius radius std::endl;// 如果radiusSearch返回0个以上的邻居则打印出这些点的坐标和距离查询点的距离。if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) 0 ){for (std::size_t i 0; i pointIdxRadiusSearch.size (); i)std::cout (*cloud)[ pointIdxRadiusSearch[i] ].x (*cloud)[ pointIdxRadiusSearch[i] ].y (*cloud)[ pointIdxRadiusSearch[i] ].z (squared distance: pointRadiusSquaredDistance[i] ) std::endl;}
}CMakeLists.txt
cmake_minimum_required(VERSION 3.5 FATAL_ERROR)project(kdtree_search)find_package(PCL 1.2 REQUIRED)include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})add_executable (kdtree_search kdtree_search.cpp)
target_link_libraries (kdtree_search ${PCL_LIBRARIES})编译并运行
$ ./kdtree_searchK nearest neighbor search at (611.912 714.643 318.294) with K10577.656 735.628 365.639 (squared distance: 3855.37)649.357 780.609 335.414 (squared distance: 6046.84)577.517 711.442 234.653 (squared distance: 8189.24)514.245 714.403 350.59 (squared distance: 10581.8)543.427 716.362 397.122 (squared distance: 10906.9)618.627 792.671 234.902 (squared distance: 13087.8)578.426 601.173 354.667 (squared distance: 15319.5)727.001 658.541 309.456 (squared distance: 16471.1)570.426 835.116 348.912 (squared distance: 17172.3)649.734 731.461 448.939 (squared distance: 18781.3)
Neighbors within radius search at (611.912 714.643 318.294) with radius179.772577.656 735.628 365.639 (squared distance: 3855.37)649.357 780.609 335.414 (squared distance: 6046.84)577.517 711.442 234.653 (squared distance: 8189.24)514.245 714.403 350.59 (squared distance: 10581.8)543.427 716.362 397.122 (squared distance: 10906.9)618.627 792.671 234.902 (squared distance: 13087.8)578.426 601.173 354.667 (squared distance: 15319.5)727.001 658.541 309.456 (squared distance: 16471.1)570.426 835.116 348.912 (squared distance: 17172.3)649.734 731.461 448.939 (squared distance: 18781.3)743.243 723.445 256.789 (squared distance: 21108.2)473.974 706.994 251.991 (squared distance: 23481.7)621.783 651.078 178.381 (squared distance: 23713.6)598.976 828.553 210.661 (squared distance: 24727.7)745.616 724.787 235.044 (squared distance: 24910.3)651.89 622.844 195.508 (squared distance: 25101.8)569.43 822.901 210.423 (squared distance: 25160.6)718.618 605.08 262.162 (squared distance: 26541)701.311 849.097 348.259 (squared distance: 26968.1)669.089 845.626 399.293 (squared distance: 26986.7)571.297 744.318 475.38 (squared distance: 27206.2)772.471 716.909 278.333 (squared distance: 27381.2)653.716 808.661 448.121 (squared distance: 27442)514.185 849.465 270.6 (squared distance: 30002.4)718.503 851.82 287.835 (squared distance: 31107.1)619.124 787.451 479.45 (squared distance: 31324.2)733.23 588.522 281.216 (squared distance: 31999.3)
