使用Flann库查找点云中的最近点代码

最新推荐文章于 2025-04-20 16:39:33 发布

该-昵称已被占用

最新推荐文章于 2025-04-20 16:39:33 发布

阅读量3.3k

点赞数 2

分类专栏： PCL 文章标签： PCL

本文链接：https://blog.youkuaiyun.com/u011091739/article/details/80690647

版权

PCL 专栏收录该内容

5 篇文章

订阅专栏

本文介绍如何使用FLANN库替代PCL库中的KdTreeFlann进行点云匹配，通过具体代码示例展示了如何构建索引并查找源点云在目标点云中的最近邻点及其距离。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

之前使用了PCL库中的KdTreeFlann来查找点云中的最近点，但是为了这个功能而去包含PCL库就有点划不来了，而且PCL库中的KdTreeFlann是基于Flann库实现了，所以直接使用Flann库提供的函数实现该功能，代码如下：

void searchNearestPoints(const double * vSourcePoints, int iSourceNum,
	const double * vTargetPoints, int iTargetNum,
	int * vIndices, double * vDists)
{
	// 建立索引
	flann::Matrix<double> data(new double[iTargetNum * 3], iTargetNum, 3);
	for (int i = 0; i < iTargetNum; i++)
	{
		data[i][0] = vTargetPoints[i * 3 + 0];
		data[i][1] = vTargetPoints[i * 3 + 1];
		data[i][2] = vTargetPoints[i * 3 + 2];
	}
	//flann::Index<flann::L2_3D<double> > index(data, flann::LinearIndexParams());
	flann::Index<flann::L2_Simple<double> > index(data, flann::KDTreeSingleIndexParams(15));
	index.buildIndex();
	
	// 查找最近邻
	int knn = 1;
	flann::Matrix<double> p(new double[iSourceNum * 3], iSourceNum, 3);
	for (int i = 0; i < iSourceNum; i++)
	{
		p[i][0] = vSourcePoints[i * 3 + 0];
		p[i][1] = vSourcePoints[i * 3 + 1];
		p[i][2] = vSourcePoints[i * 3 + 2];
	}
	flann::Matrix<int> indices(new int[iSourceNum*knn], iSourceNum, knn);
	flann::Matrix<double> dists(new double[iSourceNum*knn], iSourceNum, knn);
	//index.knnSearch(p, indices, dists, knn, flann::SearchParams(512));
	index.knnSearch(p, indices, dists, knn, flann::SearchParams());

	// 返回结果
	for (int i = 0; i < iSourceNum; ++i)
	{
		vIndices[i] = indices[i][0];
		vDists[i] = dists[i][0];
	}

	// 释放内存
	delete[] data.ptr();
	delete[] p.ptr();
	delete[] indices.ptr();
	delete[] dists.ptr();
}

其中的vSourcePoints和vTargetPoints分别是需要查找的点云和被查找的点云，将其中的每一个顶点的x、y、z值依次放入数组中即可，vIndices和vDists分别是查找得到的source中每个点在target点云中最近的下标及距离。

被注释的两行是根据自己理解设置的参数，但是结果不正确；修改的参数是参考PCL中设置的，结果正确。