C++ 【PCL】点云配准

点云配准是将两个或多个点云数据集融合到一个统一的坐标系统中的过程。这通常是为了创建一个完整的模型或融合从不同视角采集的数据。
点云配准一般分为粗配准和精配准，粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。这里我们主要介绍精配准。

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/io/ply_io.h>//pcd输入输出头文件
#include <pcl/registration/icp.h>//点云icp算法头文件
#include <pcl/registration/ndt.h>//点云NDT算法头文件
#include <pcl/registration/ia_ransac.h>//点云ransac迭代对齐算法(SAC-IA)算法头文件
#include <pcl/registration/gicp.h>//点云GICP算法的头文件
#include <pcl/visualization/pcl_visualizer.h>//点云可视化头文件
#include <time.h>
#include <boost/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
typedef pcl::PointXYZ PointT; //重定义pcl::PointXYZ为PointT
typedef pcl::PointCloud<PointT> PointCloud; //重定义pcl::PointCloud<PointT>为PointCloud
//点云配准类型
typedef pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> ICP;
typedef pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> NDT;
typedef pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> SAC_IA;
typedef pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> GICP;
//FPFH特征
typedef pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> FPFHEstimation;
typedef pcl::PointCloud<pcl::FPFHSignature33> FPFH;
//法线估计
typedef pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> NormalEstimation;
typedef pcl::PointCloud<pcl::Normal> Normal;
typedef pcl::search::KdTree<pcl::PointXYZ> KdTreeT;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
    PointCloud::Ptr pcd_tgt,
    PointCloud::Ptr pcd_final)
{
    pcl::visualization::PCLVisualizer viewer("registration Viewer");
    // 设置背景
    viewer.setBackgroundColor(1, 1, 1);
    // 添加坐标轴到可视化对象，参数指定轴的大小（长度），这里设置为1.0单位长度
    viewer.addCoordinateSystem(0.5);
    // 设置相机位置和方向
    viewer.initCameraParameters();
    pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h(pcd_src, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h(pcd_tgt, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<PointT> final_h(pcd_final, 0, 0, 255);
    //viewer.addPointCloud(pcd_src, src_h, "source cloud");    //source绿色
    viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");     //target红色
    viewer.addPointCloud(pcd_final, final_h, "final cloud");  //final蓝色

    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(1000));
    }
}

void printinfo(Eigen::Matrix4f icp_trans, Eigen::Matrix3f rotation,
    Eigen::Vector3f translation, Eigen::Vector3f euler_angles_deg,
    Eigen::Quaternionf quaternion)
{
    cout.setf(ios::fixed);
    cout.precision(5);
    cout << endl << "Transformation matrix:" << endl << icp_trans << endl;//输出变换矩阵
    cout << endl << "Rotation matrix:" << endl << rotation << endl;
    cout << endl << "Translation vector:" << endl << translation << endl;
    cout << endl << "Euler Angle:" << endl;
    cout << endl << "roll(x):" << euler_angles_deg[2] << endl;
    cout << endl << "pitch(y):" << euler_angles_deg[1] << endl;
    cout << endl << "yaw(z):" << euler_angles_deg[0] << endl;
    // 输出四元数,由于欧拉角时常出现万向节，所以可以使用四元数来表示旋转，这样更精确
    cout << endl << "Rotation quaternion: " << endl
        << "w = " << quaternion.w() << endl
        << ", x = " << quaternion.x() << endl
        << ", y = " << quaternion.y() << endl
        << ", z = " << quaternion.z() << endl;
}

