原理的相关介绍:KD-Tree详解: 从原理到编程实现
k-d树(k-dimensional树的简称),是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索(如:范围搜索和最近邻搜索)。K-D树是二进制空间分割树的特殊的情况。
索引结构中相似性查询有两种基本的方式:一种是范围查询(range searches),另一种是K近邻查询(K-neighbor searches)。范围查询就是给定查询点和查询距离的阈值,从数据集中找出所有与查询点距离小于阈值的数据;K近邻查询是给定查询点及正整数K,从数据集中找到距离查询点最近的K个数据,当K=1时,就是最近邻查询(nearest neighbor searches)。
PCL中类pcl::KdTree<PointT>
是kd-tree数据结构的实现。并且提供基于FLANN进行快速搜索的一些相关子类与包装类。具体可以参考相应的API。下面给出2个类的具体用法。
- pcl::search::KdTree < PointT >
pcl::search::KdTree<PointT>
是pcl::search::Search< PointT >
的子类,是pcl::KdTree<PointT>
的包装类。包含(1) k 近邻搜索;(2) 邻域半径搜索。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h> // 包含kdtree头文件
typedef pcl::PointXYZ PointT;
int main()
{
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile("../input_cloud/20230605_132512.pcd", *cloud);
// 定义KDTree对象
pcl::search::KdTree<PointT>::Ptr kdtree(new pcl::search::KdTree<PointT>);
kdtree->setInputCloud(cloud); // 设置要搜索的点云,建立KDTree
std::vector<int> indices; // 存储查询近邻点索引
std::vector<float> distances; // 存储近邻点对应距离的平方
PointT point = cloud->points[0]; // 初始化一个查询点
// 查询距point最近的k个点
int k = 10;
int size = kdtree->nearestKSearch(point, k, indices, distances);
std::cout << "nearest search point : " << size << std::endl;
// 查询point半径为radius邻域球内的点
double radius = 2.0;
size = kdtree->radiusSearch(point, radius, indices, distances);
std::cout << "radius search point : " << size << std::endl;
system("pause");
return 0;
}
注意: 搜索结果默认是按照距离point点的距离从近到远排序;如果InputCloud中含有point点,搜索结果的的第一个点是point本身。
- pcl::KdTreeFLANN < PointT >
pcl::KdTreeFLANN<PointT>
是pcl::KdTree<PointT>
的子类,可以实现同样的功能。
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 包含相关头文件
#include <pcl/kdtree/kdtree_flann.h>
typedef pcl::PointXYZ PointT;
int main()
{
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile("../input_cloud/20230605_132833.pcd", *cloud);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; //创建KDtree
kdtree.setInputCloud(cloud); // 设置要搜索的点云,建立KDTree
std::vector<int> indices; // 存储查询近邻点索引
std::vector<float> distances; // 存储近邻点对应距离的平方
PointT point = cloud->points[1]; // 初始化一个查询点
// 查询距point最近的k个点
int k = 10;
int size = kdtree.nearestKSearch(point, k, indices, distances);
std::cout << "nearest search point : " << size << std::endl;
// 查询point半径为radius邻域球内的点
double radius = 2.0;
size = kdtree.radiusSearch(point, radius, indices, distances);
std::cout << "radius search point : " << size << std::endl;
system("pause");
return 0;
}
srand(time(NULL));
srand(time(NULL));是一个常用的用于设置随机数种子的代码片段。它使用当前时间作为种子,以便每次程序运行时生成不同的随机数序列。
要使用srand()
函数生成伪随机数,需要包含头文件<cstdlib>
和<ctime>
。
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
pcl::search::KdTree<pcl::PointXYZ> kdtree;
这两行代码都是使用了 PCL(Point Cloud Library)中的 KdTree 数据结构,但有一些细微的区别:
-
类型声明:
第一行代码使用了pcl::KdTreeFLANN<pcl::PointXYZ>
类型,该类型实现了基于FLANN库的 KdTree,用于点云的最近邻搜索等操作。
第二行代码使用了pcl::search::KdTree<pcl::PointXYZ>
类型,该类型是 PCL 中的另一个 KdTree 实现,用于点云的最近邻搜索和半径搜索等操作。 - 实现差异:
pcl::KdTreeFLANN
是基于 FLANN(Fast Library for Approximate Nearest Neighbors)库实现的 KdTree。FLANN 库提供了高效的最近邻搜索算法,支持多种数据类型和距离度量方式,并且对大规模数据集具有良好的扩展性。
pcl::search::KdTree
是在 PCL 中自定义的 KdTree 实现,它也支持最近邻搜索和半径搜索等操作,但可能不如 FLANN 库提供的实现在某些情况下高效。
因此,选择使用哪个 KdTree 类型取决于你的需求以及对性能和功能的要求。如果你需要更高效和灵活的最近邻搜索算法,可以选择第一行代码中的 pcl::KdTreeFLANN
类型;如果你对性能要求不高,或更偏向于使用 PCL 默认的实现,可以选择第二行代码中的 pcl::search::KdTree
类型。
if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
在Kd树(K-dimensional tree)中,nearestKSearch()
函数是用于搜索K个最近邻的函数。它可以帮助我们找到给定查询点附近最接近的K个点。
nearestKSearch()
函数通常具有以下参数:
query_point
: 要查询的点,它是一个K维向量或数组。k
: 要搜索的最近邻数量,也就是要返回的最接近的K个点。result
: 用于存储搜索结果的容器,通常是一个空的列表或数组。distances
: 用于存储每个最近邻点与查询点之间的距离的容器,通常也是一个空的列表或数组。
size_t i = 0;
size_t
是C++中的一种无符号整数类型,被用于表示内存大小、数组长度等非负整数值。它的大小在不同的编译器和操作系统下可能会有所不同,但通常被设计为足够大以容纳目标平台上最大可能的对象大小。
在C++中,size_t
类型通常被用作循环计数器、数组索引和指针算术等场景中。它可以保证能够表示当前平台上最大可能的对象大小,并且不受特定平台的位数限制(如32位或64位)的影响。
为什么循环计数时用size_t而不是int?
在循环计数时,使用size_t
类型比使用int
类型更好的原因是因为:
-
无符号性:
size_t
是一种无符号整数类型,它只表示非负的整数值。这意味着你不需要担心负数循环计数的问题。当循环计数器必须始终是非负数时,使用size_t能
提供更好的语义。 -
内存大小:
size_t
的大小在不同平台上可能会有所变化,但通常情况下,它足够大以容纳较大的数组或容器的元素数量。相比之下,int
类型的大小是固定的,在大多数平台上,int
类型的大小是4个字节(32位),其范围通常为-2,147,483,648到2,147,483,647(即-2^31到2^31-1)。并且可能无法容纳大型数据结构的元素数量。 -
兼容性:某些库或函数接口中使用了
size_t
类型来表示长度、索引或容量。如果你在循环计数中使用int
而不是size_t
,可能会导致类型不匹配的错误或警告。
总体而言,使用size_t
类型进行循环计数可以提供更好的代码清晰性、可移植性和兼容性,特别是当处理大型数据结构时。然而,在某些情况下,使用int
类型也是可以接受的,特别是当循环计数器的范围明确限制在整数范围内时。
viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");
在函数viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud")
中,这三个参数的含义如下:
-
searchCloud
: 这是要添加到可视化器中的点云对象。它是一个指向pcl::PointCloud<pcl::PointXYZ>
类型的指针。 -
searchColorHandler
: 这是颜色处理器,用于设置点云的颜色。它是pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
类型的对象。 -
"search_cloud"
: 这是点云对象的名称。可视化器使用该名称来标识和管理不同的点云。
viewer.addCoordinateSystem(200);
viewer.addCoordinateSystem(200);
是PCL中的一个函数调用,用于在可视化器中添加一个坐标系。
这个函数有几个重载版本,参数200
表示坐标系的尺寸大小(单位为mm)。它指定了坐标系的轴的长度,即x、y和z轴的长度。
// K nearest neighbor search 搜索K个最近邻居
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv) {
// 用系统时间初始化随机种子
srand(time(NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 生成点云数据1000个
cloud->width = 1000;
cloud->height = 1; // 1 表示点云为无序点云
cloud->points.resize(cloud->width * cloud->height);
// 给点云填充数据 0 - 1023
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);
}
// 创建KdTree的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
//pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
pcl::search::KdTree<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud); // 设置搜索空间,把cloud作为输入
pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointXYZ searchPoint(300, 100, 50);
// 初始化一个随机的点,作为查询点
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);
searchCloud->push_back(searchPoint);
//创建K和两个向量来保存搜索到的数据
int K = 10; //表示搜索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;
if (kdtree.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
<< " (距离: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originColorHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchColorHandler(searchCloud, 0, 255, 0);
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.3, 0.2, 0.5);
viewer.addPointCloud<pcl::PointXYZ>(cloud, originColorHandler,"cloud");
viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
viewer.addLine(originPoint, searchPoint, "line"); // 添加从原点到搜索点的线段
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.9373, 1, 0.3216, "line");
viewer.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
}
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
在Kd树(K-dimensional tree)中,radiusSearch()
函数是用于搜索指定半径内的点的函数。它可以帮助我们找到给定查询点附近指定半径范围内的所有点。
radiusSearch()
函数通常具有以下参数:
query_point
: 要查询的点,它是一个K维向量或数组。radius
: 搜索的半径范围,只返回距离查询点小于等于半径的点。result
: 用于存储搜索结果的容器,通常是一个空的列表或数组。distances
: 用于存储每个点与查询点之间的距离的容器,通常也是一个空的列表或数组。
返回值表示在指定半径范围内找到的点的数量。这个返回值是一个整数,表示满足条件的点的数量。
// Neighbors within radius search 通过指定半径搜索
#include <iostream>
#include <vector>
#include <ctime>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char** argv) {
srand(time(NULL)); // 用系统时间初始化随机种子
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 生成点云数据1000个
cloud->width = 1000;
cloud->height = 1; // 1 表示点云为无序点云
cloud->points.resize(cloud->width * cloud->height);
// 给点云填充数据 0 - 1023
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);
}
// 创建KdTree的实现类KdTreeFLANN (Fast Library for Approximate Nearest Neighbor)
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//pcl::search::KdTree<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud); // 设置搜索空间,把cloud作为输入
pcl::PointCloud<pcl::PointXYZ>::Ptr searchCloud(new pcl::PointCloud<pcl::PointXYZ>);
// 初始化一个随机的点,作为查询点
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);
searchCloud->push_back(searchPoint);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// 创建一个随机[0,50)的半径值
float radius = 50.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 (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
<< " (距离平方:: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> originColorHandler(cloud, 255, 255, 255);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> searchColorHandler(searchCloud, 0, 255, 0);
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
viewer.setBackgroundColor(0.1176, 0.1176, 0.2353);
viewer.addPointCloud<pcl::PointXYZ>(cloud, originColorHandler, "cloud");
viewer.addPointCloud<pcl::PointXYZ>(searchCloud, searchColorHandler, "search_cloud");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "search_cloud");
pcl::PointXYZ originPoint(0.0, 0.0, 0.0);
viewer.addLine(originPoint, searchPoint, "line"); // 添加从原点到搜索点的线段
viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0.2784, "line");
viewer.addSphere(searchPoint, radius, 0.945, 0.4118, 0.6941,"sphere",0); // 添加一个以搜索点为圆心,搜索半径为半径的球体229
viewer.addCoordinateSystem(200); // 指定坐标轴的长度,单位mm
while (!viewer.wasStopped()) {
viewer.spinOnce();
}
return 0;
}