点云学习笔记11——多幅点云文件PCS+ICP配准

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_fpcs.h>
#include <vector>
#include <string>

using namespace std;
using namespace pcl;

int main() 
{
    // 定义点云类型
    typedef PointXYZ PointT;
    typedef PointCloud<PointT> PointCloud;

    // 文件列表，假设文件名按顺序排列
    vector<string> filenames = { "1.pcd", "2.pcd", "3.pcd", "4.pcd", "5.pcd", "6.pcd", "7.pcd", "8.pcd", "9.pcd" };

    // 存储所有配准后的点云
    vector<PointCloud::Ptr> aligned_clouds;

    // 读取第一个点云作为参考点云
    PointCloud::Ptr reference_cloud(new PointCloud);
    if (io::loadPCDFile("F:\\" + filenames[0], *reference_cloud) < 0) 
    {
        PCL_ERROR("Couldn't read file %s \n", filenames[0].c_str());
        return (-1);
    }
    cout << "Loaded reference cloud with " << reference_cloud->size() << " points." << endl;

    // 对参考点云进行降采样
    PointCloud::Ptr reference_subsampled(new PointCloud);
    VoxelGrid<PointT> sor;
    sor.setInputCloud(reference_cloud);
    sor.setLeafSize(0.05f, 0.05f, 0.05f);
    sor.filter(*reference_subsampled);

    aligned_clouds.push_back(reference_subsampled); // 将降采样后的参考点云加入列表

    // 依次与前一个点云配准
    for (size_t i = 1; i < filenames.size(); ++i) 
    {
        PointCloud::Ptr current_cloud(new PointCloud);
        if (io::loadPCDFile("F:\\" + filenames[i], *current_cloud) < 0) 
        {
            PCL_ERROR("Couldn't read file %s \n", filenames[i].c_str());
            return (-1);
        }
        cout << "Loaded cloud " << filenames[i] << " with " << current_cloud->size() << " points." << endl;

        // 对当前点云进行降采样
        PointCloud::Ptr current_subsampled(new PointCloud);
        sor.setInputCloud(current_cloud);
        sor.filter(*current_subsampled);

        // PCS 配准
        registration::FPCSInitialAlignment<PointT, PointT> fpcs;
        fpcs.setInputSource(current_subsampled);
        fpcs.setInputTarget(aligned_clouds.back());
        fpcs.setApproxOverlap(0.7);
        fpcs.setDelta(0.05);
        fpcs.setNumberOfSamples(100);
        PointCloud::Ptr aligned(new PointCloud);
        fpcs.align(*aligned);
        if (fpcs.hasConverged()) 
        {
            cout << "PCS has converged, score is " << fpcs.getFitnessScore() << endl;
            Eigen::Matrix4f transformation_matrix = fpcs.getFinalTransformation();
            cout << "PCS transformation matrix:\n" << transformation_matrix << endl;

            // 将变换应用到当前点云
            pcl::transformPointCloud(*current_cloud, *aligned, transformation_matrix);

            // ICP 配准
            IterativeClosestPoint<PointT, PointT> icp;
            icp.setInputSource(aligned);
            icp.setInputTarget(aligned_clouds.back());
            icp.setMaximumIterations(100);
            icp.setTransformationEpsilon(1e-6);
            icp.setEuclideanFitnessEpsilon(1e-6);
            icp.align(*aligned);
            if (icp.hasConverged()) 
            {
                cout << "ICP has converged, score is " << icp.getFitnessScore() << endl;
                cout << "ICP transformation matrix:\n" << icp.getFinalTransformation() << endl;

                // 计算配准后的点云总数
                size_t totalPoints = 0;
                for (const auto& cloud : aligned_clouds)
                {
                    totalPoints += cloud->size();
                }
                cout << "Total points in aligned clouds: " << totalPoints << endl;

                // 计算平均配准得分
                double totalScore = 0.0;
                for (const auto& cloud : aligned_clouds) 
                {
                    // 假设这里有一个方法可以获取每个点云的特定配准得分
                    double score = 0.0;
                    // 这里只是一个占位符，实际需要根据你的配准算法来计算每个点云的得分
                    totalScore += score;
                }
                double averageScore = totalScore / aligned_clouds.size();
                cout << "Average registration score: " << averageScore << endl;
            }
            else 
            {
                cout << "ICP did not converge." << endl;
            }
        }
        else 
        {
            cout << "PCS did not converge." << endl;
        }

        aligned_clouds.push_back(aligned); // 将配准后的点云加入列表
    }

    // 可视化所有配准后的点云
    visualization::PCLVisualizer viewer("Multi-Cloud Viewer");
    viewer.setBackgroundColor(0, 0, 0);
    for (size_t i = 0; i < aligned_clouds.size(); ++i) 
    {
        visualization::PointCloudColorHandlerCustom<PointT> single_color(aligned_clouds[i], 0, 255, 0);
        viewer.addPointCloud(aligned_clouds[i], single_color, "cloud" + to_string(i));
    }
    viewer.initCameraParameters();

    while (!viewer.wasStopped()) 
    {
        viewer.spinOnce(100);
    }

    return 0;
}