//
// Created by zx on 2021/9/17.
//

#ifndef FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_
#define FIND_CHASSIS__CHASSIS_CERES_SOLVER_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>
#include <iostream>
#include <string>
#include <chrono>

#include "error_code/error_code.hpp"

class chassis_ceres_solver {
public:
    chassis_ceres_solver();

    ~chassis_ceres_solver();

    Error_manager solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &z, double &w, double &h);

    // 利用图像优化，计算速度提升
    Error_manager solve_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &z, double &w, double &h,
                            bool update_debug_img = false);

    // 创建优化地图
    static cv::Mat create_mat();

    // 返回更新投影地图
    cv::Mat get_projected_mat() { return m_projected_mat; }

    // 返回投影地图转换的点云, 便于投影到pcl视野中
    pcl::PointCloud<pcl::PointXYZ>::Ptr get_projected_cloud();

private:
    static cv::Mat m_model;
    cv::Mat m_projected_mat;
};

#endif //FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_