点云配准-粗配准+精配准(C++实现)

简介：

点云配准是点云处理中的一项重要的技术，在slam中的三维地图重建、高精地图定位、位姿预测等方面有着广泛的应用。点云配准通常会采用粗配准后再进行精配准的方式，常见的粗配准方法有PPF、NDT、4PCS等，精配准方法则主要是ICP及其变种。分享一份C++实现的点云配准代码以供交流。

依赖：

PCL

代码：

#include <iostream>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.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/filters/uniform_sampling.h>

#include <pcl/registration/gicp.h>
#include <pcl/registration/joint_icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/ndt.h>       // ndt配准文件头
#include <pcl/registration/gicp6d.h>   // 引入gicp6d头文件
#include <pcl/registration/icp.h> // ICP配准类相关的头文件
#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp_nl.h> // ICP配准类相关的头文件
#include <pcl/registration/ppf_registration.h> //PPF点对特征类相关的头文件
#include <pcl/registration/ia_fpcs.h>

#include <pcl/features/fpfh.h>
#include <pcl/features/ppf.h>
#include <pcl/features/normal_3d.h>

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/extract_clusters.h>

// PPF+ICP
int main(int argc, char **argv)
{
    // 加载源点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_raw(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPCDFile<pcl::PointXYZ>("../img/source/2_20230510.pcd", *cloud_source_raw);

    #if 1
    /*这里是因为目标点云和源点云的单位不一致，一个是m，一个是mm，所以需要对源点云进行缩放*/
    // 定义缩放比例
    float scale = 0.001;

    // 对点云中每个点的坐标进行缩放
    for (int i = 0; i < cloud_source_raw->points.size(); ++i)
    {
        cloud_source_raw->points[i].x *= scale;
        cloud_source_raw->points[i].y *= scale;
        cloud_source_raw->points[i].z *= scale;
    }
    #endif

    // 加载目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_raw(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::io::loadPCDFile<pcl::PointXYZ>("../img/target/cup_withoutbottom.pcd", *cloud_target_raw);

    std::cout<< "源点云和目标点云加载完成！"<<std::endl;


    #if 1
    // 创建统计滤波器对象
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> staticOR;
    staticOR.setInputCloud(cloud_source_raw);
    staticOR.setMeanK(50);   // 设置统计平均点数
    staticOR.setStddevMulThresh(1);   // 设置标准差倍数阈值

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

    #if 0
     // 可视化原始点云和滤波后的点云
    pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");
    viewer.setBackgroundColor(0.0, 0.0, 0.0);   // 背景为黑色
    viewer.addPointCloud(cloud, "original_cloud");
    viewer.addPointCloud(cloud_filtered, "filtered_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "original_cloud");   // 设置原始点云为红色
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "filtered_cloud");   // 设置滤波后的点云为绿色
    viewer.spin();
#endif


#if 0

    // 对源点云进行聚类，取出点数最多的类
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_eucli(new pcl::search::KdTree<pcl::PointXYZ>);
    tree_eucli->setInputCloud(cloud_source_raw);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(45000);
    ec.setSearchMethod(tree_eucli);
    ec.setInputCloud(cloud_source_raw);
    ec.extract(cluster_indices);
    pcl::PointIndices::Ptr largest_cluster(new pcl::PointIndices);
    int max_num_points = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
        if (it->indices.size() > max_num_points)
        {
            max_num_points = it->indices.size();
            *largest_cluster = *it;
        }
    }
    // 提取点云中最多的类，并保存到新的源点云变量 src_filtered_cloud 中
    pcl::PointCloud<pcl::PointXYZ>::Ptr src_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
    extract_indices.setInputCloud(cloud_source_raw);
    extract_indices.setIndices(largest_cluster);
    extract_indices.filter(*src_filtered_cloud);
    cloud_source_raw = src_filtered_cloud;
    std::cout<< "源点云聚类完成！"<<std::endl;

