//
// Created by zx on 2020/7/1.
//

#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
#define CERES_TEST_DETECT_WHEEL_CERES_H

#include <ceres/ceres.h>
#include <glog/logging.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>
#include <opencv2/opencv.hpp>

class detect_wheel_ceres {
public:
    static detect_wheel_ceres *iter() {
        static detect_wheel_ceres *instance = nullptr;
        if (instance == nullptr) {
            instance = new detect_wheel_ceres();
        }
        return instance;
    }

    ~detect_wheel_ceres();

    bool detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                      pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                      float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);

    /*
     * 只检测动态参数， 固定参数已知
     */
    bool detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);

private:
    detect_wheel_ceres();
    bool detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                    float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);

    bool detect_XWF(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                    pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                    float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);

    pcl::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r);


    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云

public:
    cv::Mat                                 m_map;                  //保存一份地图用作匹配
    cv::Mat                                 m_mapr;

    std::vector<double>                     m_line_l;
    std::vector<double>                     m_line_r;
    std::vector<double>                     m_line_y_2stp;
    std::vector<double>                     m_line_y;

};


#endif //CERES_TEST_DETECT_WHEEL_CERES_H