6：C++搭配PCL点云配准之FPFH特征

代码：

#pragma warning(disable:4996)
#include <pcl/registration/ia_ransac.h>//采样一致性
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>//
#include <pcl/filters/filter.h>//
#include <pcl/registration/icp.h>//icp配准
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <time.h>//时间

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

//点云可视化
void visualize_pcd2(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_src1, PointCloud::Ptr pcd_tgt1)
{

    //创建初始化目标
    pcl::visualization::PCLVisualizer viewer("registration Viewer");
    int v1(0);
    int v2(1);
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h1(pcd_src1, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h1(pcd_tgt1, 255, 0, 0);
    viewer.setBackgroundColor(255, 255, 255);
    viewer.addPointCloud(pcd_src, src_h, "source cloud", v1);
    viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud", v1);
    viewer.addPointCloud(pcd_src1, src_h1, "source cloud1", v2);
    viewer.addPointCloud(pcd_tgt1, tgt_h1, "tgt cloud1", v2);

    //viewer.addCoordinateSystem(0.05);
    while (!viewer.wasStopped())
    {
        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}
//由旋转平移矩阵计算旋转角度
void matrix2angle(Eigen::Matrix4f &result_trans, Eigen::Vector3f &result_angle)
{
    double ax, ay, az;
    if (result_trans(2, 0) == 1 || result_trans(2, 0) == -1)
    {
        az = 0;
        double dlta;
        dlta = atan2(result_trans(0, 1), result_trans(0, 2));
        if (result_trans(2, 0) == -1)
        {
            ay = M_PI / 2;
            ax = az + dlta;
        }
        else
        {
            ay = -M_PI / 2;
            ax = -az + dlta;
        }
    }
    else
    {
        ay = -asin(result_trans(2, 0));
        ax = atan2(result_trans(2, 1) / cos(ay), result_trans(2, 2) / cos(ay));
        az = atan2(result_trans(1, 0) / cos(ay), result_trans(0, 0) / cos(ay));
    }
    result_angle << ax, ay, az;

    cout << "x轴旋转角度：" << ax << endl;
    cout << "y轴旋转角度：" << ay << endl;
    cout << "z轴旋转角度：" << az << endl;
}

int main(int argc, char** argv)
{
    //加载点云文件
    PointCloud::Ptr cloud_src_o(new PointCloud);//原点云，待配准
    pcl::io::loadPCDFile("bun270.pcd", *cloud_src_o);
    PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云
    //pcl::io::loadPLYFile("erwo2.ply", *cloud_tgt_o);
    pcl::io::loadPCDFile("bun090.pcd", *cloud_tgt_o);


    //去除NAN点
    std::vector<int> indices_src; //保存去除的点的索引
    pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);
    std::cout << "remove *cloud_src_o nan" << endl;

    std::vector<int> indices_tgt;
    pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);
    std::cout << "remove *cloud_tgt_o nan" << endl;

    //下采样滤波
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;//源点云滤波器
    voxel_grid.setLeafSize(0.005, 0.005, 0.005);
    voxel_grid.setInputCloud(cloud_src_o);
    PointCloud::Ptr cloud_src(new PointCloud);//滤波后的源点云
    voxel_grid.filter(*cloud_src);
    std::cout << "down size *cloud_src_o.pcd from " << cloud_src_o->size() << "to" << cloud_src->size() << endl;
    // pcl::io::savePCDFileASCII("bunny_src_down.pcd", *cloud_src);//保存

    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;//目标点云滤波器
    voxel_grid_2.setLeafSize(0.005, 0.005, 0.005);
    voxel_grid_2.setInputCloud(cloud_tgt_o);
    PointCloud::Ptr cloud_tgt(new PointCloud);//滤波后的目标点云
    voxel_grid_2.filter(*cloud_tgt);
    std::cout << "down size *cloud_tgt_o.pcd from " << cloud_tgt_o->size() << "to" << cloud_tgt->size() << endl;
    clock_t start = clock();

    //计算表面法线
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src;//源点云法线估计对象
    ne_src.setInputCloud(cloud_src);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());//KDTree方式搜索
    ne_src.setSearchMethod(tree_src);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);//源点云法线数据集
    ne_src.setRadiusSearch(0.008);
    ne_src.compute(*cloud_src_normals);

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;//目标点云法线估计对象
    ne_tgt.setInputCloud(cloud_tgt);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());//KDTree方式搜索
    ne_tgt.setSearchMethod(tree_tgt);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);//目标点云法线数据集
    //ne_tgt.setKSearch(20);0
    ne_tgt.setRadiusSearch(0.008);//搜索半径
    ne_tgt.compute(*cloud_tgt_normals);

    //计算FPFH
    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;//源点云快速特征估计对象
    fpfh_src.setInputCloud(cloud_src);//输入源点云
    fpfh_src.setInputNormals(cloud_src_normals);//输入源点云发现数据集
    pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);//KDTree方式搜索
    fpfh_src.setSearchMethod(tree_src_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());//源点云快速特征值数据集
    fpfh_src.setRadiusSearch(0.01);
    fpfh_src.compute(*fpfhs_src);
    std::cout << "compute *cloud_src fpfh" << endl;

    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
    fpfh_tgt.setInputCloud(cloud_tgt);
    fpfh_tgt.setInputNormals(cloud_tgt_normals);
    pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
    fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_tgt.setRadiusSearch(0.01);
    fpfh_tgt.compute(*fpfhs_tgt);
    std::cout << "compute *cloud_tgt fpfh" << endl;

    //SAC配准
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
    scia.setInputSource(cloud_src);
    scia.setInputTarget(cloud_tgt);
    scia.setSourceFeatures(fpfhs_src);
    scia.setTargetFeatures(fpfhs_tgt);
    //scia.setMinSampleDistance(1);  //最小样本距离
    //scia.setNumberOfSamples(2);  //样本数量
    //scia.setCorrespondenceRandomness(20);  //选择邻近点数量
    PointCloud::Ptr sac_result(new PointCloud);
    scia.align(*sac_result);
    std::cout << "sac has converged:" << scia.hasConverged() << "  score: " << scia.getFitnessScore() << endl;
    Eigen::Matrix4f sac_trans;
    sac_trans = scia.getFinalTransformation();
    std::cout << sac_trans << endl;
    //pcl::io::savePCDFileASCII("bunny_transformed_sac.pcd", *sac_result);
    clock_t sac_time = clock();

    //icp配准
    PointCloud::Ptr icp_result(new PointCloud);
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setInputSource(cloud_src);

    icp.setInputTarget(cloud_tgt_o);
    
    icp.setMaxCorrespondenceDistance(0.008);//对应点对最大距离

    icp.setMaximumIterations(100);    // 最大迭代次数
    
    icp.setTransformationEpsilon(1e-10);// 两次变化矩阵之间的差值
    
    icp.setEuclideanFitnessEpsilon(0.2);// 均方误差阈值

    icp.align(*icp_result, sac_trans);

    clock_t end = clock();
    cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
    cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
    cout << "icp time: " << (double)(end - sac_time) / (double)CLOCKS_PER_SEC << " s" << endl;

    std::cout << "ICP has converged:" << icp.hasConverged() << ",ICP has 迭代次数:" << icp.getMaximumIterations()
        << " score: " << icp.getFitnessScore() << std::endl;
    Eigen::Matrix4f icp_trans;
    icp_trans = icp.getFinalTransformation();
    cout<<"the icp.getfinaltransformation is:"<<endl;
    std::cout << icp_trans << endl;
    //使用创建的变换对未过滤的输入点云进行变换
    pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
    //保存转换的输入点云
    //pcl::io::savePCDFileASCII("_transformed_sac_ndt.pcd", *icp_result);

    //计算误差
    /*Eigen::Vector3f ANGLE_origin;
    Eigen::Vector3f TRANS_origin;
    ANGLE_origin << 0, 0, M_PI / 4;//M_PI / 4
    TRANS_origin << 0, 0.3, 0.2;
    double a_error_x, a_error_y, a_error_z;
    double t_error_x, t_error_y, t_error_z;
    Eigen::Vector3f ANGLE_result;
    matrix2angle(icp_trans, ANGLE_result);
    a_error_x = fabs(ANGLE_result(0)) - fabs(ANGLE_origin(0));
    a_error_y = fabs(ANGLE_result(1)) - fabs(ANGLE_origin(1));
    a_error_z = fabs(ANGLE_result(2)) - fabs(ANGLE_origin(2));
    cout << "点云实际旋转角度:\n" << ANGLE_origin << endl;
    cout << "x轴旋转误差 : " << a_error_x << "  y轴旋转误差 : " << a_error_y << "  z轴旋转误差 : " << a_error_z << endl;

    cout << "点云实际平移距离:\n" << TRANS_origin << endl;
    t_error_x = fabs(icp_trans(0, 3)) - fabs(TRANS_origin(0));
    t_error_y = fabs(icp_trans(1, 3)) - fabs(TRANS_origin(1));
    t_error_z = fabs(icp_trans(2, 3)) - fabs(TRANS_origin(2));
    cout << "计算得到的平移距离" << endl << "x轴平移" << icp_trans(0, 3) << endl << "y轴平移" << icp_trans(1, 3) << endl << "z轴平移" << icp_trans(2, 3) << endl;
    cout << "x轴平移误差 : " << t_error_x << "  y轴平移误差 : " << t_error_y << "  z轴平移误差 : " << t_error_z << endl;
    *///可视化
    visualize_pcd2(cloud_src_o, cloud_tgt_o, icp_result,cloud_tgt_o);
    return (0);
}