点云库PCL学习笔记 -- k-d tree 与八叉树 -- 3.基于 Octree 八叉树的空间划分及邻域搜索(体素近邻搜索、K近邻搜索、半径r内近邻搜索)
使用 Octree 八叉树的进行空间划分及邻域搜索(体素近邻搜索、K近邻搜索、半径r内近邻搜索)代码octree_search.cpp
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
int
main (int argc, char** argv)
{
//用系统时间初始化随机种子
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 (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);
}
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; //存储体素近邻搜索的结果向量
//执行搜索,返回结果到 pointIdxVec ,并对搜索到的结果进行打印
if (octree.voxelSearch (searchPoint, pointIdxVec))
{
std::cout << "Neighbors within voxel search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z << ")"
<< std::endl;
for (std::size_t i = 0; i < pointIdxVec.size (); ++i)
std::cout << " " << (*cloud)[pointIdxVec[i]].x
<< " " << (*cloud)[pointIdxVec[i]].y
<< " " << (*cloud)[pointIdxVec[i]].z << std::endl;
}
// K 近邻搜索
int K = 10; //设置搜索的 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;
//如果 k-d tree 存在近邻点,则输出近邻点的相关位置信息和平方距离信息
if (octree.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 (octree.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 2.8 FATAL_ERROR)
project(octree_search)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (octree_search octree_search.cpp)
target_link_libraries (octree_search ${PCL_LIBRARIES})
编译
mkdir build
cd build/
cmake ..
make
执行程序
cd ..
./build/octree_search
结果如下: