1.kd-tree、八叉树简介

1）kd-tree, 或称 k 维树,是计算机科学中使用的一种数据结构,用来组织表示 k 维空间 中的点集合。一般在点云处理中都是应用到三维的kd-tree。PCL 中 k-d tree 库提供了 k-d tree 数据结构,基于 FLANN 进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、邻域特征提取中是非常基础的核心操作。 kd-tree 模块利用 两个类与两个函数实现了利用 k-d tree 数据结构对点云的高效管理和检索,其依赖于 pcl_ common 模块。
2）八叉树结构是 Hunter 博士于 1978 年首次提出的一种数据模型,如图 4.2 所示。八叉树)结构通&过循环递归的划分方法对大小为 2nx2nx2n 的三维空间的几何对象进行剖分,从而 构成一个具有根节点的方向图。

2.快速邻域搜索

利用kd-tree实现点云的k近邻搜索（当k为1是就是最近邻点）以及按照搜索点为中心的半径距离进行搜索。
1）k近邻搜索

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h> //kd-tree依赖的头文件
#include <iostream>
#include <vector>
#include <ctime>

int main (int argc, char**argv)
{
srand (time (NULL));
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);
  }
 //建立kd-tree
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
//输入点云
kdtree.setInputCloud (cloud);
//设定搜索的中心点
pcl::PointXYZ searchPoint;
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;
std::vector<int>pointIdxNKNSearch(K);//存储k个最近邻点的索引
std::vector<float>pointNKNSquaredDistance(K);//存储最近邻点的距离平方
//打印
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0 )//执行搜索，搜索到后返回值大于0
  {
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;
  }
 return 0;
}

2）按照搜索半径进行搜索

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

int main (int argc, char**argv)
{
srand (time (NULL));
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;
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;
}

3.点云压缩

点云由庞大的数据集组成,这些数据集通过距离、颜色、法线等附加信息来描述空间三维点。此外,点云能以非常高的速率被创建出来,因此需要占用相当大的存储资源,一旦点云需要存储或者通过速率受限制的通信信道进行传输,提供针对这种数据的压缩方法就变得 十分有用。 PCL 库提供了点云压缩功能,它允许编码压缩所有类型的点云,包括“无序” 点云,它具有无参考点和变化的点尺寸、分辨率、分布密度和点顺序等结构特征。而且,底层的八叉树数据结构允许从几个输入源高效地合并点云数据。
参考

下面是我们可以使用的一些压缩文件：
- LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率1立方厘米，压缩完之后无颜色，快速在线编码
- LOW_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率1立方厘米，压缩完之后有颜色，快速在线编码
- MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率5立方毫米，压缩完之后无颜色，快速在线编码
- MED_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率5立方毫米，压缩完之后有颜色，快速在线编码
- HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR：分辨率1立方毫米，压缩完之后无颜色，快速在线编码
- HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR：分辨率1立方毫米，压缩完之后有颜色，快速在线编
- LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率1立方厘米，压缩完之后无颜色，高效离线编码
- LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率1立方厘米，压缩完之后有颜色，高效离线编码
- MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率5立方毫米，压缩完之后无颜色，高效离线编码
- MED_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率5立方毫米，压缩完之后有颜色，高效离线编码
- HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR：分辨率1立方毫米，压缩完之后无颜色，高效离线编码
- HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR：分辨率1立方毫米，压缩完之后有颜色，高效离线编码

#include <pcl/compression/octree_pointcloud_compression.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>

#include <iostream>

int main()
{
    // 加载点云
    pcl::PointCloud<pcl::PointXYZRGB> sourceCloud;
    if (pcl::io::loadPCDFile("coloredBox.pcd", sourceCloud) == -1)
        return -1;

    // 是否查看压缩信息
    bool showStatistics = true;
    // 配置文件，如果想看配置文件的详细内容，可以参考: /io/include/pcl/compression/compression_profiles.h
    pcl::io::compression_Profiles_e compressionProfile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

    // 初始化点云压缩器和解压器
    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>* PointCloudEncoder;
    PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>(compressionProfile, showStatistics);
    PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>(compressionProfile,true,0.002);



    // 压缩结果stringstream
    std::stringstream compressedData;
    // 输出点云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZRGB>());
    // 压缩点云
    PointCloudEncoder->encodePointCloud(sourceCloud.makeShared(), compressedData);
    std::cout << compressedData.str() << std::endl;
    // 解压点云
    PointCloudEncoder->decodePointCloud(compressedData, cloudOut);
    pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
    viewer.showCloud(cloudOut);
    while (!viewer.wasStopped())
    {
    }

    std::system("pause");
    return 0;
}

完全自定义压缩参数
为了能完全控制压缩相关的参数, PointCloudCompression 类的构造函数可以在初始化时附加压缩参数。请注意,为了能够进行高级参数化, compressionProfile_arg 参数需要被设置成MANUAL_CONRGURATION。

OctreePointCloudCompression (compression_Profiles_e compressionProfile_arg = MED_RES_ONLINE_COMPRESSION_WITH_COLOR,
                               bool showStatistics_arg = false,
                               const double pointResolution_arg = 0.001,
                               const double octreeResolution_arg = 0.01,
                               bool doVoxelGridDownDownSampling_arg = false,
                               const unsigned int iFrameRate_arg = 30,
                               bool doColorEncoding_arg = true,
                               const unsigned char colorBitResolution_arg = 6) :

