【笔记】【点云PCL从入门到精通】第四章 K-D Tree 与 八叉树

通过雷达、激光扫描、立体摄像机等三维测量设备获取的殿宇数据，具有数据量大、分布不均与等特点。作为三维领域里面另一个重要的数据来源，点云数据主要是表征目标表面的海量点集合，并不具备传统实体网格数据的集合拓扑信息。点云数据处理中最为核心的问题就是建立离散点间的拓扑关系，实现基于领域关系的快速查找。建立空间索引在点云数据处理中已被广泛应用，常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构，比较有代表性的包括BSP Tree、K-D Tree、KDB Tree、R Tree、R+ Tree、Cell Tree、四叉树和八叉树等索引结构，而在这些结构中K-D Tree与八叉树在3D点云数据组织中的应用较为广泛。PCL对八叉树的数据结构建立和索引方法进行了实现，以方便在次基础上的其他点云处理操作。

4.1 K-D Tree和八叉树的概念及相关算法

4.1.1 K-D Tree 概念及相关算法

K-D Tree 简介

K-D Tree，或称K维树，是计算机科学中使用的一种数据结构，用来组织表示K维空间中的点集合，如图4-1所示。它是一种带有其他约束条件的二分查找树。K-D Tree对于区间和临近搜索十分有用。我们维了达到目的，通常只在三个维度中进行处理，因此所有的K-D Tree都是三维K-D Tree。K-D Tree的每一级在指定维度上分开所有的子节点。在书的根部，所有子节点在第一个指定的维度上被分开（也就是说，第一维坐标消毒根节点的将分在左边的子树中，大于根节点的将分在右边的子树中）。树的每一级都在下一个维度上分开，所有其他的维度用完之后就回到第一个维度。建立K-D Tree最高效的方法是，像开苏分类一样使用分割法，把指定维度的值放在根上，在改维度上包含较小数值的在左子树，较大的在右子树。然后分别在缩编和右边子树上充分这个过程，知道逆准备分类的左右一个树仅仅有一个元素组成。

4.1.2 PCL 中K-D Tree模块及类

PCL中K-D Tree库提供了K-D Tree数据结构，基于FLANN进行快速最近邻检索。最近邻检索在匹配、特征描述子计算、邻域特征提取中式非常基础的核心操作。K-D Tree模块利用两个类与两个函数实现了利用K-D Tree数据结构对点云高效管理和检索，其依赖于pcl_common模块。具体说明受篇幅所限，请感兴趣的读者自行查询相关资料以及http://docs.pointclouds.org/trunk/group_k-d tree.html。

4.1.3 八叉树概念及相关算法

八叉树简介

八叉树结构式Hunter博士于1978年首次提出的一种数据模型，如图4-2所示。八叉树结构通过循环递归的划分方法对大小为2n * 2n * 2n的三维空间的几何对象进行剖分，从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性，则该体元构成一个叶节点；否则继续将该体元剖分成8个子立方体，依次递归剖分。对于2n * 2n * 2n大小的空间对象，最多剖分n次。

4.1.4 PCL中八叉树模块及类

PCL中八叉树库提供了八叉树数据结构，利用FLANN进行快速邻域检索。邻域检索在匹配、特征描述子计算、邻域特征提取中式非常基础的核心操作。八叉树模块利用15个类实现了利用八叉树数据结构对于点云的高效管理和检索，以及相应的一些空间处理算法，例如压缩、空间变化检测，其依赖于pcl_common模块。具体说明受篇幅所限，请感兴趣的读者自行查询相关资料以及http://docs.pointclouds.org/trunk/group_octree.html。

4.2 K-D Tree与八叉树入门级实例解析

4.2.1 在PCL中如何实现快速邻域检索

在本小节中一起学习如何用K-D Tree找到具体点或空间位置的K近邻，然后学习如何找到用户指定的（本例中式随机的）某一半径内的所有近邻。

代码

首先，在本书提供的第4章例1文件夹中，打开名为k-d tree_search.cpp的代码文件。

解释说明

下面对源代码中的关键语句进行解析。代码首先用系统时间初始化rand()函数的种子，利用时间初始化，每次运行时所产生的随机数都是不同的，或者哟可能过户可以用固定的数值或不初始化随机种子，运行程序产生的随机数不变，然后创建点云对象，并用随机数据填充点云对象。

srand(time(nullptr));
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);
}

