/*
 * @Description: ICP SVD lidar odometry
 * @Author: Ge Yao
 * @Date: 2020-10-24 21:46:57
 */
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_

#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <pcl/kdtree/kdtree_flann.h>

class ICPSVDRegistration {
  public:
    ICPSVDRegistration(const YAML::Node& node);
    ICPSVDRegistration(
      float max_corr_dist, 
      float trans_eps, 
      float euc_fitness_eps, 
      int max_iter
    );

    bool SetInputTarget(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_target);
    bool ScanMatch(
      const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_source, 
      const Eigen::Matrix4f& predict_pose, 
      pcl::PointCloud<pcl::PointXYZ>::Ptr& result_cloud_ptr,
      Eigen::Matrix4f& result_pose
    );
  
  private:
    bool SetRegistrationParam(
      float max_corr_dist, 
      float trans_eps, 
      float euc_fitness_eps, 
      int max_iter
    );

  private:
    size_t GetCorrespondence(
      const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_source, 
      std::vector<Eigen::Vector3f> &xs,
      std::vector<Eigen::Vector3f> &ys
    );

    void GetTransform(
      const std::vector<Eigen::Vector3f> &xs,
      const std::vector<Eigen::Vector3f> &ys,
      Eigen::Matrix4f &transformation
    );

    bool IsSignificant(
      const Eigen::Matrix4f &transformation,
      const float trans_eps
    );

    float max_corr_dist_;
    float trans_eps_; 
    float euc_fitness_eps_; 
    int max_iter_;

    pcl::PointCloud<pcl::PointXYZ>::Ptr input_target_;
    pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr input_target_kdtree_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_source_;

    // Eigen::Matrix4f transformation_;
};

#endif