点云学习笔记4——点云滤波降采样后进行4PCS粗配准【四点一致集配准算法（4-Point Congruent Set）】

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h> 
#include <pcl/registration/gicp.h>
#include <pcl/registration/icp.h> 
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/registration/ia_fpcs.h> 
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h> 

using namespace std;

int main()
{
    // 读取点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\pig_view1.pcd", *target);
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\pig_view2.pcd", *source);

    // 创建统计滤波器对象
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> staticOR;
    staticOR.setInputCloud(source);
    staticOR.setMeanK(20);   
    staticOR.setStddevMulThresh(5);   

    // 进行滤波
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
    staticOR.filter(*cloud_filtered);
    source = cloud_filtered;
    cout << "点云滤波完成！" << endl;

    // 创建统计滤波器对象
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> staticOR1;
    staticOR1.setInputCloud(target);
    staticOR1.setMeanK(20);   
    staticOR1.setStddevMulThresh(5);   

    // 进行滤波
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered1(new pcl::PointCloud<pcl::PointXYZ>());
    staticOR1.filter(*cloud_filtered1);
    target = cloud_filtered1;
    cout << "点云滤波完成！" << endl;

    // 对源点云进行VoxelGrid降采样
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(source);
    sor.setLeafSize(0.05f, 0.05f, 0.05f); // 设置体素大小
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
    sor.filter(*cloud_source_subsampled);
    target = cloud_source_subsampled;

    // 对目标点云进行VoxelGrid降采样
    sor.setInputCloud(target);
    sor.setLeafSize(0.05f, 0.05f, 0.05f); // 设置体素大小
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
    sor.filter(*cloud_target_subsampled);
    target = cloud_target_subsampled;

    //PCS配准
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(source);
    fpcs.setInputTarget(target);
    fpcs.setApproxOverlap(0.7);         // 设置近似重叠率，值越大，算法期望的重叠区域越大
    fpcs.setDelta(0.01);                // 精度参数，值越小，算法的精度越高，但计算时间可能会增加
    fpcs.setNumberOfSamples(100);       // 采样点数量，越大，算法可能会更准确，但计算时间也会增加
    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);
    fpcs.align(*aligned);               // 执行配准
    Eigen::Matrix4f final_transform = fpcs.getFinalTransformation(); // 获取变换矩阵
    // 输出变换矩阵
    cout << "变换矩阵：\n" << final_transform << endl;

    // 保存点云到文件
    pcl::io::savePCDFileBinary("D:\\PPCL\\pig_view1112.pcd", *aligned);
    cout << "配准后点云文件已经保存" << endl;

    //可视化
    pcl::visualization::CloudViewer viewer("Viewer");
    viewer.showCloud(aligned); // 直接显示点云

    while (!viewer.wasStopped()) // 循环直到用户关闭窗口
    {
    }

    return (0);
}