//
// Created by zx on 2021/8/17.
//

#include "lidar_caliber.h"
#include <pcl/registration/ndt.h>
lidar_caliber::lidar_caliber()
{

}
bool lidar_caliber::caliber(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1
        ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,Eigen::Matrix<float,6,1>& transform1
        ,Eigen::Matrix<float,6,1>& transfoem2)
{
    //transform1=match();

}

Eigen::Matrix4f lidar_caliber::match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center
        ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Eigen::Matrix<float, 6,1> init_transform)
{
    //初始化正态分布变换（NDT）
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    //设置依赖尺度NDT参数
    //为终止条件设置最小转换差异
    ndt.setTransformationEpsilon(0.01);
    //为More-Thuente线搜索设置最大步长
    ndt.setStepSize(0.1);
    //设置NDT网格结构的分辨率（VoxelGridCovariance）
    ndt.setResolution(1.0);
    //设置匹配迭代的最大次数
    ndt.setMaximumIterations(35);
    // 设置要配准的点云
    ndt.setInputCloud(cloud);
    //设置点云配准目标
    ndt.setInputTarget(cloud_center);
    //设置使用机器人测距法得到的初始对准估计结果

    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(init_transform(0), Eigen::Vector3f::UnitX()));
    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(init_transform(1), Eigen::Vector3f::UnitY()));
    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(init_transform(2), Eigen::Vector3f::UnitZ()));
    Eigen::AngleAxisf rotation ;
    rotation = yawAngle*pitchAngle*rollAngle;
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

    transform_2.translation() << init_transform(3),init_transform(4),init_transform(5);	// 三个数分别对应X轴、Y轴、Z轴方向上的平移
    transform_2.rotate(rotation);	//同理，UnitX(),绕X轴；UnitY(

    Eigen::Matrix4f init_guess = transform_2.matrix();
    //计算需要的刚体变换以便将输入的点云匹配到目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    ndt.align(*output_cloud, init_guess);

    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
              << " score: " << ndt.getFitnessScore() << std::endl;
    //使用创建的变换对未过滤的输入点云进行变换
    return ndt.getFinalTransformation();
}