pcl kd-tree查找近邻点

Mr.Q

已于 2022-12-26 11:26:31 修改

阅读量1.1k

点赞数 2

分类专栏： pcl 文章标签：算法 c++ pcl

于 2022-12-16 18:28:49 首次发布

本文链接：https://blog.csdn.net/jizhidexiaoming/article/details/128346221

版权

pcl 专栏收录该内容

8 篇文章 7 订阅

订阅专栏

1. 查询最近的k个点

2. 查询所有的在范围r内近邻点

调用区别：

// squared distance
int result = kd_tree.nearestKSearch(search_pt, k, pt_idx_search, pt_sqr_dis);

int num_neighbours = kd_tree.radiusSearch(search_pt, radius, pt_idx_search, pt_sqr_dis);

1. 查询最近的k个点

#include <pcl/point_types.h>
#include <pcl/io/file_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>

/// <summary>
/// 
/// </summary>
/// <param name="kd_tree">查询空间</param>
/// <param name="search_pt">查询点</param>
/// <param name="k">需要获取多少个近邻点</param>
/// <param name="pt_idx_search">近邻点索引</param>
/// <param name="pt_sqr_dis">近邻点到查询点距离</param>
bool find_k_nearest_neigbour(pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree, pcl::PointXYZ search_pt, int k, std::vector<int>& pt_idx_search, std::vector<float>& pt_sqr_dis)
{
	// 执行k近邻搜索
	int result = kd_tree.nearestKSearch(search_pt, k, pt_idx_search, pt_sqr_dis);
	return result;
}

main函数调用

int
main(int argc, char** argv)
{
	srand(time(NULL));  // 系统时间初始化随机种子，每次运行随机数都不同。
	// 1,随机点云生成
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 1000;
	cloud->height = 1;
	cloud->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,创建kd-tree对象，并把上面的点云设置成输入，然后创建一个查询点，查询该点
	pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;
	kd_tree.setInputCloud(cloud);

	// 3,创建一个查询点
	pcl::PointXYZ search_pt;
	search_pt.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_pt.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_pt.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	// 4,k近邻搜索：在距离查询点最近的k个点中，某个类别最多，则该查询点就属于对应类别。
	// 这里是找到k个近邻点
	int k = 10;
	std::vector<int> pt_idx_search(k);  // 存储查询到的最近邻的k个点的索引
	std::vector<float> pt_sqr_dis(k);  //  存储近邻点对应的距离平方

	// 执行k近邻搜索
	if (find_k_nearest_neigbour(kd_tree, search_pt, k, pt_idx_search, pt_sqr_dis) > 0)
	{
		for (size_t i = 0; i < pt_idx_search.size(); i++)  // 打印所有的近邻点坐标和距离
		{
			std::cout << "(" << cloud->points[pt_idx_search[i]].x << ","
				<< cloud->points[pt_idx_search[i]].y << ","
				<< cloud->points[pt_idx_search[i]].z << ")"
				<< "dis: " << pt_sqr_dis[i] << std::endl;
		}

	}
}

测试结果

(578.312,56.0312,157.75)dis: 6764.52
(551.562,168.469,145.031)dis: 8353.54
(605.688,56.9375,136.469)dis: 10081.2
(559.75,148.344,201.125)dis: 11910.9
(582.188,148.719,199.844)dis: 14421.3
(396.375,66.0312,46.6562)dis: 19644.3
(445.938,20.0312,17)dis: 20342.3
(503.281,232.781,100.438)dis: 21170.4
(377.281,151.031,98.6875)dis: 22444
(385.688,113.906,207.469)dis: 23593.7

2. 查询所有的在范围r内近邻点

#include <pcl/point_types.h>
#include <pcl/io/file_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <iostream>

/// <summary>
/// 
/// </summary>
/// <param name="kd_tree">查询空间</param>
/// <param name="search_pt">查询点</param>
/// <param name="radius">查询半径</param>
/// <param name="pt_idx_search">近邻点索引</param>
/// <param name="pt_sqr_dis">近邻点到查询点距离</param>
/// <returns>范围r内临近点的个数</returns>
bool find_neigbour_radius(pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree, pcl::PointXYZ search_pt, float radius, std::vector<int>& pt_idx_search, std::vector<float>& pt_sqr_dis) {
	int num_neighbours = kd_tree.radiusSearch(search_pt, radius, pt_idx_search, pt_sqr_dis);
	return num_neighbours;
}

main函数调用

int
main(int argc, char** argv)
{
	srand(time(NULL));  // 系统时间初始化随机种子，每次运行随机数都不同。
	// 1,随机点云生成
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 1000;
	cloud->height = 1;
	cloud->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,创建kd-tree对象，并把上面的点云设置成输入，然后创建一个查询点，查询该点
	pcl::KdTreeFLANN<pcl::PointXYZ> kd_tree;
	kd_tree.setInputCloud(cloud);

	// 3,创建一个查询点
	pcl::PointXYZ search_pt;
	search_pt.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_pt.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_pt.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	// 4, 查询所有的在半径r内邻居点
	std::vector<int> pt_idx_search;  // 存储查询到的最近邻的k个点的索引
	std::vector<float> pt_sqr_dis;  //  存储近邻点对应的距离平方
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);

	// 执行k近邻搜索
	int num_neighbours = find_neigbour_radius(kd_tree, search_pt, radius, pt_idx_search, pt_sqr_dis);
	for (size_t i = 0; i < pt_idx_search.size(); i++)  // 打印所有的近邻点坐标和距离
	{
		std::cout << "(" << cloud->points[pt_idx_search[i]].x << ","
			<< cloud->points[pt_idx_search[i]].y << ","
			<< cloud->points[pt_idx_search[i]].z << ")"
			<< "dis: " << pt_sqr_dis[i] << "; radius: " << radius << std::endl;
	}


}

测试结果

从小到大排序的

(573.875,514.156,621.062)dis: 1433.16; radius: 173.664
(600.312,517.094,621.469)dis: 3102.49; radius: 173.664
(561.219,560.25,566.688)dis: 3874.85; radius: 173.664
(567.406,568.25,598.75)dis: 4543.86; radius: 173.664
(612.5,471.656,620.781)dis: 4968.97; radius: 173.664
(556.438,539.219,497.094)dis: 9823.76; radius: 173.664
(580.406,601.562,581.281)dis: 10442; radius: 173.664
(496.75,420.906,545.562)dis: 12204.7; radius: 173.664
(457.156,548.031,627.531)dis: 13641.8; radius: 173.664
(591.875,440.312,697.438)dis: 16766.9; radius: 173.664
(472.094,618.031,569.781)dis: 21064.1; radius: 173.664
(568.25,385.938,498.938)dis: 21773.9; radius: 173.664
(522.969,631.469,660.625)dis: 23003; radius: 173.664
(583.406,643.906,538.062)dis: 23329.8; radius: 173.664
(558.594,363.75,653.406)dis: 23332.2; radius: 173.664
(403.875,541.781,536.125)dis: 27963.3; radius: 173.664