//
// 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:
    detect_wheel_ceres();
    ~detect_wheel_ceres();
    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta);

protected:
    bool update_mat(int rows, int cols);
    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss);

private:
    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
    cv::Mat                                 m_map;                  //保存一份地图用作匹配
};


#endif //CERES_TEST_DETECT_WHEEL_CERES_H