下面的代码块创建KdTreeFLANN对象，并把我们创建的点云设置成输入，然后创建一个serach_point变量作为查询点，并且为它分配随机坐标值。

// 创建K-D Tree对象
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

// 向K-D Tree对象中填充点云
kdtree.setInputCloud(cloud);

// 创建搜索点
pcl::PointXYZ search_point;
search_point.x = rand() * 1024.0f / (RAND_MAX + 1.0f);
search_point.y = rand() * 1024.0f / (RAND_MAX + 1.0f);
search_point.z = rand() * 1024.0f / (RAND_MAX + 1.0f);

现在创建一个整数(设置成10)和两个向量来存储搜索到的K近邻，两个向量中，一个存储搜索到查询点近邻的索引，另一个存储对应近邻的平方距离。

// 邻域大小进行搜索
const auto k = 10;
std::vector<int> point_idx_nkn_search(k);
std::vector<float> point_nkn_squared_distance(k);

std::cout << "K nearest neighbor search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with K=" << k << std::endl;

假设K-D Tree对象返回了多于0个近邻，搜索结果已经存储在我们之前创建的两个向量中，并把10个近邻的位置打印输出。

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

下面代码展示找到了给定的半径内的所有近邻，它重新定义两个向量，来存储相关近邻信息。

// 邻域半径进行搜索
std::vector<int> point_idx_radius_search;
std::vector<float> point_radius_squared_distance;
const auto radius = rand() * 256.0 / (RAND_MAX + 1.0f);

std::cout << "Neighbors within radius search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with radius=" << radius << std::endl;

如果近邻的数量大于0，打印输出

if (kdtree.radiusSearch(search_point, radius, point_idx_radius_search, point_radius_squared_distance) > 0)
{
    for (size_t i = 0; i < point_idx_radius_search.size(); ++i)
        std::cout << "    " << cloud->points[point_idx_radius_search[i]].x << " " << cloud->points[point_idx_radius_search[i]].y << " " << cloud->points[point_idx_radius_search[i]].z << " (squared distance: " << point_radius_squared_distance[i] << ")" << std::endl;
}

CMAKE

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(kdtree_search)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(kdtree_search kdtree_search.cpp)
target_link_libraries(kdtree_search ${PCL_LIBRARIES})

FULL CODE

#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));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 设置参数
    cloud->width = 2000;
    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对象
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

    // 向K-D Tree对象中填充点云
    kdtree.setInputCloud(cloud);

    // 创建搜索点
    pcl::PointXYZ search_point;
    search_point.x = rand() * 1024.0f / (RAND_MAX + 1.0f);
    search_point.y = rand() * 1024.0f / (RAND_MAX + 1.0f);
    search_point.z = rand() * 1024.0f / (RAND_MAX + 1.0f);

    // 邻域大小进行搜索
    const auto k = 10;
    std::vector<int> point_idx_nkn_search(k);
    std::vector<float> point_nkn_squared_distance(k);

    std::cout << "K nearest neighbor search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with K=" << k << std::endl;
    
    if (kdtree.nearestKSearch(search_point, k, point_idx_nkn_search, point_nkn_squared_distance) > 0)
    {
        for (size_t i = 0; i < point_idx_nkn_search.size(); ++i)
            std::cout << "    " << cloud->points[point_idx_nkn_search[i]].x << " " << cloud->points[point_idx_nkn_search[i]].y << " " << cloud->points[point_idx_nkn_search[i]].z << " (squared distance: " << point_nkn_squared_distance[i] << ")" << std::endl;
    }

    // 邻域半径进行搜索
    std::vector<int> point_idx_radius_search;
    std::vector<float> point_radius_squared_distance;
    const auto radius = rand() * 256.0 / (RAND_MAX + 1.0f);

    std::cout << "Neighbors within radius search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with radius=" << radius << std::endl;

    const auto num = kdtree.radiusSearch(search_point, radius, point_idx_radius_search, point_radius_squared_distance);
    if (num > 0)
    {
        for (size_t i = 0; i < point_idx_radius_search.size(); ++i)
            std::cout << "    " << cloud->points[point_idx_radius_search[i]].x << " " << cloud->points[point_idx_radius_search[i]].y << " " << cloud->points[point_idx_radius_search[i]].z << " (squared distance: " << point_radius_squared_distance[i] << ")" << std::endl;
    }
    return 0;
}

4.2.2 在PCL中如何实现点云压缩

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

