#pragma once
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/registration/ia_fpcs.h>
#include <time.h>
#include <boost/thread/thread.hpp>

using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

int main() {
    // 读取点云
    PointCloud::Ptr cloud_target(new PointCloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("F:\\1.pcd", *cloud_target);
    PointCloud::Ptr cloud_source(new PointCloud);
    pcl::io::loadPCDFile<pcl::PointXYZ>("F:\\2.pcd", *cloud_source);
    cout << "加载完点云文件" << endl;

    // 四点法配准
    PointCloud::Ptr transformed_source(new PointCloud); // 创建一个空的点云用于存放变换后的源点云
    pcl::registration::FPCSInitialAlignment<PointT, PointT> fpcs;

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

    // 参数设置
    fpcs.setApproxOverlap(0.2); // 两点云重叠度
    fpcs.setDelta(0.5); // 参数设置，可能需要根据实际情况调整
    fpcs.setMaxComputationTime(50); // 最大计算时间
    fpcs.setNumberOfSamples(int(cloud_source->size() /20)); // 采样点数量
    cout << "配准参数设置完成，开始配准" << endl;
    clock_t start = clock(); // 记录开始时间
    fpcs.align(*transformed_source); // 执行配准
    clock_t end = clock(); // 记录结束时间
    cout << "完成配准" << endl;

    cout << "Time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " seconds" << endl;
    cout << "Score: " << fpcs.getFitnessScore() << endl;

    Eigen::Matrix4f final_transform = fpcs.getFinalTransformation(); // 获取最终变换矩阵
    cout << "Transformation Matrix:" << endl << final_transform << endl << endl << endl;

    // 可视化配准结果
    pcl::visualization::PCLVisualizer viewer("Point Cloud Registration");
    viewer.addPointCloud(cloud_target, "target");
    viewer.addPointCloud(transformed_source, "source");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source");
    viewer.spin();

    return 0;
}