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

#ifndef SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_
#define SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include "match3d/detect_ceres3d.h"
#include <vector>
class rotate_center_caliber
{
    typedef struct
    {
        float theta;
        float dx;
        float dy;
    }match_transform;

 public:
    rotate_center_caliber();
    ~rotate_center_caliber();
    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();

 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 //SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_