2 PCL-KDtree

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
#include <ctime>

using namespace pcl;
using namespace std;

int main(int argc, char** argv)
{
	srand(time(NULL));
	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>); // cloud 指针
	//点云生成 随机生成
	cloud->width = 1000;
	cloud->height = 1;
	cloud->points.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);
	}

	// k-d tree
	KdTreeFLANN<PointXYZ>kdtree;
	kdtree.setInputCloud(cloud); //指定点云,将cloud点云放在里面，kdtree进行cloud点云的构建

	// 生成随机点
	PointXYZ searchPoint;  
	//PointXYZ searchPoint; 这一行代码在编程中通常表示声明了一个名为 searchPoint 的变量，
	// 该变量的类型为 PointXYZ
	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);

	// k近邻搜索
	int K = 10;//搜索10个点
	std::vector<int>pointIdxNKNSearch(K);	// 一个向量，用于存储返回的最近邻居的索引，对点云cloud的索引值
	std::vector<float>pointNKNSquaredDistance(K);// 一个向量，用于存储每个最近邻居到查询点的均方根
	std::cout << "K nearest neighbor search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with K=" << K << std::endl;

	// kd tree 搜索 nearestKSearch
	if (kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
	//kdtree.nearestKSearch得到的结果是一个整数——即如果找到了至少一个邻近点（即k>0）
	//将searchpoint的这一个点进行K=10周围的索引，存放在pointidnknsearch上，计算均方根
	{
		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
			<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
	}


	// 在半径r内搜索近邻
 	std::vector<int> pointIdxRadiusSearch;               // 一个向量，用于存储返回的最近邻居的索引
	std::vector<float> pointRadiusSquaredDistance;       //存放半径均放值
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	std::cout << "Neighbors within radius search at (" << searchPoint.x
		<< " " << searchPoint.y
		<< " " << searchPoint.z
		<< ") with radius=" << radius << std::endl;

	// kd tree 搜索 radiusSearch
	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
			<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
	}
	return 0;
	system("pause");
}

• "size" 是一个属性，用于获取容器中元素的数量。
• "resize" 是一个操作，用于改变容器的大小，可能会增加或减少元素数量。
• 当需要表示整数并且不关心其符号时，int 通常是一个很好的选择。但是，如果整数用于表示大小或索引（如数组索引或内存大小），则 size_t 是更好的选择，因为它可以保证足够大以表示任何大小或索引，并且是无符号的，这可以避免一些意外的符号扩展问题
• 随机生成的点云：前者随机生成的是1000个点云+随机生成的对应坐标，而后者的searchpoint生成的随机点云是从这1000个里面随机生成的一个点云+随机生成的对应坐标

3.1  定义

KD-tree 又称 K 维树,是计算机科学中使用的一种数据结构，用来组织表示 K 维空间中点集合。它是一种带有其他约束条件的二分查找树。KD-tree对于区间和近邻搜索十分有用。我们为了达到目的，通常只在三个维度中进行处理，因此所有的 KD-tree 都将是三维 KD-tree。 如下图所示， KD-tree 的每一级在指定维度上分开所有的子节点。在树的根部所有子节点是以第一个指定的维度上被分开（即如果第一维坐标小于根节点的点它将被分在左边的子树中，如果大于根节点的点它将分在右边的子树中）。