#endif



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

    //对目标点云进行VoxelGrid降采样
    sor.setInputCloud(cloud_target_raw);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_subsampled(new pcl::PointCloud<pcl::PointXYZ>());
    sor.filter(*cloud_target_subsampled);

    std::cout<< "源点云和目标点云降采样完成！"<<std::endl;

    // 计算法向量
    pcl::PointCloud<pcl::Normal>::Ptr normals_source(new pcl::PointCloud<pcl::Normal>()); // 源点云的法向量
    pcl::PointCloud<pcl::Normal>::Ptr normals_target(new pcl::PointCloud<pcl::Normal>()); // 目标点云的法向量
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud_source_subsampled);
    ne.setRadiusSearch(0.032);
    ne.compute(*normals_source);
    ne.setInputCloud(cloud_target_subsampled);
    ne.compute(*normals_target);

    std::cout<< "源点云和目标点云法向量计算完成！"<<std::endl;

    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source(new pcl::PointCloud<pcl::PointNormal>());
    pcl::concatenateFields (*cloud_source_subsampled, *normals_source, *cloud_source);
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_target(new pcl::PointCloud<pcl::PointNormal>());
    pcl::concatenateFields (*cloud_target_subsampled, *normals_target, *cloud_target);

    std::cout<< "源点云和目标点云PointNormal拼接完成！"<<std::endl;

#if 0
    //4PCS配准
	pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
    fpcs.setInputSource(cloud_source_subsampled);
	// fpcs.setInputCloud(cloud_source_subsampled);  //源点云
	fpcs.setInputTarget(cloud_target_subsampled);  //目标点云
	fpcs.setApproxOverlap(0.7);//设置源和目标之间的近似重叠。
	fpcs.setDelta(0.01);//对内部计算参数进行加权的常数因子增量。
	fpcs.setMaxComputationTime(100);//设置最大计算时间(以秒为单位)。
	fpcs.setNumberOfSamples(100); //设置配准期间要使用的源点云采样的数量
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcss(new pcl::PointCloud<pcl::PointXYZ>);
	fpcs.align(*pcss);
    Eigen::Matrix4f icp_initial = fpcs.getFinalTransformation();
    std::cout << "4PCS transformation matrix：" << std::endl<< icp_initial << std::endl;

#endif


#if 1
    //SAC_IA配准
    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    fpfh.setSearchMethod(tree);
    fpfh.setInputCloud(cloud_source_subsampled);
    fpfh.setInputNormals(normals_source);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_in(new pcl::PointCloud<pcl::FPFHSignature33>);
    fpfh.setRadiusSearch(0.042);
    fpfh.compute(*fpfhs_in);

    fpfh.setInputCloud(cloud_target_subsampled);
    fpfh.setInputNormals(normals_target);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_out(new pcl::PointCloud<pcl::FPFHSignature33>);
    fpfh.compute(*fpfhs_out);

    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(cloud_source_subsampled);
    sac_ia.setSourceFeatures(fpfhs_in);
    sac_ia.setInputTarget(cloud_target_subsampled);
    sac_ia.setTargetFeatures(fpfhs_out);
    // sac_ia.setMethodType(pcl::SAC_IA::METHOD_LSM); // 将模型估计方法设置为最小二乘法
    sac_ia.setMinSampleDistance(0.0001);
    sac_ia.setMaxCorrespondenceDistance(0.1);
    sac_ia.setMaximumIterations(200);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned(new pcl::PointCloud<pcl::PointXYZ>);
    sac_ia.align(*cloud_aligned);

    Eigen::Matrix4f icp_initial = sac_ia.getFinalTransformation();
    std::cout << "SAC_IA transformation matrix：" << std::endl<< icp_initial << std::endl;

#endif


