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

#ifndef ROTATE_CANTER_CALIBER_HH
#define ROTATE_CANTER_CALIBER_HH
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include "../velodyne_lidar/match3d/detect_ceres3d.h"
#include <vector>

#include "../tool/singleton.h"

class rotate_center_caliber:public Singleton<rotate_center_caliber>
{
    typedef struct
    {
        float theta;
        float dx;
        float dy;
    }match_transform;
 // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
	friend class Singleton<rotate_center_caliber>;
 public:

  // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
	rotate_center_caliber(const rotate_center_caliber&)=delete;
	rotate_center_caliber& operator =(const rotate_center_caliber&)= delete;
 	~rotate_center_caliber()=default;

   void rotate_center_caliber_init();
   void rotate_center_caliber_uninit();

   void set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model);

   bool push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);

   static Eigen::Matrix4f match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Matrix<float, 6, 1> init_transform);

protected:
   bool solve();
   void updata_points();

private:
   // 父类的构造函数必须保护，子类的构造函数必须私有。
	rotate_center_caliber()=default;

 protected:
    pcl::PointCloud<pcl::PointXYZ>::Ptr             m_model_cloud;
    std::vector<match_transform>                    m_match_datas;

    detect_ceres3d*                                 mp_matcher;

    //待解算的参数， 原点中心坐标和旋转中心坐标
    pcl::PointXYZ                                   m_center;
    float       m_rotation_center_x;
    float       m_rotation_center_y;
};


#endif //ROTATE_CANTER_CALIBER_HH