#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
//#include <pcl/common/transforms.h>

int main(int argc, char **argv)
{
    // ndtオブジェクト
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

    // plyのロード
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPLYFile("../0000000000.ply", *target_cloud);
    pcl::io::loadPLYFile("../0000000010.ply", *source_cloud);
    std::cout << "load ply" << std::endl;

    // フィルタリング・オブジェクトの作成
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setLeafSize(1.0, 1.0, 1.0);

    sor.setInputCloud(source_cloud);
    sor.filter(*source_cloud);
    sor.setInputCloud(target_cloud);
    sor.filter(*target_cloud);

    // パラメータ設定
    ndt.setTransformationEpsilon(0.000001);
    ndt.setStepSize(1.0);
    ndt.setResolution(5.0);
    ndt.setMaximumIterations(30);
    
    // ポイント・クラウドの設定
    ndt.setInputSource(source_cloud);
    ndt.setInputTarget(target_cloud);

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    std::cout << "ndt execute" << std::endl;

    // ndtの実行
    ndt.align(*output_cloud);

    std::cout << "ndt fin" << std::endl;

    // 変換行列の取得
    Eigen::Matrix4f matrix4f = ndt.getFinalTransformation();
