#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>
#include <pcl/registration/ia_kfpcs.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");
    //原始點(diǎn)云綠色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
    //目標(biāo)點(diǎn)云紅色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
    //匹配好的點(diǎn)云藍(lán)色
    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)
{
    //加載點(diǎn)云文件
    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 kpcs(new PointCloud);
    pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ>kfpcs;
    kfpcs.setInputSource(cloud_source);
    kfpcs.setInputTarget(cloud_target);

    //參數(shù)設(shè)置
    kfpcs.setApproxOverlap(0.7);
    kfpcs.setDelta(0.01);
    kfpcs.setMaxComputationTime(1000);
    kfpcs.setNumberOfSamples(200);
    //四點(diǎn)法配準(zhǔn)
    //PointCloud::Ptr pcs(new PointCloud);
    //pcl::registration::FPCSInitialAlignment<pcl::PointXYZ,pcl::PointXYZ> fpcs;
    //
    //fpcs.setInputSource(cloud_source);
    //fpcs.setInputTarget(cloud_target);

    //參數(shù)設(shè)置(別人參數(shù))
    //fpcs.setApproxOverlap(0.7);
    //fpcs.setDelta(0.01);
    //fpcs.setMaxComputationTime(1000);
    //fpcs.setNumberOfSamples(200);

    //fpcs.align(*pcs);

    kfpcs.align(*kpcs);
    Eigen::Matrix4f tras = kfpcs.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);


}

配準(zhǔn)結(jié)果

變換矩陣