（十一）OcTree教程三--OcTree在PCL中的应用-无序点云在空间中的动态检测

无序点云在空间中的动态检测

在这篇教程中我们会学到如何用八叉树实现对多幅无序点云在空间变化上的检测。这里说的空间变化包括了点云的大小，分辨率，密度和点序。通过递归得比较八叉树的树结构，空间上的变化会从体素的配置参数上看出来。此外，我们解释了如何使用pcl八叉树的“双缓冲（double buffering）”技术来让我们高效地每次处理多幅点云。

步骤

对无序点云在空间变化上的检测，其实是对前后两幅点云在八叉树结构下的差异检测。因此我们要首先载入一个原始点云，并生成第一个八叉树结构；然后切缓冲，载入第二个点云，生成第二个八叉树结构；最后进行比较，如果有一些叶子结点在第二个八叉树上，但是不在第一个八叉树上，那么就认为这些叶子节点内的点是空间上变化多出来的点。

头文件

#include <pcl\octree\octree.h>

初始化八叉树

// 初始化octree
float resolution = 0.3f;        // 分辨率
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);

载入原始点云

// 载入原始点云
octree.setInputCloud(sourceCloud.makeShared());
octree.addPointsFromInputCloud();

切换缓冲区，这会重置八叉树，但是会保留之前八叉树的树结构

octree.switchBuffers();

载入一个与原始点云不同的另一个点云

octree.setInputCloud(handledCloud.makeShared());
octree.addPointsFromInputCloud();

空间变化检测

// 空间变化检测
std::vector<int> detectedPoints;
int numberOfNewPoints = octree.getPointIndicesFromNewVoxels(detectedPoints);

关键函数：

std::size_t getPointIndicesFromNewVoxels (std::vector<int> &indicesVector_arg,
            const int minPointsPerLeaf_arg = 0)

输入输出参数：
- indicesVector_arg[out]：第二个缓冲中多出来的点，它们在第二个点云中的下标
- minPointsPerLeaf_arg[in]：默认为0，表示只有大于等于这个数量的叶子结点，才可能成为检测的对象。
- 返回值(size_t)：“多出来的点”的个数

程序

/*
 * 功能：OcTree对空间变化的检测
 */
#include <pcl\octree\octree.h>
#include <pcl\point_cloud.h>
#include <pcl\io\pcd_io.h>
#include <pcl\visualization\cloud_viewer.h>

#include <iostream>


int main()
{
    // 随机种子
    srand(time(NULL));

    // 原始点云
    pcl::PointCloud<pcl::PointXYZ> sourceCloud;
    pcl::io::loadPCDFile("cube.pcd", sourceCloud);

    // 改造原始点云
    pcl::PointCloud<pcl::PointXYZ> handledCloud(sourceCloud);       // 拷贝复制
    // 随机增加100个点
    for (int i = 0; i < 100000; i++)
    {
        handledCloud.push_back(
            pcl::PointXYZ(1.0f*rand() / (RAND_MAX + 1.0f), 
            10.0f*rand() / (RAND_MAX + 1.0f), 
            10.0f*rand() / (RAND_MAX + 1.0f)));
    }

    // 初始化octree
    float resolution = 0.3f;
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);

    // 载入原始点云
    octree.setInputCloud(sourceCloud.makeShared());
    octree.addPointsFromInputCloud();

    // 切换缓冲区，这会重置八叉树，但是会保留之前八叉树的树结构
    octree.switchBuffers();
    octree.setInputCloud(handledCloud.makeShared());
    octree.addPointsFromInputCloud();

    // 空间变化检测
    std::vector<int> detectedPoints;
    int numberOfNewPoints = octree.getPointIndicesFromNewVoxels(detectedPoints);
    std::cout << numberOfNewPoints << " New Points Detected!" << std::endl;

    //// 输出检测到的点
    //std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
    //for (size_t i = 0; i < detectedPoints.size(); ++i)
    //  std::cout << i << "# Index:" << detectedPoints[i]
    //  << "  Point:" << handledCloud.points[detectedPoints[i]].x << " "
    //  << handledCloud.points[detectedPoints[i]].y << " "
    //  << handledCloud.points[detectedPoints[i]].z << std::endl;

    // 显示出空间检测出来的点
    pcl::PointCloud<pcl::RGB> pointsRGB;
    pcl::PointCloud<pcl::PointXYZRGBA> showPointsCloud;

    pointsRGB.width = showPointsCloud.width = handledCloud.size();
    pointsRGB.height = showPointsCloud.height = 1;
    pointsRGB.resize(pointsRGB.width);
    showPointsCloud.resize(showPointsCloud.width);

    for (int i = 0; i < pointsRGB.size(); i++)
        pointsRGB.points[i].b = 255;

    for each(auto detected in detectedPoints)
        pointsRGB.points[detected].r = 255;

    pcl::concatenateFields(handledCloud, pointsRGB, showPointsCloud);

    // 可视窗口初始化
    pcl::visualization::PCLVisualizer viewer;
    viewer.setCameraFieldOfView(0.785398);      // fov 大概45度
    viewer.setBackgroundColor(0.5, 0.5, 0.5);   // 背景设为灰色
    viewer.setCameraPosition(
        0, 0, 5,                                // camera位置
        0, 0, -1,                               // view向量--相机朝向
        0, 1, 0                                 // up向量
        );
    viewer.addPointCloud(showPointsCloud.makeShared(), "Out");
    viewer.addPointCloud(sourceCloud.makeShared(), "Source");
    viewer.addCoordinateSystem(0.2, "cloud", 0);
    while (!viewer.wasStopped()) { // 显示，直到‘q’键被按下
        viewer.spinOnce();
    }

    system("pause");
    return 0;
}

运行结果

蓝色的表示没检测出来的点；
白色的点表示原始点云中的点；
红色表示检测出来的“多出来的”点。