低重叠率配准方法---四点法

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/features/normal_3d.h>//法线
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/math/special_functions/round.hpp>
#include <iostream>
#include <pcl/registration/ia_fpcs.h>




using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> 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<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> 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);

    pcl::io::loadOBJFile("E:/vc14/data/hippo1.obj", *cloud_source);
    pcl::io::loadOBJFile("E:/vc14/data/hippo2.obj", *cloud_target);

    //四点法配准
    PointCloud::Ptr pcs(new PointCloud);
    pcl::registration::FPCSInitialAlignment<pcl::PointXYZ,pcl::PointXYZ> fpcs;
    //
    fpcs.setInputSource(cloud_source);
    fpcs.setInputTarget(cloud_target);

    //参数设置(别人参数)
    fpcs.setApproxOverlap(0.7);
    fpcs.setDelta(0.01);
    fpcs.setMaxComputationTime(1000);
    fpcs.setNumberOfSamples(200);

    fpcs.align(*pcs);
    Eigen::Matrix4f tras = fpcs.getFinalTransformation();
    cout << "变换矩阵" << tras << endl;

    PointCloud::Ptr cloud_end(new PointCloud);
    pcl::transformPointCloud(*cloud_source,*cloud_end,tras);
    //pcl::io::loadOBJFile("E:/vc14/choose_bat/supre4pcs/super4pcs_fast.obj", *cloud_end);

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


}