3.Octree相关

1.Octree理论：

八叉树（Octree）是一种用于描述三维空间的树状数据结构。八叉树的每个节点表示一个正方体的体积元素，每个节点有八个子节点，这八个子节点所表示的体积元素加在一起就等于父节点的体积。

• 一般中心点作为节点的分叉中心。
• 八叉树若不为空树的话，树中任一节点的子节点恰好只会有八个，或零个，也就是子节点不会有0与8以外的数目。
• 分割一直要进行到节点所对应的立方体或是完全空白，或是完全为V占据，或是其大小已是预先定义的体素大小，并且对它与V之交作一定的“舍入”，使体素或认为是空白的，或认为是V占据的。

实现八叉树的流程

　　(1). 设定最大递归深度。

　　(2). 找出场景的最大尺寸，并以此尺寸建立第一个立方体。

　　(3). 依序将单位元元素丢入能被包含且没有子节点的立方体。

　　(4). 若没达到最大递归深度，就进行细分八等份，再将该立方体所装的单位元元素全部分担给八个子立方体。

　　(5). 若发现子立方体所分配到的单位元元素数量不为零且跟父立方体是一样的，则该子立方体停止细分，因为跟据空间分割理论，细分的空间所得到的分配必定较少，若是一样数目，则再怎么切数目还是一样，会造成无穷切割的情形。

　　(6). 重复3，直到达到最大递归深度。

2、PCL中的相关应用：

2.1点云压缩

点云可以以非常高的速度被创建出来，因此需要占用相当大的存储资源，一旦点云需要存储或者通过速率受限制的通信信道进行传输，提供针对这种数据的压缩方法就变得十分有用，PCL 提供了点云的压缩功能，它允许编码压缩所有类型的点云。

#pragma once
#include<pcl/kdtree/kdtree_flann.h>
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/compression/octree_pointcloud_compression.h>
#include<pcl/io/openni2_grabber.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<sstream>
class SimpleOpenNIViewer
{
public:
	pcl::io::OctreePointCloudCompression<pcl::PointXYZ>*encoder;//压缩编码
	pcl::io::OctreePointCloudCompression<pcl::PointXYZ>*decoder;//解压缩解码

	pcl::visualization::CloudViewer viewer;

	SimpleOpenNIViewer() : viewer("cloud_compression")
	{

	}

	//回调函数
	void cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
	{
		if (!viewer.wasStopped())
		{
			//存储待压缩流
			stringstream need_compression_data;
			//存储输出点云
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

			//开始压缩
			encoder->encodePointCloud(cloud, need_compression_data);

			//解压缩
			decoder->decodePointCloud(need_compression_data, cloud_out);

			//可视化
			viewer.showCloud(cloud_out);
		}
	}

	void run()
	{
		//设置压缩选项
		bool showStatistics = true;

		pcl::io::compression_Profiles_e compressionFile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR;
		//初始化成员encoder,decoder
		encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>(compressionFile, showStatistics);
		decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZ>();

		//创建从OpenNI设备抓取点云的对象
		pcl::Grabber* myInterface = new pcl::io::OpenNI2Grabber();

		//建立回调函数
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f=
			bind(&SimpleOpenNIViewer::cloud_cb_, this,_1);

		//建立回调函数和回调信号连接
		boost::signals2::connection c = myInterface->registerCallback(f);

		//开始接受点云数据
		myInterface->start();

		while (!viewer.wasStopped())
		{
			Sleep(1);
		}

		myInterface->stop();

		delete decoder;
		delete encoder;


	}

};


int main()
{
    SimpleOpenNIViewer item;
    item.run();
    return 0;
}

2.用八叉树进行空间分区和搜索

octree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance)

K近邻搜索方法把搜索结果写到两个分开的向量

• 第一个pointIdxNKNSearch包含搜索结果   （结果点的索引的向量）  
• 第二个向量pointNKNSquaredDistance存储搜索点与近邻之间的距离的平方

分别实现

• “体素内近邻搜索（Neighbors within VOxel Search）”
• "K近邻搜索（K Nearest Neighbor Search）"
• "半径内近邻搜索"（Neighbors within Radius Search）

