【PCL】（十三）使用KdTree查找点

以下代码实现使用KdTree来查找特定点的K个最近邻点，以及查找指定的某个半径内的所有邻点。

kdtree_search.cpp：

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>#include <iostream>
#include <vector>
#include <ctime>int main ()
{srand (time (NULL)); // 把当前的时间作为种子，使程序每次运行产生不同的随机数pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::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::KdTreeFLANN<pcl::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::vector<int> pointIdxKNNSearch(K); // 存储K个邻点在目标点云cloud的索引std::vector<float> 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::vector<int> pointIdxRadiusSearch;std::vector<float> 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_search

K nearest neighbor search at (611.912 714.643 318.294) with K=10577.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 radius=179.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)