点云配准-粗配准+精配准(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::