#pragma once
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/octree/octree.h>
#include<vector>
//体素内近邻搜索
void octree_search()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// 创建点云数据
	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);
	}
	//pcl::io::loadPCDFile("ajaccio_2 - Cloud.pcd",*cloud);
	
	//创建八叉树，用设置的分辨率初始化
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);

	octree.setInputCloud(cloud);
	//构建八叉树
	octree.addPointsFromInputCloud();

	//随机生成一个点
	pcl::PointXYZ search_point;
	search_point.x = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_point.y = 1024.0f * rand() / (RAND_MAX + 1.0f);
	search_point.z = 1024.0f * rand() / (RAND_MAX + 1.0f);

	//体素近邻搜索
	std::vector<int>index_vexel;
	if (octree.voxelSearch(search_point, index_vexel))
	{
		for (size_t i = 0; i < index_vexel.size(); ++i)
		{
			std::cout << "    " << cloud->points[index_vexel[i]].x
			<< " " << cloud->points[index_vexel[i]].y
			<< " " << cloud->points[index_vexel[i]].z << std::endl;
		}
	}


	//k近邻搜索
	int k = 10;
	std::vector<int>index_knn;
	std::vector<float>distance_knn;

	if (octree.nearestKSearch(search_point, k, index_knn, distance_knn) > 0)
	{
		for (size_t i = 0; i < index_knn.size(); ++i)
		{	std::cout << "    " << cloud->points[index_knn[i]].x
			<< " " << cloud->points[index_knn[i]].y
			<< " " << cloud->points[index_knn[i]].z
			<< " (squared distance: " << distance_knn[i] << ")" << std::endl;
		}
	}

	//半径内近邻搜索
	std::vector<int>index_radius;
	std::vector<float>Distance_radius;
	float radius = 256.0f * rand() / (RAND_MAX + 1.0f);
	
	if (octree.radiusSearch(search_point, radius, index_radius, Distance_radius) > 0)
	{
		for (size_t i = 0; i < index_radius.size(); ++i)
			std::cout << "    " << cloud->points[index_radius[i]].x
			<< " " << cloud->points[index_radius[i]].y
			<< " " << cloud->points[index_radius[i]].z
			<< " (squared distance: " << Distance_radius[i] << ")" << std::endl;
	}

}

3.无序点云的空间变化检测

#pragma once
#include<pcl/point_types.h>
#include<pcl/io/pcd_io.h>
#include<pcl/octree/octree.h>
#include<pcl/point_cloud.h>
#include<vector>
#include<ctime>
//无序点云数据集空间变化

void octree_change_detection()
{
	srand(unsigned int(NULL));
	//八叉树分辨率
	float resolution = 32.0f;
	//初始化空间变化检测对象
	pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree_det(resolution);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
	//为cloudA创建点云
	cloudA->width = 128;
	cloudA->height = 1;
	cloudA->points.resize(cloudA->width * cloudA->height);
	for (size_t i = 0; i < cloudA->points.size(); ++i)
	{
		cloudA->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
		cloudA->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
		cloudA->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
	}
	//添加点云到八叉树
	octree_det.setInputCloud(cloudA);
	octree_det.addPointsFromInputCloud();

	//交换八叉树缓存，cloudA仍在内存
	octree_det.switchBuffers();

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
	// 为cloudB创建点云
	cloudB->width = 128;
	cloudB->height = 1;
	cloudB->points.resize(cloudB->width * cloudB->height);
	for (size_t i = 0; i < cloudB->points.size(); ++i)
	{
		cloudB->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
		cloudB->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
		cloudB->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
	}
	//将cloudB添加到八叉树
	octree_det.setInputCloud(cloudB);
	octree_det.addPointsFromInputCloud();

	//获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
	std::vector<int>new_point_index;
	octree_det.getPointIndicesFromNewVoxels(new_point_index);

	//打印输出点
	std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
	for (size_t i = 0; i < new_point_index.size(); ++i)
		std::cout << i << "# Index:" << new_point_index[i]
		<< "  Point:" << cloudB->points[new_point_index[i]].x << " "
		<< cloudB->points[new_point_index[i]].y << " "
		<< cloudB->points[new_point_index[i]].z << std::endl;

}