无序点云的空间变化检测

//
// Created by xuming on 21-5-24.
//

#include <iostream>
#include <pcl-1.11/pcl/point_types.h>
#include <pcl-1.11/pcl/octree/octree.h>

// 无序点云数据集的空间变化检测
// octree 是一种
// 无序点云数据集的空间变化检测

int main()
{
    srand((unsigned int)time(NULL));

    float resolution = 32.0f; // 八叉树分辨率即体素的大小

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

    // 创建点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    cloud->width = 128;     // 设置点云 数
    cloud->height = 1;      // 设置无序点云

    cloud->points.resize(cloud->width*cloud->height);

    for (size_t i=0;i<cloud->points.size();i++){
        cloud->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloud->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 添加点云到八叉树
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();


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

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

    cloud_b->width = 128;     // 设置点云 数
    cloud_b->height = 1;      // 设置无序点云

    cloud_b->points.resize(cloud->width*cloud->height);

    for (size_t i=0;i<cloud->points.size();i++){
        cloud_b->points[i].x = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloud_b->points[i].y = 64.0f * rand() / (RAND_MAX + 1.0f);
        cloud_b->points[i].z = 64.0f * rand() / (RAND_MAX + 1.0f);
    }

    // 添加点云到八叉树
    octree.setInputCloud(cloud_b);
    octree.addPointsFromInputCloud();

    // 探测了 cloud B 相对于 cloud 变化的点击

    std::vector<int> newPointIdxVector;

    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:"
                  << cloud_b->points[newPointIdxVector[i]].x
                  << cloud_b->points[newPointIdxVector[i]].y
                  << cloud_b->points[newPointIdxVector[i]].z
                  << std::endl;
    }




    return 0;
}