PCL学习笔记——利用kdtree计算点云点距均值

template

int pcl::search::KdTree< PointT, Tree >::nearestKSearch (
const PointT & point,
int k,
std::vector< int > & k_indices,
std::vector< float > & k_sqr_distances
) const

Search for the k-nearest neighbors for the given query point.

搜索给定查询点邻域周围最近的K个点

Parameters：

[in] p_q the given query point
[in] k the number of neighbors to search for
[out] k_indices the resultant indices of the neighboring points
[out] k_sqr_distances the resultant squared distances to the neighboring points

Returns：

number of neighbors found

Code:

// PointCloudsDis.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include "pch.h"
#include 
#include
#include 
#include 
#include 
#include 
#include 
#include

using namespace std;

double computerCloudResolution(const pcl::PointCloud::ConstPtr cloud) {
	double res = 0.0;
	int n_points = 0;
	int nres;
	std::vector indices(2);
	std::vector sqr_distances(2);
	pcl::search::KdTree tree;
	tree.setInputCloud(cloud);

	for (size_t i = 0; i < cloud->size(); ++i)
	{
		if (!std::isfinite((*cloud)[i].x))
		{
			continue;
		}
		nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
		if (nres == 2)
		{
			res += sqrt(sqr_distances[1]);
			++n_points;
		}
	}
	if (n_points != 0)
	{
		res /= n_points;
	}
	return res;
}

int main()
{
	ifstream data;
	data.open("PointCloud3D01.TXT",ios::in);

	int rows = -1;
	char buf[200];
	while (!data.eof()) {
		data.getline(buf, sizeof(buf));
		rows++;
	}
	data.clear();
	data.seekg(0, ios::beg);
	//pcl::PointCloudcloud;
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	cloud->width = rows;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->resize(cloud->width*cloud->height);
	
	for (int i = 0; i < rows; i++) {
		double num[3];
		data >> num[0];
		data >> num[1];
		data >> num[2];

		cloud->points[i].x = num[0];
		cloud->points[i].y = num[1];
		cloud->points[i].z = num[2];
	}

	double res=computerCloudResolution(cloud);
	cout << "distance=" <

Result：