void downsampling(PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o,
    pcl::VoxelGrid<pcl::PointXYZ>::Ptr voxel_grid)
{
    float leaf = 0.005f;
    voxel_grid->setLeafSize(leaf, leaf, leaf); // 示例叶子大小
    voxel_grid->setInputCloud(cloud_src_o);
    voxel_grid->filter(*cloud_src_o);
    voxel_grid->setInputCloud(cloud_tgt_o);
    voxel_grid->filter(*cloud_tgt_o);
}
void normalestimation(NormalEstimation ne, Normal::Ptr src_normals,
    Normal::Ptr tgt_normals, KdTreeT::Ptr tree,
    PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{
    //创建一个NormalEstimationOMP对象, 进行法线计算;
    ne.setSearchMethod(tree);
    ne.setKSearch(20);
    ne.setInputCloud(cloud_src_o);
    ne.compute(*src_normals);
    ne.setInputCloud(cloud_tgt_o);
    ne.compute(*tgt_normals);
    cout << "原点云法线计算完成" << endl << *src_normals << endl;
    cout << "目标点云法线计算完成" << endl << *tgt_normals << endl;
}

void fpfh_features(FPFHEstimation fpfh, FPFH::Ptr src_fpfhs,
    FPFH::Ptr tgt_fpfhs, PointCloud::Ptr cloud_src_o,
    PointCloud::Ptr cloud_tgt_o, KdTreeT::Ptr tree,
    Normal::Ptr src_normals, Normal::Ptr tgt_normals)
{
    fpfh.setSearchMethod(tree);
    fpfh.setRadiusSearch(0.05); // Use a radius search instead of a KdTree search
    fpfh.setInputCloud(cloud_src_o);
    fpfh.setInputNormals(src_normals);
    fpfh.compute(*src_fpfhs);
    fpfh.setInputCloud(cloud_tgt_o);
    fpfh.setInputNormals(tgt_normals);
    fpfh.compute(*tgt_fpfhs);
    cout << "原点云特征计算完成" << *src_fpfhs << endl;
    cout << "目标点云特征计算完成" << *tgt_fpfhs << endl;
}

GICP& gicp_registration(GICP& gicp, PointCloud& final_cloud,
    PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{
    gicp.setInputSource(cloud_src_o);
    gicp.setInputTarget(cloud_tgt_o);
    // 配准时最大迭代次数
    gicp.setMaximumIterations(50);
    // 两次变化矩阵之间的差异小于这个阈值时，就认为已经收敛，停止迭代
    gicp.setTransformationEpsilon(1e-6);
    // 对应点之间的最大距离
    gicp.setMaxCorrespondenceDistance(0.05);
    // 采用随机采样一致性方法进行配准
    gicp.setRANSACOutlierRejectionThreshold(1.5);
    // 最小内点比例。在RANSAC配准方法中，当内点的比例小于此值时，认为配准失败。
    gicp.setRANSACIterations(20);
    // 执行配准，并将结果存储在Final中
    gicp.align(final_cloud);
    return gicp;

}
SAC_IA& sacia_registration(SAC_IA& sac_ia, PointCloud& final_cloud,
    PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o,
    FPFH::Ptr& src_fpfhs, FPFH::Ptr& tgt_fpfhs)
{
    sac_ia.setInputSource(cloud_src_o);
    sac_ia.setSourceFeatures(src_fpfhs);
    sac_ia.setInputTarget(cloud_tgt_o);
    sac_ia.setTargetFeatures(tgt_fpfhs);
    // 设置SAC-IA配准的参数
    sac_ia.setMinSampleDistance(0.05f);
    sac_ia.setMaxCorrespondenceDistance(0.01f);
    sac_ia.setMaximumIterations(500);
    // 创建一个空的PointCloud对象来接收结果
    pcl::PointCloud<pcl::PointXYZ> final_registration;
    // 执行配准，并将结果存储在final_registration中
    sac_ia.align(final_registration);
    return sac_ia;
}
NDT& ndt_registration(NDT& ndt, PointCloud& final_cloud,
    PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{
    // 配置NDT
    ndt.setTransformationEpsilon(0.1);
    ndt.setStepSize(0.5);
    ndt.setResolution(2.0);
    ndt.setMaximumIterations(20);
    ndt.setInputSource(cloud_src_o);
    ndt.setInputTarget(cloud_tgt_o);
    ndt.align(final_cloud);
    return ndt;
}

ICP& icp_registration(ICP& icp, PointCloud& final_cloud,
    PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{
    icp.setMaximumIterations(50);
    // 两次变化矩阵之间的差值
    icp.setTransformationEpsilon(1e-10);
    // 均方误差
    icp.setEuclideanFitnessEpsilon(0.01);
    icp.setInputSource(cloud_src_o);//录入source点云
    icp.setInputTarget(cloud_tgt_o);//录入target点云
    icp.align(final_cloud);//最终配准结果
    return icp;
}
int main(int argc, char** argv)
{
    //加载点云文件
    PointCloud::Ptr cloud_src_o(new pcl::PointCloud<pcl::PointXYZ>);//原点云，待配准
    pcl::io::loadPLYFile("0.ply", *cloud_src_o);
    PointCloud::Ptr cloud_tgt_o(new pcl::PointCloud<pcl::PointXYZ>);//目标点云
    pcl::io::loadPLYFile("1.ply", *cloud_tgt_o);
    clock_t start = clock();

    //下采样
    pcl::VoxelGrid<pcl::PointXYZ>::Ptr voxel_grid(new pcl::VoxelGrid<pcl::PointXYZ>);
    downsampling(cloud_src_o, cloud_tgt_o, voxel_grid);


    //创建一个NormalEstimationOMP对象, 进行法线计算
        NormalEstimation ne;
    Normal::Ptr src_normals(new pcl::PointCloud<pcl::Normal>);
    Normal::Ptr tgt_normals(new pcl::PointCloud<pcl::Normal>);
    KdTreeT::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    normalestimation(ne, src_normals, tgt_normals, tree, cloud_src_o, cloud_tgt_o);

    //计算FPFH特征
    /*FPFHEstimation fpfh;
    FPFH::Ptr src_fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
    FPFH::Ptr tgt_fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_features(fpfh, src_fpfhs, tgt_fpfhs, cloud_src_o, cloud_tgt_o, tree, src_normals, tgt_normals);*/


    // 点云配准
    PointCloud::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);
    PointCloud final_cloud;
    ICP icp;
    ICP& reg_p = icp_registration(icp, final_cloud, cloud_src_o, cloud_tgt_o);

    /*NDT ndt;
    NDT & reg_p = ndt_registration(ndt, final_cloud, cloud_src_o, cloud_tgt_o);*/

    /*GICP gicp;
    GICP& reg_p = gicp_registration(gicp, final_cloud, cloud_src_o, cloud_tgt_o);*/

    //SAC_IA sac_ia;
    //SAC_IA& reg_p = sacia_registration(sac_ia, final_cloud, cloud_src_o, cloud_tgt_o, src_fpfhs, tgt_fpfhs);
    clock_t end = clock();
    cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;//输出配准所用时间
    cout << "ICP has converged:" << reg_p.hasConverged() << " score: " << reg_p.getFitnessScore() << std::endl;

    Eigen::Matrix4f reg_p_trans;
    // 变换矩阵
    reg_p_trans = reg_p.getFinalTransformation();
    // 平移向量
    Eigen::Vector3f translation = reg_p_trans.block<3, 1>(0, 3);
    // 旋转矩阵
    Eigen::Matrix3f rotation = reg_p_trans.block<3, 3>(0, 0);
    // 转换为四元数
    Eigen::Quaternionf quaternion(rotation);
    // 使用四元数重新计算欧拉角
    Eigen::Vector3f euler_angles_rad = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);

    //获取欧拉角（弧度）
        //Eigen::Vector3f euler_angles_rad = rotation.eulerAngles(0, 1, 2);
        // 将弧度转换为角度
    Eigen::Vector3f euler_angles_deg = euler_angles_rad * 180.0 / M_PI;
    printinfo(reg_p_trans, rotation, translation, euler_angles_deg, quaternion);
    //使用创建的变换对未过滤的输入点云进行变换
    pcl::transformPointCloud(*cloud_src_o, *icp_result, reg_p_trans);

    visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);
    return (0);
}