#if 0
    // 计算PPF特征
    pcl::PointCloud<pcl::PPFSignature>::Ptr ppf_source(new pcl::PointCloud<pcl::PPFSignature>());
    pcl::PointCloud<pcl::PPFSignature>::Ptr ppf_target(new pcl::PointCloud<pcl::PPFSignature>());
    pcl::PPFEstimation<pcl::PointNormal, pcl::PointNormal, pcl::PPFSignature> ppf_estimator;
    ppf_estimator.setInputCloud(cloud_source);
    ppf_estimator.setInputNormals(cloud_source);
    ppf_estimator.compute(*ppf_source);

    std::cout<< "源点云PPF特征计算完成！"<<std::endl;

    ppf_estimator.setInputCloud(cloud_target);
    ppf_estimator.setInputNormals(cloud_target);
    ppf_estimator.compute(*ppf_target);
    pcl::PPFHashMapSearch::Ptr hashmap_search(new pcl::PPFHashMapSearch( 2 * float(M_PI)/20 ,0.5f));
    hashmap_search->setInputFeatureCloud(ppf_source);

    std::cout<< "目标点云PPF特征计算完成！"<<std::endl;

    PCL_INFO ("Feature cloud sizes: %u , %u\n", ppf_target->points.size (), ppf_source->points.size ());

    // 创建PPF匹配器
    pcl::PPFRegistration<pcl::PointNormal, pcl::PointNormal> ppf_registration;
    ppf_registration.setSceneReferencePointSamplingRate(10);              // 设置场景采样率
    ppf_registration.setPositionClusteringThreshold(0.2f);                // 设置位置聚类阈值
    ppf_registration.setRotationClusteringThreshold(12.0 / 180.0 * M_PI); // 设置旋转聚类阈值
    // ppf_registration.setSearchMethodTarget(pcl::search::KdTree<pcl::PointNormal>::Ptr(new pcl::search::KdTree<pcl::PointNormal>));
    // ppf_registration.setSearchMethodSource(pcl::search::KdTree<pcl::PointNormal>::Ptr(new pcl::search::KdTree<pcl::PointNormal>));
    ppf_registration.setSearchMethod(hashmap_search);
    // ppf_registration.setMaximumIterations(100);

    // 设置匹配器的输入和目标特征
    ppf_registration.setInputSource(cloud_source);
    ppf_registration.setInputTarget(cloud_target);
    // ppf_registration.setSourceFeature(ppf_source);
    // ppf_registration.setTargetFeature(ppf_target);

    std::cout<< "PPF匹配器创建完成！"<<std::endl;

    // 计算初始变换矩阵
    pcl::PointCloud<pcl::PointNormal>::Ptr source_transformed(new pcl::PointCloud<pcl::PointNormal>());
    ppf_registration.align(*source_transformed);

    Eigen::Matrix4f icp_initial = ppf_registration.getFinalTransformation();
    std::cout << "PPF transformation matrix：" << std::endl<< icp_initial << std::endl;
    std::cout << "PPF score:" << ppf_registration.getFitnessScore() << std::endl;

    std::cout<< "PPF初始匹配完成！"<<std::endl;
#endif

    // ICP优化
    // pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::NormalDistributionsTransform<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::GeneralizedIterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::GeneralizedIterativeClosestPoint6D<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::registration::ElasticICP<pcl::PointNormal, pcl::PointNormal> icp;
    // pcl::JointIterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    // pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;
    // pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_source_subsampled);
    icp.setInputTarget(cloud_target_subsampled);
    // typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
	// boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
    // icp.setTransformationEstimation(point_to_plane);
    // icp.setMaxCorrespondenceDistance(0.1);
    icp.setMaximumIterations(200);
    icp.setTransformationEpsilon(1e-18);
    // icp.setEuclideanFitnessEpsilon(1e-18);
    // pcl::PointCloud<pcl::PointNormal>::Ptr cloud_result(new pcl::PointCloud<pcl::PointNormal>);   // 存储配准变换源点云后的点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZ>);   // 存储配准变换源点云后的点云

    icp.align (*cloud_result, icp_initial);
    // icp.align (*cloud_result);

    std::cout<< "ICP精调完成！"<<std::endl;

    // 输出结果
    Eigen::Matrix4f icp_trans;
    icp_trans = icp.getFinalTransformation();
    std::cout << "icp变换矩阵：" << std::endl<<icp_trans << std::endl;
    std::cout << "icp score:" << icp.getFitnessScore() << std::endl;

    //可视化
    pcl::visualization::PCLVisualizer viewer("PPF ICP - Results");
    viewer.setBackgroundColor(0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(cloud_target_subsampled->makeShared(),128,128,0);
    viewer.addPointCloud(cloud_target_subsampled, target_color, "target_cloud");
    viewer.spinOnce(10);


    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(cloud_source_subsampled->makeShared(),255,0,0);
    viewer.addPointCloud(cloud_source_subsampled, source_color, "source_cloud");

    // visualization::PointCloudColorHandlerCustom<PointXYZ> output(cloud_output_subsampled_xyz, 255, 0, 0);
    // pcl::visualization::PointCloudColorHandlerRandom<pcl::PointNormal> random_color(cloud_result->makeShared());
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> random_color(cloud_result->makeShared(),0,255,0);
    viewer.addPointCloud(cloud_result, random_color, "mode_name");

    // pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rsac_color(cloud_aligned->makeShared(),0,0,255);
    // viewer.addPointCloud(cloud_aligned, rsac_color, "rsac_name");

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

    return 0;
}