PCL库学习笔记（Octree寻找邻近点及其应用）

上文我们利用了kdtree进行邻近点的提取。在点云pcl库中，还有一个重要的点查找方法就是octree。其原理和应用在这篇博文中有了比较详细的介绍：
PCL中八叉树理论
octree有三种寻找最近邻的方式分别为：
1、neighbors within voxel search 就是返回该点所在的voxel中的所有其他点的索引；
2、K nearest Neighbor search 就是返回该点最近的K个点；
3、Neighbors within radius search 就是返回限定半径范围内的所有点；
下面代码只输出了三种情况的前10个点，可以发现其实找的点还是有存在差异的。具体哪种情况用哪一种方式，还是需要具体分析。后期可能会补充三种方式在邻近点寻找上的可视化展示，方便更好理解。

#include<iostream>
#include<pcl/point_cloud.h>
#include<pcl/io/io.h>
#include<pcl/point_types.h>
#include<pcl/octree/octree_search.h>
#include<vector>

using namespace std;

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->height = 100000;
	cloud->width = 1;
	cloud->is_dense = true;
	cloud->resize(cloud->width*cloud->height);
	for (size_t i = 0; i < cloud->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);
	}
	// construct octree
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	// set search point
	pcl::PointXYZ searchPoint;
	searchPoint = cloud->points[10];

	// neighbors within voxel search
	vector<int> Idx1;
	octree.voxelSearch(searchPoint, Idx1);
	cout << "Neighbors within voxel search points:\n";
	for (size_t i = 0; i < 10; i++) {
		cout << cloud->points[Idx1[i]].x << " "
			 << cloud->points[Idx1[i]].y << " "
			 << cloud->points[Idx1[i]].z << endl;
	}

	// K nearest Neighbor search
	int K = Idx1.size();
	vector<int> Idx2;
	vector<float> Distance1;
	octree.nearestKSearch(searchPoint, K, Idx2, Distance1);
	cout << "\nK nearest Neighbor search points:\n";
	for (size_t i = 0; i < 10; i++) {
		cout << cloud->points[Idx2[i]].x << " "
			 << cloud->points[Idx2[i]].y << " "
			 << cloud->points[Idx2[i]].z << endl;
	}

	// Neighbors within radius search
	float radius = resolution;
	vector<int> Idx3;
	vector<float> Distance2;
	octree.radiusSearch(searchPoint, radius, Idx3, Distance2);
	cout << "\nNeighbors within radius search points:\n";
	for (size_t i = 0; i < 10; i++) {
		cout << cloud->points[Idx3[i]].x << " "
			<< cloud->points[Idx3[i]].y << " "
			<< cloud->points[Idx3[i]].z << endl;
	}
}