欢迎点云相关产学研的学者和团体加入我们。
本小节中一起学习如何用kd-tree树找到具体点或空间位置的k近邻,然后学习如何找到用户(本例中是随机的)指定的某一半径内的所有近邻。
首先,在PCL(Point Cloud Learning)中国协助发行的书[1]提供光盘的第5章例1文件夹中,打开名为kdtree_search.cpp的代码文件。
下面对源代码中的关键语句进行解析,代码首先用系统时间初始化rand()函数的种子,利用时间初始化,每次运行时所产生的随机数都是不同的,或者用户可以用固定的数值或不进行初始化随机种子,运行程序产生的随机数不变,然后创建点云对象,并用随机数据填充点云对象。
srand(time(NULL)); //用系统时间初始化随机种子
pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(new pcl::PointCloud<pcl::PointXYZ>);
// 随机点云生成
cloud->width=1000; //此处为点云数量
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);
}
下面的代码块创建了KdTreeFLANN对象,并把我们创建的点云设置成输入,然后创建一个searchPoint变量作为查询点,并且分配随机坐标值给它。
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建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);
现在创建一个整数(设置成10)和两个向量来存储搜索到的K近邻,两个向量中,一个存储搜索到查询点近邻的索引,另一个存储对应近邻的平方距离。
// k近邻搜索
int 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;
假设kdtree对象返回了多于0个近邻,搜索结果已经存储在我们之前创建的两个向量pointIdxNKNSearch、pointNKNSquaredDistance中,并把所有10个近邻的位置打印输出。
if(kdtree.nearestKSearch(searchPoint,K,pointIdxNKNSearch,pointNKNSquaredDistance)>0) //执行k近邻搜索
{
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;
}
下面代码展示找到了给定searchPoint的某一半径(随机产生)内的所有近邻,它重新定义两个向量pointIdxRadiusSearch、pointRadiusSquaredDistance来存储关于近邻的信息。
// 半径r内近邻搜索方式
std::vector<int> pointIdxRadiusSearch; //存储近邻索引
std::vector<float> pointRadiusSquaredDistance; //存储近邻对应的平方距离
float radius=256.0f*rand()/(RAND_MAX+1.0f);
像之前一样,如果kdtree对象在指定半径内返回多于0个近邻,它将打印输出向量中存储的点的坐标与距离。
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;
}
利用光盘提供的CMakeLists.txt文件,在cmake中建立工程文件,并生成相应的可执行文件。生成执行文件后,就可以运行了,在cmd中键入命令:
...>kdtree_search.exe
运行之后你将看到类似如图1所示的结果,打印输出所有近邻的点坐标以及对应的平方距离值。
图1 快速邻域搜索结果
敬请关注PCL(Point Cloud Learning)中国更多的点云库PCL(Point Cloud Library)相关官方教程。
参考文献:
1.朱德海、郭浩、苏伟.点云库PCL学习教程(ISBN 978-7-5124-0954-5)北京航空航天出版社 2012-10