点云库PCL学习笔记 -- k-d tree 与八叉树 -- 2.使用 k-d tree 实现快速邻域搜索(K近邻搜索、半径r内近邻搜索)
使用 k-d tree 实现快速邻域搜索代码kdtree_search.cpp
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
//用系统时间初始化随机种子
srand (time (NULL));
//定义并实例化一个PointCloud指针对象,并用随机点集赋值给它
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);
}
//创建 k-d tree 对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//设置搜索空间
kdtree.setInputCloud (cloud);
//定义查询目标点,并利用随机数对其进行赋值
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);
// K 近邻搜索
int K = 10; //设置搜索的 K 近邻数量为10
std::vector<int> pointIdxNKNSearch(K); //存储查询目标点的近邻索引
std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应的平方距离
//打印查询目标点的相关信息
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
//如果 k-d tree 存在近邻点,则输出近邻点的相关位置信息和平方距离信息
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
for (std::size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << (*cloud)[ pointIdxNKNSearch[i] ].x
<< " " << (*cloud)[ pointIdxNKNSearch[i] ].y
<< " " << (*cloud)[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
//半径 r 内近邻搜索方式
std::vector<int> pointIdxRadiusSearch; //存储近邻索引(设置在半径 r 内的搜索近邻)
std::vector<float> pointRadiusSquaredDistance; //存储近邻点对应的平方距离
float radius = 256.0f * rand () / (RAND_MAX + 1.0f); //利用随机数产生近邻搜索半径r
//打印随机生成的近邻搜索半径 r
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
//如果在半径 r 内存在近邻点,则输出近邻点的相关位置信息和平方距离信息
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;
}
return 0;
}
设置编译文件CMakeLists.txt
cmake_minimum_required(VERSION 2.8 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})
编译
mkdir build
cd build/
cmake ..
make
执行程序
cd ..
./build/kdtree_search
结果如下: