PCL入门（四）：kdtree简单介绍和使用

#include <iostream>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <vector>
#include <ctime>int main(int argc, char** argv)
{srand(time(NULL));//第一步：生成点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);cloud->width = 100;cloud->height = 1;cloud->points.resize(cloud->width * cloud->height);for (size_t i=0; i < cloud->points.size(); i++){cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);}//第二步：生成搜索点pcl::PointXYZ searchPoint;searchPoint.x = 1024.0f * rand() / (RAND_MAX + 1.0f);searchPoint.y = 1024.0f * rand() / (RAND_MAX + 1.0f);searchPoint.z = 1024.0f * rand() / (RAND_MAX + 1.0f);//第三步：定义kd树pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;kdtree.setInputCloud(cloud); //将点云cloud作为输入//第四步：采用kdtree.nearestKSearch方法，输出点searchPoint的最近10个点云int K = 10;std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with K=" << K << std::endl;std::vector<int> pointIdxNKNSearch(K);std::vector<float> pointNKNSquaredDistance(K);if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){for (size_t i=0; i<pointIdxNKNSearch.size(); ++i){std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x << " " << cloud->points[ pointIdxNKNSearch[i] ].y << " " << cloud->points[ pointIdxNKNSearch[i] ].z << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;}}//第五步：采用kdtree.radiusSearch方法，输出点searchPoint的给定半径距离内的其他点float radius = 256.0f * rand() / (RAND_MAX + 1.0f);std::cout << "Neighbors within radius search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z<< ") with radius=" << radius << std::endl;std::vector<int> pointIdxRadiusSearch;std::vector<float> pointRadiusSquaredDistance;if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0){for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x << " " << cloud->points[ pointIdxRadiusSearch[i] ].y << " " << cloud->points[ pointIdxRadiusSearch[i] ].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;}}return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)project(kdtree_search)find_package(PCL)add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})