Measure/Modules/MeasureNodeAlgTest/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"
#include "wheel_model_3d.h"
#include "tool/point3D_tool.h"

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() {
            lf_loss = 0;
            rf_loss = 0;
            lb_loss = 0;
            rb_loss = 0;
            total_avg_loss = 0;
        }
        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 wheel_width;
        double width;
        double length;
        double body_width;
        double front_theta;
        // 220110 added by yct
        double single_wheel_width;
        double single_wheel_length;

        bool success;
        Loss_result loss;
        Detect_result() {
            cx = 0;
            cy = 0;
            theta = 0;
            wheel_base = 0;
            wheel_width = 0;
            width = 0;
            length = 0;
            body_width = 0;
            front_theta = 0;
            single_wheel_width = 0;
            single_wheel_length = 0;
            success = false;
            loss = Loss_result();
        }

        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->wheel_width = detect_result.wheel_width;
            this->width = detect_result.width;
            this->length = detect_result.length;
            this->front_theta = detect_result.front_theta;
            this->single_wheel_width = detect_result.single_wheel_width;
            this->single_wheel_length = detect_result.single_wheel_length;

            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 wwd:%.3f  lg:%.3f fth:%.3f suc:%s sww:%.3f swl:%.3f ", cx, cy, theta, wheel_base, wheel_width, length, front_theta, success ? "true" : "false", single_wheel_width, single_wheel_length);
            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 wheebase_estimate(double &wheelbase, double &wheel_width, double &wheel_length);

    // 外接矩形估算车轮宽度，以此调整模型比例，此比例变换点云贴合模型
    bool wheel_size_estimate(double &wheel_width, double &wheel_length, double car_angle, float &ratio);

    // 根据点云计算外接矩形
    /**
      * @brief   输入点云，通过cv库计算点云的最小外接矩形，并判断矩形是否异常
      * @param   cloud 输入的点云数据;
      * @param   rect 返回最小外接矩形;
      * @param   theta 最小外接矩形的旋转角度，与cv库求最小外接矩形的angle不一样;
      * @param   length 外接矩形的长边;
      * @param   width 外接矩形的短边;
      * @return  如果输入点云异常或最小外接矩形的长不属于(0.3, 0.9)，宽不属于(0.1, 0.3)都会返回false
      */
    bool get_wheel_rect(pcl::PointCloud<pcl::PointXYZ> cloud, cv::RotatedRect &rect, double &theta, double &length, double &width);

    // 根据点云与模型比例参数更新模型
    // 模型更新很慢，通常仅初始化时调用该函数
    bool update_model(float ratio = 1.0f);

    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;

    // 左右模型原始点云, 用于动态调整模型大小
    pcl::PointCloud<pcl::PointXYZ> m_left_model_cloud;
    pcl::PointCloud<pcl::PointXYZ> m_right_model_cloud;
};


#endif //DETECT_WHEEL_CERES_3D_H