下面，我们解释单个点云和点云数据流时如何高效压缩的，在给出的例子中，我们用PCL点云压缩奇数来压缩用OpenNIGrabber抓取到的点云。

代码

首先，在本书提供的第4章例2文件夹中，打开名为point_cloud_compression.cpp的代码文件。

解释说明

下面详细解析打开的源代码。从主函数开始，首先创建一个新的SimpleOpenNIViewer实例，并调用它的run()方法。

int main(int argc, char** argv)
{
    SimpleOpenNIViewer v;
    
    v.run();
    return (0);
}

在run()函数中，创建pcl::io::OctreePointCloudCompression类对象来编码和解码，这些对象把压缩配置文件作为配置压缩算法的参数，所提供的压缩配置文件为OpenNI2兼容设备采集到的点云预先确定的通用参数集。本例中，使用pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR配置参数集，它应用5立方毫米的编码精度并且允许彩色纹理成份编码，并进一步优化，用于快速在线压缩。

// 压缩选项详见 /compression/compression_profiles.h
const auto compression_profile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

// 初始化压缩与解压缩对象
point_cloud_encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>(compression_profile, show_statistics);
point_cloud_decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>();

下面的代码为OpenNI2兼容设备实例一个新的采集器，并且叮咚循环回调接口，每从设备获取一帧数据，就调用回调函数一次，这里的回调函数实现数据压缩和可视化解压缩结果。

//创建从 OpenNI获取点云的对象
pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

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

auto c = interface->registerCallback(f);

// 开始接收点云数据流
interface->start();
while (!viewer.wasStopped())
{
    SLEEP(1);
}

interface->stop();

在OpenNI2Grabber采集循环执行的回调函数cloud_cb_中，我们首先获取到的点云压缩stringstream缓冲区，下一步是压缩，它对压缩了的二进制数据进行编码，存储在新的点云对象中，解码了的点云被发送到点云可视化对象中进行实时可视化。

void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
{
    if (!viewer.wasStopped())
    {
        // 存储压缩点云的字节流
        std::stringstream compressedData;
        // 输出点云
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZRGBA>());
        // 压缩点云
        point_cloud_encoder->encodePointCloud(cloud, compressedData);
        // 解压缩点云
        point_cloud_decoder->decodePointCloud(compressedData, cloudOut);
        //可视化解压缩点云
        viewer.showCloud(cloudOut);
    }
}

CMAKE

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(point_cloud_compression)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(point_cloud_compression point_cloud_compression.cpp)
target_link_libraries(point_cloud_compression ${PCL_LIBRARIES})

FULL CODES

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni2_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/compression/octree_pointcloud_compression.h>
#include <pcl/compression/compression_profiles.h>
#include <sstream>

#ifdef WIN32
# define SLEEP(x) Sleep((x)*1000)
#endif

class SimpleOpenNIViewer
{
public:
    SimpleOpenNIViewer() : viewer(" Point Cloud Compression Example")
    {
    }

void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr& cloud)
{
    if (!viewer.wasStopped())
    {
        // 存储压缩点云的字节流
        std::stringstream compressedData;
        // 输出点云
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZRGBA>());
        // 压缩点云
        point_cloud_encoder->encodePointCloud(cloud, compressedData);
        // 解压缩点云
        point_cloud_decoder->decodePointCloud(compressedData, cloudOut);
        //可视化解压缩点云
        viewer.showCloud(cloudOut);
    }
}

    void run()
    {
        const auto show_statistics = true;

        // 压缩选项详见 /compression/compression_profiles.h
        const auto compression_profile = pcl::io::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

        // 初始化压缩与解压缩对象
        point_cloud_encoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>(compression_profile, show_statistics);
        point_cloud_decoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>();

        //创建从 OpenNI获取点云的对象
        pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

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

        auto c = interface->registerCallback(f);

        // 开始接收点云数据流
        interface->start();
        while (!viewer.wasStopped())
        {
            SLEEP(1);
        }

        interface->stop();

        // 删除点云压缩与解压缩对象实例
        delete (point_cloud_encoder);
        delete (point_cloud_decoder);
    }

    pcl::visualization::CloudViewer viewer;
    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* point_cloud_encoder;
    pcl::io::OctreePointCloudCompression<pcl::PointXYZRGBA>* point_cloud_decoder;
};


int main(int argc, char** argv)
{
    SimpleOpenNIViewer v;
    v.run();
    return (0);
}

