一、简介
八叉树可以用于管理稀疏3D数据的树状数据结构,每个内部节点都正好有八个子节点,本小节将介绍如何用八叉树在点云数据中进行空间划分及近邻搜索,特别地,解释了如何完成“体素内近邻搜索”、“K近邻搜索”和“半径内近邻搜索”。
二、代码分析
1)首先定义并实例化一个PointCloud指针对象,并用随机的点集赋值给它:
srand ((unsigned int) 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 (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);
}
2)然后创建一个八叉树实例,用设置分辨率进行初始化,该八叉树用它的叶节点存放点索引向量,该分辨率参数描述最低一级八叉树的最小体素的尺寸,因此八叉树的深度是分辨率和点云空间维数的函数,如果知道点云的边界框,应该用defineBoundingBox方式把它分配给八叉树,然后通过点云指针把所有点增加到八叉树中:
float resolution =128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//初始化八叉树
octree.setInputCloud (cloud);//输入点云
octree.addPointsFromInputCloud ();//构建八叉树
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);
3)一旦PointCloud和八叉树联系在一起,我们就能进行搜索操作,这里使用的第一种搜索方法是“体素近邻搜索”,他把查询点所在的体素中其他点的索引作为查询结果返回,结果以点索引的向量形式保存,因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数:
std::vector<int>pointIdxVec;
if (octree.voxelSearch (searchPoint, pointIdxVec))
{
std::cout<<"Neighbors within voxel search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z<<")"
<<std::endl;
for (size_t i=0; i<pointIdxVec.size (); ++i)
std::cout<<" "<< cloud->points[pointIdxVec[i]].x
<<" "<< cloud->points[pointIdxVec[i]].y
<<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
}
4)接下来,介绍k近邻搜索,本例中,k被设置为10,“k近邻搜索”方法把搜索结果写到两个分开保存的向量中,第一个pointIdxNKNSearch包含搜索结果(结果点的索引的向量),第二个向量保存相应的搜索点和近邻之间的平方距离:
int K =10;
std::vector<int>pointIdxNKNSearch;
std::vector<float>pointNKNSquaredDistance;
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if (octree.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;
}
5)“半径内近邻搜索”原理和“k近邻搜索”类似,它的搜索结果被写入两个分开的向量中,这两个向量分别存储结果点的索引和对应的平方距离:
std::vector<int>pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
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;
if (octree.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;
}
6)整体代码如下:
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/octree/octree.h> //八叉树的头文件
int main(int argc, char** argv)
{
srand((unsigned int)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 (size_t i = 0;i < cloud->points.size();++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
float resolution = 128.0f; //设置最低一级的八叉树的最小体素尺寸,根据该尺寸初始化八叉树
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
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);
std::vector<int>pointIdxVec; //进行体素近邻搜索,将查询点所在的体素中其他的点的索引作为查询结果返回
if (octree.voxelSearch(searchPoint, pointIdxVec)) //结果以索引的向量形式保存
{
std::cout << "Neighbors within voxel search at(" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ")" << std::endl;
for (size_t i = 0; i < pointIdxVec.size();++i)
{
std::cout << " " << cloud->points[pointIdxVec[i]].x << " " << cloud->points[pointIdxVec[i]].y << " " << cloud->points[pointIdxVec[i]].z << std::endl;
}
}
else
{
std::cout << "Voxel search failed!" << std::endl;
}
std::cout << "\n" << std::endl;
int K = 10; //执行k近邻搜索
std::vector<int>pointIdxNKNSearch(K); //保存点的索引
std::vector<float>pointNKNSquareDistance(K); //保存搜索到达的点与近邻之间的平方距离
std::cout << "K nearest neightbor searchpoint at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K = " << K << std::endl;
if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquareDistance) > 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:" << pointNKNSquareDistance[i] << ")" << std::endl;
}
}
else
{
std::cout << "NO K nearested point detected" << std::endl;
}
std::cout << "\n" << std::endl;
std::vector<int>pointIdxRadiusSearch; //执行半径搜索,保存搜索到的点的索引
std::vector<float>pointRadiusSquareDistance; //保存搜索到的点与查询点之间的平方距离
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;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquareDistance) > 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:" << pointRadiusSquareDistance[i] << ")" << std::endl;
}
}
else
{
std::cout << "No enough points detected in Radius Search!" << std::endl;
}
return(0);
}
三、编译结果