PCL 使用八叉树进行点云变化检测

目录

• 一、概述
• 二、代码
• 三、结果

一、概述

  PCL中的 pcl::octree::OctreePointCloudChangeDetector函数能够实现同时构建八叉树并完成空间变化检测。

二、代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree_pointcloud_changedetector.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv)
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSre(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTat(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("lamp.pcd", *cloudSre) == -1 ||
        pcl::io::loadPCDFile<pcl::PointXYZ>("lamp1.pcd", *cloudTat) == -1)
    {
        PCL_ERROR("Couldn't read the PCD files!\n");
        return -1;
    }
    // 构建八叉树并检测变化
    float resolution = 0.05f;  // 八叉树的分辨率
    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree(resolution);

    // 添加第一个点云到八叉树
    octree.setInputCloud(cloudSre);
    octree.addPointsFromInputCloud();
    // 切换到第二个点云并检测变化
    octree.switchBuffers();  // 切换缓存
    octree.setInputCloud(cloudTat);
    octree.addPointsFromInputCloud();  // 添加第二组点云
    // 获取变化的点索引
    std::vector<int> newPointIdxVector;
    octree.getPointIndicesFromNewVoxels(newPointIdxVector);
    // 输出可视化结果到渲染窗口
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Change Detection Viewer"));
    viewer->setBackgroundColor(0.0, 0.0, 0.0);  // 黑色背景
    // 显示第一组点云（绿色）
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud1_color_handler(cloudSre, 0, 255, 0);
    viewer->addPointCloud(cloudSre, cloud1_color_handler, "cloud1");
    // 显示变化的点（红色）
    pcl::PointCloud<pcl::PointXYZ>::Ptr changed_points(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*cloudTat, newPointIdxVector, *changed_points);  // 将变化的点拷贝出来
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> changed_points_color_handler(changed_points, 255, 0, 0);
    viewer->addPointCloud(changed_points, changed_points_color_handler, "changed_points");
    // 设置点的大小
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud1");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "changed_points");
    // 添加坐标系
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();
    // 可视化
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
    }

    return 0;
}

三、结果