压缩配置文件

// 分辨率为1cm³，无颜色，快速在线编码
LOW_RES_ONLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为1cm³，有颜色，快速在线编码
LOW_RES_ONLINE_COMPRESSION_WITH_COLOR 

// 分辨率为5mm³，无颜色，快速在线编码
MED_RES_ONLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为5mm³，有颜色，快速在线编码
MED_RES_ONLINE_COMPRESSION_WITH_COLOR

// 分辨率为1mm³，无颜色，快速在线编码
HIGH_RES_ONLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为1mm³，有颜色，快速在线编码
HIGH_RES_ONLINE_COMPRESSION_WITH_COLOR

// 分辨率为1cm³，无颜色，高效离线编码
LOW_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为1cm³，有颜色，高效离线编码
LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR 	

// 分辨率为5mm³，无颜色，高效离线编码
MED_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为5mm³，有颜色，高效离线编码
MED_RES_OFFLINE_COMPRESSION_WITH_COLOR

// 分辨率为1mm³，无颜色，高效离线编码
HIGH_RES_OFFLINE_COMPRESSION_WITHOUT_COLOR
// 分辨率为1mm³，有颜色，高效离线编码
HIGH_RES_OFFLINE_COMPRESSION_WITH_COLOR

// 允许为高级参数进行手工配置
MANUAL_CONFIGURATION

高级参数化

为了能完全控制压缩相关参数，pcl::io::OctreePointCloudCompression类的构造函数可以在初始化时附加压缩参数。请注意，为了能进行高级参数化，该参数需要设置为MANUAL_CONFIGURATION。

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:把压缩相关的统计信息打印到标准输出
 pointResolution_arg:定义点坐标的编码精度，该参数应该设置成小于传感器精度的一个值
 octreeResolution_arg:该参数定义展开了的八叉树的体素大小，较大的体素分辨率使得压缩更快，但是压缩质量下降，这在较高的帧率和压缩效率中间进行折衷设置
 doVoxelGridDownDownSampling_arg : 如果激活该参数，那么只编码分层八叉树的数据结构，解码对象在体素中心生成点，通过这种该方法，点云在压缩期间被下采样，同时达到了较高的压缩性能
 iFrameRate_arg：点云压缩模式对点云进行差分编码压缩
 doColorEncoding_arg：该选项启用彩色纹理成份编码压缩
 colorBitResolution_arg：该参数定义每一个彩色成份编码后所占的比特数
*/

4.2.3 基于八叉树的空间划分及搜索操作

八叉树是一种用于管理稀疏3D数据的树状数据结构，每个内部节点都正好有八个子节点，本小节中我们学习如何用八叉树在点云数据中进行空间划分及近邻搜索，特别地，解释了如何完成，“体素内近邻搜索(Neighbors within Voxel Search)”，“K近邻搜索(K Nearest Neighbor Search)”和“半径内近邻搜索(Neighbors within Radius Search)”。

代码

首先，在本书提供的第4章例3文件夹中，打开名为octree_search.cpp的代码文件。

解释说明

下面解析打开的源代码。我们首先定义并实例化一个PointCloud指针对象，并且用随机点集赋值给它。

srand(static_cast<unsigned>(time(nullptr)));
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);
}

然后创建一个八叉树，用设置分辨率进行初始化，该八叉树用它的叶节点存放点索引向量，该分辨率参数描述最低一级八叉树的最小体素的尺寸，因此该八叉树的深度是分辨率和点云空间位数的函数，如果知道点云的边界框，应该用defineBoundingBox方法把它分配给八叉树，然后通过点云指针把所有点增加到八叉树中。

const auto resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();

一旦PointCloud和八叉树联系在一起，我们就能进行搜索操作，这里使用的第一种搜索方法是体素近邻搜索，它把查询点所在的体素中其他点索引作为查询结果返回，结果以点索引的向量的形式保存，因此搜索点和搜索结果之间的距离取决于八叉树的分辨率参数。

