4pcs点云配准

配准点云来自斯坦福的兔子点云，本文使用其中0度及45度扫描点云，但经过放大处理，xyz都放大了100倍。原因是原点云太小，导致阈值不好控制。比如对于原点云来说，点间距离本来就特别小，即使是非正确对应点。
本文将45度点云配准至0度点云。

delta的可通过以下方式调节：
delta太小，结果是输出的是单位矩阵，应该增大delta；
delta太大，结果是运行很久，内存一直增加，可能还会占满电脑所有内存然后程序崩掉，应该减小delta

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
    pcl::visualization::PCLVisualizer viewer("registration Viewer");
    //原始点云绿色
    pcl::visualization::PointCloudColorHandlerCustom src_h(pcd_src, 0, 255, 0);
    //目标点云红色
    pcl::visualization::PointCloudColorHandlerCustom tgt_h(pcd_tgt, 255, 0, 0);
    //匹配好的点云蓝色
    pcl::visualization::PointCloudColorHandlerCustom final_h(pcd_final, 0, 0, 255);

    viewer.setBackgroundColor(255, 255, 255);
    viewer.addPointCloud(pcd_src, src_h, "source cloud");
    viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
    viewer.addPointCloud(pcd_final, final_h, "result cloud");
    while (!viewer.wasStopped())
    {
        viewer.spinonce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

}

int main(int argc, char** argv)
{
    //加载点云文件
    PointCloud::Ptr cloud_source(new PointCloud);
    PointCloud::Ptr cloud_target(new PointCloud);

    //std::string filename = "point_source/hippo1-1224_s100.pcd";//
    std::string filename = "point_source/bun000_s100.pcd";//

    if (pcl::io::loadPCDFile(filename, *cloud_target) == -1)//*打开点云文件

    {
        PCL_ERROR("Couldn't read file test_pcd.pcdn");
        return(-1);
    }

    //filename = "point_source/hippo2-1224_s100.pcd";//
    filename = "point_source/bun045_s100.pcd";//

    if (pcl::io::loadPCDFile(filename, *cloud_source) == -1)//*打开点云文件

    {
        PCL_ERROR("Couldn't read file test_pcd.pcdn");
        return(-1);
    }


    //for (size_t i = 0; i < cloud_source->points.size(); ++i)
    //{
    //    cloud_source->points[i].x = cloud_source->points[i].x * 100.0;
    //    cloud_source->points[i].y = cloud_source->points[i].y * 100.0;
    //    cloud_source->points[i].z = cloud_source->points[i].z * 100.0;
    //}

    //for (size_t i = 0; i < cloud_target->points.size(); ++i)
    //{
    //    cloud_target->points[i].x = cloud_target->points[i].x * 100.0;
    //    cloud_target->points[i].y = cloud_target->points[i].y * 100.0;
    //    cloud_target->points[i].z = cloud_target->points[i].z * 100.0;
    //}
    pcl::io::savePCDFileBinary("point_source/hippo1-1224_s100.pcd", *cloud_target);
    pcl::io::savePCDFileBinary("point_source/hippo2-1224_s100.pcd", *cloud_source);

    //四点法配准
    PointCloud::Ptr pcs(new PointCloud);
    pcl::registration::FPCSInitialAlignment fpcs;

    fpcs.setInputSource(cloud_source);//输入待配准点云
    fpcs.setInputTarget(cloud_target);//输入目标点云

    //参数设置
    fpcs.setApproxOverlap(0.7);//两点云重叠度
    fpcs.setDelta(0.05);//Bunny
    //fpcs.setDelta(0.5);//hippo

    //fpcs.setNumberOfSamples(200);
    fpcs.setMaxComputationTime(50);
    Eigen::Matrix4f tras;
    clock_t start = clock();
    fpcs.align(*pcs);
    clock_t end = clock();
    cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << endl;
    cout << "score:" << fpcs.getFitnessScore() << endl;
    tras = fpcs.getFinalTransformation();
    cout << "matrix:" << endl << tras << endl << endl << endl;

    //for (int i = 0; i < 20; i++)//for循环20次用于查看多次实验下的不同结果
    //{
    //    clock_t start = clock();
    //    fpcs.align(*pcs);
    //    clock_t end = clock();
    //    cout << "time:" << (double)(end - start) / (double)CLOCKS_PER_SEC << endl;
    //    cout << "score:" << fpcs.getFitnessScore() << endl;
    //    tras = fpcs.getFinalTransformation();
    //    cout << "matrix:" << endl << tras << endl << endl << endl;
    //}


    PointCloud::Ptr cloud_end(new PointCloud);
    pcl::transformPointCloud(*cloud_source, *cloud_end, tras);

    visualize_pcd(cloud_source, cloud_target, cloud_end);
    return (0);


}

4pcs每次都是随机地采样，所以出来的结果每次是不一样的，而icp与3d-ndt在同样参数的情况下出来的结果是一样的，相对更好调参数；即使在最优的参数下，经过多次重复实验，发现会出现配准效果差或者直接跑偏的情况，这就感觉算法不稳定。

4-points congruent sets for robust pairwise surface registration

感谢参考来源

https://blog.csdn.net/qq_41102371/article/details/111715115