puai_wj_2021/velodyne_lidar/match3d/detect_wheel_ceres3d.h

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

#ifndef DETECT_WHEEL_CERES_3D_H
#define DETECT_WHEEL_CERES_3D_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 "boost/format.hpp"
#include "hybrid_grid.hpp"

class detect_wheel_ceres3d {
public:
    struct Loss_result
    {
        public: 
        double lf_loss;
        double rf_loss;
        double lb_loss;
        double rb_loss;
        double total_avg_loss;
        Loss_result& operator=(const Loss_result& loss_result)
        {
            this->lf_loss = loss_result.lf_loss;
            this->rf_loss = loss_result.rf_loss;
            this->lb_loss = loss_result.lb_loss;
            this->rb_loss = loss_result.rb_loss;
            this->total_avg_loss = loss_result.total_avg_loss;
            return *this;
        }
    };
    struct Detect_result
    {
     public:
        double cx;
        double cy;
        double theta;
        double wheel_base;
        double width;
        double body_width;
        double front_theta;
        bool success;
        Loss_result loss;
        Detect_result& operator=(const Detect_result& detect_result)
        {
            this->cx = detect_result.cx;
            this->cy = detect_result.cy;
            this->theta = detect_result.theta;
            this->wheel_base = detect_result.wheel_base;
            this->width = detect_result.width;
            this->front_theta = detect_result.front_theta;
            this->success = detect_result.success;
            this->body_width=detect_result.body_width;
            this->loss=detect_result.loss;
            return *this;
        }
        std::string to_string()
        {
            char buf[512];
            sprintf(buf, "cx:%.3f cy:%.3f th:%.3f wb:%.3f wd:%.3f fth:%.3f suc:%s\n", cx, cy, theta, wheel_base, width, front_theta, success ? "true" : "false");
            return std::string(buf);
        }
    };
    detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
                         pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud);
    ~detect_wheel_ceres3d();
    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_vec, Detect_result &detect_result, std::string &error_info);

    void save_debug_data(std::string file );
protected:
    void save_model(std::string file);

    bool Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info);
    void get_costs(double* variable,double &lf, double &rf, double &lr, double &rr);
public:
    pcl::PointCloud<pcl::PointXYZ> m_left_front_transformed_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ> m_right_front_transformed_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ> m_left_rear_transformed_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ> m_right_rear_transformed_cloud;     //右后点云

    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;     //右后点云
 private:
    HybridGrid*                                  mp_left_grid;
    HybridGrid*                                  mp_right_grid;


};


#endif //DETECT_WHEEL_CERES_3D_H