// 体素内近邻搜索
std::vector<int> pointIdxVec;
if (octree.voxelSearch(search_point, pointIdxVec))
{
    std::cout << "Neighbors within voxel search at (" << search_point.x << " " << search_point.y << " " << search_point.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近邻搜索与半径内近邻搜索如下：

//K近邻搜索
const auto k = 10;
std::vector<int> point_idx_nkn_search;
std::vector<float> point_nkn_squared_distance;
std::cout << "K nearest neighbor search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with K=" << k << std::endl;

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

//半径内近邻搜索
std::vector<int>pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
const auto radius = 256.0f * rand() / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << search_point.x << " " << search_point.y << " " << search_point.z << ") with radius=" << radius << std::endl;

if (octree.radiusSearch(search_point, 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;
}

CMAKE

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(octree_search)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(octree_search octree_search.cpp)
target_link_libraries(octree_search ${PCL_LIBRARIES})

FULL CODES

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

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

    const auto 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近邻搜索
    const auto k = 10;
    std::vector<int> point_idx_nkn_search;
    std::vector<float> point_nkn_squared_distance;
    std::cout << "K nearest neighbor search at (" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z << ") with K=" << k << std::endl;

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

    //半径内近邻搜索
    std::vector<int>pointIdxRadiusSearch;
    std::vector<float>pointRadiusSquaredDistance;
    const auto 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;
    }
}

4.2.4 无序点云数据集的空间变换检测

八叉树是一种用于管理稀疏3D数据的树状数据结构，本小节中我们学习如何利用八叉树实现用于多个无序点云之间的空间变化检测，这些点云可能在尺寸、分辨率、密度和点顺序等方面有所差异。通过递归地比较八叉树的树结构，可以鉴定出由八叉树产生的体素组成之间的区别所代表的空间变化，此外，我们解释了如果使用PCL的八叉树双缓冲技术，以便能实时地探测多个点云之间的空间组成差异。

代码

首先，在本书提供的第四章例4文件夹中，打开名为octree_change_detection.cpp的代码文件。

解释说明

首先，实例化pcl::octree::OctreePointCloudChangeDetector类，并定义它的体素分辨率。

srand(static_cast<unsigned>(time(nullptr)));

// 八叉树分辨率，即体素的大小
const auto resolution = 32.0;

// 初始化空间变化检测对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);

然后创建点云实例cloudA，用随机点数据填充，所生成的点数据用于创建八叉树对象。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);

// 为cloudA点云填充数据
cloudA->width = 128;  // 设置点云cloudA点数
cloudA->height = 1;   // 设置点云cloudA为无序点云
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是参考点云，用其建立的八叉树对象描述它的空间分布。OctreePointCloudChangeDetector类集成自Octree2BufBase类，Octree2BufBase类允许同时在内存中保存和管理两个八叉树，另外，它应用了内存池，该机制能够重新利用已分配了的节点对象，因此减少了在生成多个点云八叉树对象时昂贵的内存分配和释放操作。通过访问octree.switchBuffers()重置了八叉树对象的缓冲区，但把之前的八叉树数据仍保留在内存中。

// 交换八叉树缓存，但是cloudA对应的八叉树结构仍在内存中
octree.switchBuffers();

现在我们实例化第二个点云对象cloudB并用随即点数填充它，该点云用于建立新的八叉树结构，该新的八叉树与前一个cloudA对应的八叉树共享八叉树对象，但同时在内存中驻留。

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.setInputCloud(cloudB);
octree.addPointsFromInputCloud();

为了获取存在于cloudB的点击R(此R并没有cloudA中元素)，可以调用getPointIndicesFromNewVoxels方法，通过探测两个八叉树之间体素的不同来实现。它返回cloudB中新加点的索引向量，通过索引向量可以获取R点集。很明显，这样就探测了cloudB相对于cloudA变化的点集，但只能探测在cloudA上增加的点集，而不能探测在cloudA上减少的点集。

std::vector<int>newPointIdxVector;

// 获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的点集
octree.getPointIndicesFromNewVoxels(newPointIdxVector);

CMAKE

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(octree_change_detection)
find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (octree_change_detection octree_change_detection.cpp)
target_link_libraries (octree_change_detection ${PCL_LIBRARIES})

FULL CODES

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

int main(int argc, char** argv)
{
    srand(static_cast<unsigned>(time(nullptr)));

    // 八叉树分辨率，即体素的大小
    const auto resolution = 32.0;

    // 初始化空间变化检测对象
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);

    // 为cloudA点云填充数据
    cloudA->width = 128;  // 设置点云cloudA点数
    cloudA->height = 1;   // 设置点云cloudA为无序点云
    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创建点云
    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.setInputCloud(cloudB);
    octree.addPointsFromInputCloud();

    std::vector<int>newPointIdxVector;
    // 获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的点集
    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;
}