注:体素搜索为八叉树特有,其余半径搜索、K邻域搜索和KD树的原理差异不大,可关联理解。
半径搜索、K邻域搜索时,KD树速度快于八叉树,一般使用KD树搜索。
除以上三个搜索外的其余搜索操作(【1】获取八叉树中,与射线相交的所有体素块的索引、【2】获取八叉树中,用户指定范围内的所有体素块的索引、【3】获取与输入点最邻近点的索引),不做可视化展示,仅提供方法批注。
其余可调用方法,如检查用户输入位置的体素块是否存在、获取树深、获取体素块中心点等在PCL八叉树的介绍:6.PCL八叉树关键函数中提及。
1.原理
PCL八叉树的介绍https://blog.csdn.net/xhm01291212/article/details/141917680?spm=1001.2014.3001.5502
2.使用场景
- 体素搜索(搜索同查找点在同一体素块中的点云点)
- K邻域搜索(搜索距离输入点Q,最邻近的K个点)
- 半径搜索(搜索输入点Q半径R内的所有点)
- 获取八叉树中,与射线相交的所有体素块的索引
- 获取八叉树中,用户指定范围内的所有体素块的索引
- 获取与输入点最邻近点的索引
3.注意事项
半径搜索、K邻域搜索时,KD树速度快于八叉树,一般使用KD树搜索,八叉树使用较少。
体素搜索只能用八叉树。
4.关键函数
(1)体素搜索:返回值为与待查找点同一体素块中的点云点的索引。
// 体素搜索
std::vector<int> pointIdxsVox; // 得到与查找点在同一个体素块中的点云点索引
bool bFlagVox = false;
if (octree.voxelSearch(searchPoint, pointIdxsVox))
{
if (pointIdxsVox.size() > 0)
{
bFlagVox = true;
}
}
(2)半径搜索
// 半径搜索
std::vector<int> pointIdxRadiusSearch; // 半径搜索结果的索引
std::vector<float> pointRadiusSquaredDistance; // 半径搜索得到与查找点的距离的平方
double radius = 0.03; // 查找半径
bool bFlagRadius = false;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
if (pointIdxRadiusSearch.size() > 0)
{
bFlagRadius = true;
}
}
(3)K邻域搜索
// K邻域搜索
std::vector<int> pointIdxNKNSearch; // K邻域搜索结果的索引
std::vector<float> pointNKNSquaredDistance; // K邻域搜索得到与查找点的距离的平方
int K = 500; // K邻域值
bool bFlagK = false;
if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
if (pointIdxNKNSearch.size() > 0)
{
bFlagK = true;
}
}
(4)获取八叉树中,与射线相交的所有体素块的索引
uindex_t
getIntersectedVoxelIndices(Eigen::Vector3f origin,
Eigen::Vector3f direction,
Indices& k_indices,
uindex_t max_voxel_count = 0) const;
(5)获取八叉树中,用户指定范围内的所有体素块的索引
uindex_t
boxSearch(const Eigen::Vector3f& min_pt,
const Eigen::Vector3f& max_pt,
Indices& k_indices) const;
(6)获取与输入点最邻近点的索引
inline void
approxNearestSearch(const PointCloud& cloud,
uindex_t query_index,
index_t& result_index,
float& sqr_distance)
{
return (approxNearestSearch(cloud[query_index], result_index, sqr_distance));
}
5.代码
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
int main()
{
/***************八叉树K邻域搜索及半径搜索********************/
// 原始点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("D:/code/csdn/data/bunny.pcd", *cloud); // 原始点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVox(new pcl::PointCloud<pcl::PointXYZ>); // 体素搜索结果点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRadius(new pcl::PointCloud<pcl::PointXYZ>); // 半径搜索结果点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudK(new pcl::PointCloud<pcl::PointXYZ>); // K邻域搜索结果点云
// 建立八叉搜索树
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.02);// 使用分辨率初始化八叉树
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
pcl::PointXYZ searchPoint = cloud->points[0]; // 待搜索点
// 体素搜索
std::vector<int> pointIdxsVox; // 得到与查找点在同一个体素块中的点云点索引
bool bFlagVox = false;
if (octree.voxelSearch(searchPoint, pointIdxsVox))
{
if (pointIdxsVox.size() > 0)
{
bFlagVox = true;
}
}
pcl::copyPointCloud(*cloud, pointIdxsVox, *cloudVox); // 将结果拷贝到新点云中
// 半径搜索
std::vector<int> pointIdxRadiusSearch; // 半径搜索结果的索引
std::vector<float> pointRadiusSquaredDistance; // 半径搜索得到与查找点的距离的平方
double radius = 0.03; // 查找半径
bool bFlagRadius = false;
if (octree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
if (pointIdxRadiusSearch.size() > 0)
{
bFlagRadius = true;
}
}
pcl::copyPointCloud(*cloud, pointIdxRadiusSearch, *cloudRadius); // 将结果拷贝到新点云中
// K邻域搜索
std::vector<int> pointIdxNKNSearch; // K邻域搜索结果的索引
std::vector<float> pointNKNSquaredDistance; // K邻域搜索得到与查找点的距离的平方
int K = 500; // K邻域值
bool bFlagK = false;
if (octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
if (pointIdxNKNSearch.size() > 0)
{
bFlagK = true;
}
}
pcl::copyPointCloud(*cloud, pointIdxNKNSearch, *cloudK); // 将结果拷贝到新点云中
// 展示
std::cout << "体素搜索得到点的个数:" << cloudVox->size() << std::endl;
std::cout << "半径搜索得到点的个数:" << cloudRadius->size() << std::endl;
std::cout << "K邻域搜索得到点的个数:" << cloudK->size() << std::endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewVox(new pcl::visualization::PCLVisualizer("vox"));
viewVox->addPointCloud(cloud, "V0");
viewVox->addPointCloud<pcl::PointXYZ>(cloudVox, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudVox, 0, 0, 255), "V1");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewRadius(new pcl::visualization::PCLVisualizer("Radius"));
viewRadius->addPointCloud(cloud, "R0");
viewRadius->addPointCloud<pcl::PointXYZ>(cloudRadius, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudRadius, 0, 255, 255), "R1");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewK(new pcl::visualization::PCLVisualizer("K"));
viewK->addPointCloud(cloud, "K0");
viewK->addPointCloud<pcl::PointXYZ>(cloudK, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloudK, 255, 0, 255), "K1");
while (!viewVox->wasStopped() || !viewRadius->wasStopped() || !viewK->wasStopped())
{
viewVox->spinOnce(100);
viewRadius->spinOnce(100);
viewK->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
6.结果展示
(1)体素搜索
1)打印信息:
2)可视化结果:红色部分即为搜索结果。
(2)半径搜索:
1)打印信息:
2)可视化结果:蓝色部分即为搜索结果
(3)K邻域搜索
1)打印信息
2)可视化结果:粉色部分即为搜索结果。