compressionProfile_arg：配置文件，若要启用高级参数化应设置为MANUAL_CONFIGURATION
showStatistics_arg：是否将压缩相关的统计信息打印到标准输出上
octreeResolution_arg：八叉树分辨率
pointResolution_arg：定义点坐标的编码精度，该参数应设为小于传感器精度的一个值
doVoxelGridDownDownSampling_arg：是否进行体素下采样（每个体素内只留下体素中心一个点）
iFrameRate_arg：点云压缩模式对点云进行差分编码压缩，用这种方法对新引入的点云和之前编码的点云之间的差分进行编码，以便获得最大压缩性能，iFrameRate_arg指定了一个速率，如果数据流中的帧速率低于这个速率则不进行差分编码压缩
doColorEncoding_arg：是否对彩色编码压缩
colorBitResolution_arg：定义每一个彩色成分编码后所占的位数

4.基于八叉树的点云搜索和空间划分

八叉树是一种用于管理稀疏 3D 数据的树状数据结构,每个内部节点都正好有八个子节点。
分别介绍基于八叉树的体素内搜索、k近邻搜索和半径内近邻搜索

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

int
main (int argc, char**argv)
{
srand ((unsigned int) time (NULL));
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);
  }

  //构建八叉树
float resolution =128.0f;//设置八叉树分辨率，描述的是最低一级八叉树的最小体素尺寸
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
octree.setInputCloud (cloud);//设置输入点云
octree.addPointsFromInputCloud ();//载入点云
//随机生成搜索点
pcl::PointXYZ searchPoint;
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);
// 体素最近邻搜索：搜索查询点所在的体素中的其他点，返回结果为其他点的索引向量。查询点和被搜索点之间的距离取决于八叉树的分辨率
std::vector<int>pointIdxVec;//存储搜索结果索引
if (octree.voxelSearch (searchPoint, pointIdxVec))//执行搜索
  {
std::cout<<"Neighbors within voxel search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z<<")"
<<std::endl;
for (size_t i=0; i<pointIdxVec.size (); ++i)
std::cout<<"    "<< cloud->points[pointIdxVec[i]].x 
<<" "<< cloud->points[pointIdxVec[i]].y 
<<" "<< cloud->points[pointIdxVec[i]].z <<std::endl;
  }
//K近邻搜索
int K =10;
std::vector<int>pointIdxNKNSearch;//保存索引
std::vector<float>pointNKNSquaredDistance;//保存距离平方值
std::cout<<"K nearest neighbor search at ("<<searchPoint.x
<<" "<<searchPoint.y
<<" "<<searchPoint.z
<<") with K="<< K <<std::endl;
if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) >0)
  {
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;
  }
//半径内最近邻搜索
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;
if (octree.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;
  }
}

八叉树部分类关键点说明
PCLoctree 组件提供了几个八叉树类型。它们各自的叶节点特征基本上是不同的。
OctreePointQoudPointVector (等于 OctreePointCloud ):该八叉树能够保存每一个叶节点上的点索引列。
OctreePointCloudSinglePoint: 该八叉树类仅仅保存每一个叶节点上的单个点索引。仅仅保存最后分配给 叶节点的
点索引。
OctreePointCloudOccupancy: 该八叉树不存储它的叶节点上的任何点信息。它能用于空间填充情况 检查。
OctreePointCloudDensily: 该八叉树存储每一个叶节点体素中的点的数目。它可以进行空间点集密集程 度查询。
如果需要高频率创建八叉树,请参看八叉树双缓冲技术实现( Octree2BufBase 类 )o 该 类在内存中同时保存了两个类似的八叉树对象。除了搜索操作之外,也可以用于空间变化检 测 c 此外,高级内存管理减少了八叉树建立过程中的内存分配和释放操作 G 双缓冲技术对八 叉树的实现可以通过设置模板参数 “OctreeT” 实例化不同的 OctreePointCloud 类。所有的八 叉树都支持八叉树结构和八叉树内容的串行化和反串行化。

5.无序点云数据集的空间变化检测

通过八叉树递归对比，检测被测点云CloudB和参考点云CloudA之间的不同点集，即存在与CloudB而不存在于CloudA的点集。这其中应用到八叉树双缓冲技术，使两片点云同时存储于内存中，共享一个八叉树对象。
这种检测的局限：只能检测CloudB相对于CloudA增加的点集，而不能检测减少的点集。

#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
	
int
main (int argc, char** argv)
{
srand ((unsigned int) time (NULL));
// 设置八叉树的分辨率
float resolution =32.0f;
// 实例化检测器对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );//参考点云
//填充点云
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.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();
// 重置八叉树对象的缓冲区，但CloudA数据依然存储于内存中。两个点云共享八叉树对象。
octree.switchBuffers ();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );//被检测点云
//点云填充数据
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);
  }

octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();
std::vector<int>newPointIdxVector;
//探测CloudB相对于CloudA增加的点集
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
//打印
std::cout<<"Output from getPointIndicesFromNewVoxels:"<<std::endl;
for (size_t i=0; i<newPointIdxVector.size (); ++i)
std::cout<<i<<"# Index:"<<newPointIdxVector[i]
<<"  Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "
<<cloudB->points[newPointIdxVector[i]].y <<" "
<<cloudB->points[newPointIdxVector[i]].z <<std::endl;
}