点云学习笔记9——RANSAC+ICP点云配准算法

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/sample_consensus_prerejective.h> 
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>

using namespace std;

int main()
{
    // 加载源点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\pig_view1.pcd", *source) < 0)
    {
        PCL_ERROR("无法读取源点云文件 \n");
        return -1;
    }
    cout << "成功加载源点云数据" << endl;

    // 加载目标点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\PPCL\\pig_view2.pcd", *target) < 0)
    {
        PCL_ERROR("无法读取目标点云文件 \n");
        return -1;
    }
    cout << "成功加载目标点云数据" << endl;

    // 1. 离群点滤波
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> staticOR;
    staticOR.setMeanK(20);
    staticOR.setStddevMulThresh(1);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
    staticOR.setInputCloud(source);
    staticOR.filter(*cloud_filtered);
    *source = *cloud_filtered;
    cout << "源点云离群点滤波完成！" << endl;

    staticOR.setInputCloud(target);
    staticOR.filter(*cloud_filtered);
    *target = *cloud_filtered;
    cout << "目标点云离群点滤波完成！" << endl;

    // 2. 体素网格滤波降采样
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setLeafSize(0.008f, 0.008f, 0.008f);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
    sor.setInputCloud(source);
    sor.filter(*cloud_source_subsampled);
    *source = *cloud_source_subsampled;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
    sor.setInputCloud(target);
    sor.filter(*cloud_target_subsampled);
    *target = *cloud_target_subsampled;
    cout << "源点云和目标点云降采样完成！" << endl;

    // 3. 法向量估计（OpenMP多线程加速）
    pcl::PointCloud<pcl::Normal>::Ptr source_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;// 创建FPFH特征估计类的实例，使用OpenMP优化以提高计算速度
    n.setNumberOfThreads(8);// 设置FPFH特征估计的线程数为8，以利用多核处理器加速计算
    n.setSearchMethod(pcl::search::KdTree<pcl::PointXYZ>::Ptr(new pcl::search::KdTree<pcl::PointXYZ>()));
    n.setKSearch(10);// 设置K近邻搜索的K值为10，这将决定每个点的FPFH直方图的局部邻域大小
    n.setInputCloud(source);
    n.compute(*source_normals);
    cout << "源点云法向量计算完成" << endl;

    pcl::PointCloud<pcl::Normal>::Ptr target_normals(new pcl::PointCloud<pcl::Normal>);
    n.setInputCloud(target);
    n.compute(*target_normals);
    cout << "目标点云法向量计算完成" << endl;

    // 4. FPFH特征估计
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
    pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
    f.setNumberOfThreads(8);// 设置FPFH特征估计的线程数为8，以利用多核处理器加速计算
    f.setSearchMethod(n.getSearchMethod());
    f.setKSearch(10);// 设置K近邻搜索的K值为10，这将决定每个点的FPFH直方图的局部邻域大小
    f.setInputCloud(source);
    f.setInputNormals(source_normals);
    f.compute(*source_fpfh);
    cout << "源点云FPFH特征计算完成" << endl;

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
    f.setInputCloud(target);
    f.setInputNormals(target_normals);
    f.compute(*target_fpfh);
    cout << "目标点云FPFH特征计算完成" << endl;

    // 5. 使用RANSAC的初始配准
    pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> r_sac;
    r_sac.setInputSource(source);
    r_sac.setInputTarget(target);
    r_sac.setSourceFeatures(source_fpfh);
    r_sac.setTargetFeatures(target_fpfh);
    r_sac.setCorrespondenceRandomness(5);// 设置在随机特征对应时使用的邻居数量，用于确定对应关系的随机性
    r_sac.setInlierFraction(0.8f);// 设置所需的内点比例，只有超过这个比例的对应点对会被保留
    r_sac.setNumberOfSamples(3);// 设置采样点的数量，用于RANSAC算法中的随机抽样
    r_sac.setSimilarityThreshold(0.1f);// 设置边缘长度相似度阈值，用于确定特征对应点对是否足够相似
    r_sac.setMaxCorrespondenceDistance(1.0f);// 设置最大对应点距离，超过这个距离的点对不会被考虑
    r_sac.setMaximumIterations(500);

    pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_ransac(new pcl::PointCloud<pcl::PointXYZ>);
    r_sac.align(*aligned_ransac);
    cout << "RANSAC配准完成" << endl;
    cout << "RANSAC匹配得分: " << r_sac.getFitnessScore() << endl;

    if (r_sac.hasConverged())
    {
        // 获取RANSAC计算的变换矩阵
        Eigen::Matrix4f initial_alignment = r_sac.getFinalTransformation();
        cout << "RANSAC变换矩阵：\n" << initial_alignment << endl;

        // 6. 使用ICP精细配准
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputSource(source);
        icp.setInputTarget(target);
        icp.setMaximumIterations(200);
        icp.setTransformationEpsilon(1e-5);
        icp.align(*aligned_ransac, initial_alignment);

        if (icp.hasConverged())
        {
            cout << "ICP配准完成" << endl;
            cout << "ICP匹配得分: " << icp.getFitnessScore() << endl;
            Eigen::Matrix4f final_alignment = icp.getFinalTransformation();
            cout << "ICP变换矩阵：\n" << final_alignment << endl;

            // 应用ICP变换到源点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_transformed(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::transformPointCloud(*source, *cloud_source_transformed, final_alignment);

            // 合并配准后的点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sum(new pcl::PointCloud<pcl::PointXYZ>());
            *cloud_sum += *target;
            *cloud_sum += *cloud_source_transformed;
            cout << "源点云变换后与目标点云合并完成" << endl;

            // 保存结果
            pcl::io::savePCDFileBinary("D:\\PPCL\\pig_view22221.pcd", *cloud_sum);
            cout << "合并点云已保存" << endl;
        }
        else
        {
            PCL_ERROR("ICP未能收敛");
        }
    }
    else
    {
        PCL_ERROR("RANSAC未能收敛");
    }

    return 0;
}