Measure/Modules/MeasureNode/velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

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

#include "detect_wheel_ceres3d.h"
#include "interpolated_grid.hpp"
#include <pcl/common/transforms.h>
#include "tool/point_tool.h"

class CostFunctor3d
{
 private:
    const InterpolatedProbabilityGrid m_left_interpolated_grid;
    const InterpolatedProbabilityGrid m_right_interpolated_grid;
    mutable double m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr;
    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;     //右后点云
    // 20220118 added by yct
    float m_ratio; // 模型变换比例，用于贴合模型，cloud / ratio --> model

 public:
    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ> left_front, pcl::PointCloud<pcl::PointXYZ> right_front,
                pcl::PointCloud<pcl::PointXYZ> left_rear, pcl::PointCloud<pcl::PointXYZ> right_rear,
                  const HybridGrid& left_interpolated_grid,
                  const HybridGrid& right_interpolated_grid, float ratio)
                  :m_left_interpolated_grid(left_interpolated_grid),m_right_interpolated_grid(right_interpolated_grid)
    {
        m_left_front_cloud = left_front;
        m_right_front_cloud = right_front;
        m_left_rear_cloud = left_rear;
        m_right_rear_cloud = right_rear;
        m_costs_lf = 0.0;
        m_costs_rf = 0.0;
        m_costs_lr = 0.0;
        m_costs_rr = 0.0;
        m_ratio = ratio;
    }

    template<typename T>
    bool operator()(const T *const variable, T *residual) const
    {
        T cx = variable[0];
        T cy = variable[1];
        T theta = variable[2];
        T length = variable[3];
        T width = variable[4];
        T theta_front = variable[5];
        T lf_z = variable[6];
        T lr_z = variable[7];
        T rf_z = variable[8];
        T rr_z = variable[9];
//        T lf_z = T(0);
//        T lr_z = T(0);
//        T rf_z = T(0);
//        T rr_z = T(0);

        //整车旋转
        Eigen::Rotation2D<T> rotation(theta);
        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();

        //左前偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
        //右前偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
        //左后偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
        //右后偏移
        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);

        //前轮旋转
        Eigen::Rotation2D<T> rotation_front(theta_front);
        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();


        //左前轮
        int left_front_num = m_left_front_cloud.size();
        for (int i = 0; i < m_left_front_cloud.size(); ++i)
        {
            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
                                               (T(m_left_front_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
            //旋转
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * (tanslate_point*T(m_ratio));
            residual[i] = T(1.0) - m_left_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
                                T(m_left_front_cloud[i].z) + lf_z);
        }
        //右前轮
        int right_front_num = m_right_front_cloud.size();
        for (int i = 0; i < m_right_front_cloud.size(); ++i)
        {
            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
                                               (T(m_right_front_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
            //旋转
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * (tanslate_point*T(m_ratio));

            residual[left_front_num + i] = T(1.0) - m_right_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
                                                                                 T(m_right_front_cloud[i].z) + rf_z);
        }
        //左后轮
        int left_rear_num = m_left_rear_cloud.size();

        for (int i = 0; i < m_left_rear_cloud.size(); ++i)
        {
            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
                                               (T(m_left_rear_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = (rotation_matrix * point - wheel_center_normal_left_rear)*T(m_ratio);;

            residual[left_front_num + right_front_num + i] = T(1.0) -
                    m_left_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
                                                                                                  T(m_left_rear_cloud[i].z) + lr_z);
        }

        //右后轮
        int right_rear_num = m_right_rear_cloud.size();
        for (int i = 0; i < m_right_rear_cloud.size(); ++i)
        {
            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
                                               (T(m_right_rear_cloud[i].y) - cy));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = (rotation_matrix * point - wheel_center_normal_right_rear)*T(m_ratio);

            residual[left_front_num + right_front_num + left_rear_num+i] = T(1.0) -
                    m_right_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
                                                                  T(m_right_rear_cloud[i].z) + rr_z);
        }


        return true;
    }

};

detect_wheel_ceres3d::detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
                                           pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud)
{
    m_left_model_cloud.clear();
    m_right_model_cloud.clear();

    m_left_model_cloud += *left_grid_cloud;
    m_right_model_cloud += *right_grid_cloud;

    mp_left_grid = WheelModel3D::iter()->left_grid();
    mp_right_grid = WheelModel3D::iter()->right_grid();

//    mp_left_grid = nullptr;
//    mp_right_grid = nullptr;
//    update_model();
//    DLOG(INFO) <<"init model end";
//    save_model(ETC_PATH"MeasureNode/feature_extra");
}

// 根据点云与模型比例参数更新模型
bool detect_wheel_ceres3d::update_model(float ratio)
{
    float del_x=0.05;
    float del_y=0.03;
    float del_z=0.05;
    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
    delta(0,0)=del_x*del_x;
    delta(1,1)=del_y*del_y;
    delta(2,2)=del_z*del_z;
    Eigen::Matrix3f delta_inv=delta.inverse();


    Eigen::Vector3f d(0,0,0.02);
    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
    float cut_p=exp(ce(0,0));
// std::cout << "1111" << std::endl;
    float resolution=0.01;

    if(mp_left_grid!=nullptr){
        delete mp_left_grid;
        mp_left_grid = nullptr;
    }
    if(mp_right_grid != nullptr){
        delete mp_right_grid;
        mp_right_grid = nullptr;
    }

    mp_left_grid=new HybridGrid(resolution);
    mp_right_grid=new HybridGrid(resolution);

    // 变换模型点云
    pcl::PointCloud<pcl::PointXYZ> t_left_model;
    t_left_model += m_left_model_cloud;
    pcl::PointCloud<pcl::PointXYZ> t_right_model;
    t_right_model += m_right_model_cloud;

    for (size_t i = 0; i < t_left_model.size(); i++)
    {
        t_left_model[i].x = t_left_model[i].x * ratio;
        t_left_model[i].y = t_left_model[i].y * ratio;
        t_left_model[i].z = t_left_model[i].z * ratio;
    }
    for (size_t i = 0; i < t_right_model.size(); i++)
    {
        t_right_model[i].x = t_right_model[i].x * ratio;
        t_right_model[i].y = t_right_model[i].y * ratio;
        t_right_model[i].z = t_right_model[i].z * ratio;
    }

    for(int i=0;i<t_left_model.size();++i)
    {

        pcl::PointXYZ pt=t_left_model[i];
        Eigen::Vector3f u(pt.x,pt.y,pt.z);

        DLOG(INFO) << " ------------------------------------------------------------------------------------------------- "<< this;


        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_left_grid->resolution())
        {
            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_left_grid->resolution())
            {
                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_left_grid->resolution())
                {
                    Eigen::Vector3f p(x,y,z);
                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
                    float prob=exp(B(0,0));
                    if(prob>cut_p)
                        prob=cut_p;
                    Eigen::Array3i index=mp_left_grid->GetCellIndex(p);
                    if(mp_left_grid->GetProbability(index)<prob){
                        mp_left_grid->SetProbability(index,prob);
                    }
                }
            }
        }

    }
// std::cout << "3333" << std::endl;
    for(int i=0;i<t_right_model.size();++i)
    {
        pcl::PointXYZ pt=t_right_model[i];
        Eigen::Vector3f u(pt.x,pt.y,pt.z);

        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_right_grid->resolution())
        {
            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_right_grid->resolution())
            {
                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_right_grid->resolution())
                {
                    Eigen::Vector3f p(x,y,z);
                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
                    float prob=exp(B(0,0));
                    if(prob>cut_p)
                        prob=cut_p;
                    Eigen::Array3i index=mp_right_grid->GetCellIndex(p);
                    if(mp_right_grid->GetProbability(index)<prob)
                        mp_right_grid->SetProbability(index,prob);

                }
            }
        }
    }
    return true;
}

void detect_wheel_ceres3d::save_model(std::string dir)
{

    InterpolatedProbabilityGrid inter_grid(*mp_left_grid);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
    for(int x=0;x<mp_left_grid->grid_size();++x)
    {
        for(int y=0;y<mp_left_grid->grid_size();++y)
        {
            for(int z=0;z<mp_left_grid->grid_size();++z)
            {
                Eigen::Array3i index(x-mp_left_grid->grid_size()/2,
                        y-mp_left_grid->grid_size()/2,z-mp_left_grid->grid_size()/2);
                Eigen::Vector3f pt=mp_left_grid->GetCenterOfCell(index);

                float prob=inter_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
                if(prob>0.01)
                {
                    Eigen::Vector3f pt=mp_left_grid->GetCenterOfCell(index);
                    int rgb=int((prob-0.01)*255);
                    pcl::PointXYZRGB point;
                    point.x=pt[0];
                    point.y=pt[1];
                    point.z=pt[2];
                    point.r=rgb;
                    point.g=0;
                    point.b=255-rgb;
                    cloud_grid->push_back(point);
                }
            }
        }
    }


    InterpolatedProbabilityGrid right_grid(*mp_right_grid);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_right_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
    for(int x=0;x<mp_right_grid->grid_size();++x)
    {
        for(int y=0;y<mp_right_grid->grid_size();++y)
        {
            for(int z=0;z<mp_right_grid->grid_size();++z)
            {
                Eigen::Array3i index(x-mp_right_grid->grid_size()/2,
                                     y-mp_right_grid->grid_size()/2,z-mp_right_grid->grid_size()/2);
                Eigen::Vector3f pt=mp_right_grid->GetCenterOfCell(index);

                float prob=right_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
                if(prob>0.01)
                {
                    Eigen::Vector3f pt=mp_right_grid->GetCenterOfCell(index);
                    int rgb=int((prob-0.01)*255);
                    pcl::PointXYZRGB point;
                    point.x=pt[0];
                    point.y=pt[1];
                    point.z=pt[2];
                    point.r=rgb;
                    point.g=0;
                    point.b=255-rgb;
                    cloud_right_grid->push_back(point);
                }
            }
        }
    }

//    std::cout<<"  save model :"<<dir<<std::endl;
//    save_cloud_txt(cloud_grid,dir+"/left_model.txt");
//    save_cloud_txt(cloud_right_grid,dir+"/right_model.txt");
//    exit(EXIT_FAILURE);
}

detect_wheel_ceres3d::~detect_wheel_ceres3d(){
    delete mp_left_grid;
    delete mp_right_grid;
    mp_left_grid= nullptr;
    mp_right_grid= nullptr;
}



bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_vec,
        Detect_result &detect_result, std::string &error_info)
{
    error_info = "";
    //清理点云
    m_left_front_cloud.clear();
    m_right_front_cloud.clear();
    m_left_rear_cloud.clear();
    m_right_rear_cloud.clear();
    //重新计算点云,按方位分割
    //第一步,计算整体中心,主轴方向
    pcl::PointCloud<pcl::PointXYZ> cloud_all;
    for (int i = 0; i < cloud_vec.size(); ++i)
    {
        cloud_all += (*cloud_vec[i]);
    }

    // std::cout<<"cloud size: "<<cloud_all.size()<<std::endl;

    if (cloud_all.size() < 20)
        return false;

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(cloud_all, centroid);
    double center_x = centroid[0];
    double center_y = centroid[1];
    
    // printf("3dwheel x,y:%.3f, %.3f\n", center_x, center_y);
    // save_cloud_txt(cloud_all.makeShared(), "total_wheel.txt");

    //计算外接旋转矩形
    std::vector<cv::Point2f> points_cv;
    for (int i = 0; i < cloud_all.size(); ++i)
    {
        points_cv.push_back(cv::Point2f(cloud_all[i].x, cloud_all[i].y));
    }
    cv::RotatedRect rotate_rect = cv::minAreaRect(points_cv);
    //计算旋转矩形与X轴的夹角
    cv::Point2f vec;
    cv::Point2f vertice[4];
    rotate_rect.points(vertice);

    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
    // 寻找长边，倾角为长边与x轴夹角
    if (len1 > len2)
    {
        vec.x = vertice[0].x - vertice[1].x;
        vec.y = vertice[0].y - vertice[1].y;
    }
    else
    {
        vec.x = vertice[1].x - vertice[2].x;
        vec.y = vertice[1].y - vertice[2].y;
    }
    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
    // printf("rect theta: %.3f\n",angle_x);
    // 20211203 added by yct, 初始xy不准确，逻辑调整
    pcl::PointCloud<pcl::PointXYZ> t_cloud_left, t_cloud_right, t_cloud_front, t_cloud_rear;
    //第二步, 将每份点云旋转回去,计算点云重心所在象限
    for (int i = 0; i < cloud_vec.size(); ++i)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud = (*cloud_vec[i]);
        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
        // 平移
        traslation.translation() << -center_x, -center_y, 0.0;
        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
        pcl::transformPointCloud(cloud, translate_cloud, traslation);

        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
        rotation.rotate(Eigen::AngleAxisf(-angle_x * M_PI / 180.0, Eigen::Vector3f::UnitZ()));

        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);

        //计算重心
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid(transformed_cloud, centroid);
        double x = centroid[0];
        double y = centroid[1];
        //计算象限
        if (x > 0 && y > 0)
        {
            //第一象限
            m_left_front_cloud = cloud;
            t_cloud_left+=(m_left_front_cloud);
            t_cloud_front+=(m_left_front_cloud);
        }
        if (x > 0 && y < 0)
        {
            //第四象限
            m_right_front_cloud = cloud;
            t_cloud_right+=(m_right_front_cloud);
            t_cloud_front+=(m_right_front_cloud);
        }
        if (x < 0 && y > 0)
        {
            //第二象限
            m_left_rear_cloud = cloud;
            t_cloud_left+=(m_left_rear_cloud);
            t_cloud_rear+=(m_left_rear_cloud);
        }
        if (x < 0 && y < 0)
        {
            //第三象限
            m_right_rear_cloud = cloud;
            t_cloud_right+=(m_right_rear_cloud);
            t_cloud_rear+=(m_right_rear_cloud);
        }
    }
    // 20211203 added by yct, 重新计算xy
    Eigen::Vector4f centroid_left,centroid_right,centroid_front,centroid_rear;
    // 计算x
    pcl::compute3DCentroid(t_cloud_left, centroid_left);
    pcl::compute3DCentroid(t_cloud_right, centroid_right);
    // 计算y
    pcl::compute3DCentroid(t_cloud_front, centroid_front);
    pcl::compute3DCentroid(t_cloud_rear, centroid_rear);
    // LOG(WARNING)<<"x,y: "<<(centroid_left.x()+centroid_right.x())/2.0f<<", "<<(centroid_front.y()+centroid_rear.y())/2.0f;
    
    // 使用外部第一步优化产生的x，以及当前计算后的y作为初值
    // 多次优化，获取最佳优化结果
    detect_result.cx = detect_result.cx;
    detect_result.cy = (centroid_front.y()+centroid_rear.y())/2.0f;
    detect_result.wheel_base = centroid_front.y() - centroid_rear.y() + 0.12; //std::max(len1,len2);
    // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
    int calc_count = 0;
    // double final_theta = 0;
    // 误差结构体，保存左前、右前、左后、右后、整体平均误差
    Loss_result loss_result;
    // 平均误差值，用于获取最小整体平均误差
    double avg_loss = 100;
    // 定义图像列数，控制图像大小
    int map_cols = 800;
    // 优化后图像行数，用于保存优化后结果图像
    int optimized_map_rows = 200;
    // 优化结果
    bool solve_result = false;
    double total_solve_time = 0;
    bool stop_sign = false;

    Detect_result t_detect_result = detect_result;
    // 寻找最小loss值对应的初始旋转角
    // LOG(WARNING) <<cloud_all.size()<< ", car angle: "<<t_detect_result.theta<<", "<<(-angle_x ) * M_PI / 180.0;
    // t_detect_result.theta = (-angle_x ) * M_PI / 180.0;

    Loss_result t_loss_result;
    t_loss_result.total_avg_loss = 1000;
    //输出角度已变化，需恢复成弧度
    if (calc_count > 0)
    {
        t_detect_result.front_theta *= (-M_PI) / 180.0;
    }
    std::string t_error_info;
    bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
    error_info = t_error_info;

//     std::cout<<"current_result: "<<current_result<<std::endl;
    if (current_result)
    {
            avg_loss = t_loss_result.total_avg_loss;
            // final_theta = -input_vars[2] * M_PI / 180.0;
            detect_result = t_detect_result;
            detect_result.loss = t_loss_result;
            solve_result = current_result;
            loss_result = t_loss_result;
    }
    detect_result.success = solve_result;

    return solve_result;

}

// 将点云数据根据costFunction函数的求解进行求解，获取各个轮子的losses值
void detect_wheel_ceres3d::get_costs(double* variable,double &lf, double &rf, double &lr, double &rr)
{
    double losses[4]={0};
    double cx = variable[0];
    double cy = variable[1];
    double theta = variable[2];
    double length = variable[3];
    double width = variable[4];
    double theta_front = variable[5];
    double lf_z = variable[6];
    double lr_z = variable[7];
    double rf_z = variable[8];
    double rr_z = variable[9];

    //整车旋转
    Eigen::Rotation2D<double> rotation(theta);
    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();

    //左前偏移
    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
    //右前偏移
    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
    //左后偏移
    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
    //右后偏移
    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);

    //前轮旋转
    Eigen::Rotation2D<double> rotation_front(theta_front);
    Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();


    InterpolatedProbabilityGrid left_interpolated_grid(*mp_left_grid);
    InterpolatedProbabilityGrid right_interpolated_grid(*mp_right_grid);
    //左前轮
    int left_front_num = m_left_front_cloud.size();
    m_left_front_transformed_cloud.clear();
    for (int i = 0; i < m_left_front_cloud.size(); ++i)
    {
        const Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - cx),
                                                (double(m_left_front_cloud[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
        //旋转
        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
        losses[0] += 1.0 - left_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
                                                                         double(m_left_front_cloud[i].z) + lf_z);
        m_left_front_transformed_cloud.push_back(pcl::PointXYZ(point_rotation[0],point_rotation[1],m_left_front_cloud[i].z));
    }
    //右前轮
    int right_front_num = m_right_front_cloud.size();
    m_right_front_transformed_cloud.clear();
    for (int i = 0; i < m_right_front_cloud.size(); ++i)
    {
        const Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - cx),
                                                (double(m_right_front_cloud[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
        //旋转
        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;

        losses[1]+= 1.0 - right_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
                                                                         double(m_right_front_cloud[i].z));
        m_right_front_transformed_cloud.push_back(pcl::PointXYZ(point_rotation[0],point_rotation[1],
                                                                double(m_right_front_cloud[i].z) + rf_z));
    }
    //左后轮
    int left_rear_num = m_left_rear_cloud.size();
    m_left_rear_transformed_cloud.clear();
    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
    {
        const Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - cx),
                                                (double(m_left_rear_cloud[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;

        losses[2] += 1.0 -
                left_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
                                                              double(m_left_rear_cloud[i].z));
        m_left_rear_transformed_cloud.push_back(pcl::PointXYZ(tanslate_point[0],tanslate_point[1],
                                                              double(m_left_rear_cloud[i].z) + lr_z));
    }

    //右后轮
    int right_rear_num = m_right_rear_cloud.size();
    m_right_rear_transformed_cloud.clear();
    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
    {
        const Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - cx),
                                                (double(m_right_rear_cloud[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;

        losses[3]+= 1.0 -
                right_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
                                                               double(m_right_rear_cloud[i].z));
        m_right_rear_transformed_cloud.push_back(pcl::PointXYZ(tanslate_point[0],tanslate_point[1],
                                                               double(m_right_rear_cloud[i].z) + rr_z));
    }

    lf = losses[0];
    rf = losses[1];
    lr = losses[2];
    rr = losses[3];
}


void detect_wheel_ceres3d::save_debug_data(std::string dir )
{
    ::save_cloud_txt(m_left_front_transformed_cloud.makeShared(),dir+"/left_front_tranformed.txt");
    ::save_cloud_txt(m_right_front_transformed_cloud.makeShared(),dir+"/right_front_tranformed.txt");
    ::save_cloud_txt(m_left_rear_transformed_cloud.makeShared(),dir+"/left_rear_tranformed.txt");
    ::save_cloud_txt(m_right_rear_transformed_cloud.makeShared(),dir+"/right_rear_tranformed.txt");
}

// 外接矩形估算车轮宽度，以此调整模型比例
bool detect_wheel_ceres3d::wheel_size_estimate(double &wheel_width, double &wheel_length, double car_angle, float &ratio)
{
    // 分别对四轮寻找外接矩形
    int rect_res = 0;
    cv::RotatedRect rects[4]; // t_left_front_rect, t_right_front_rect, t_left_rear_rect, t_right_rear_rect;
    double rot_angles[4], lengths[4], widths[4];
    rect_res += get_wheel_rect(m_left_front_cloud, rects[0], rot_angles[0], lengths[0], widths[0]) ? 0 : 1;
    rect_res += get_wheel_rect(m_right_front_cloud, rects[1], rot_angles[1], lengths[1], widths[1]) ? 0 : 1;
    rect_res += get_wheel_rect(m_left_rear_cloud, rects[2], rot_angles[2], lengths[2], widths[2]) ? 0 : 1;
    rect_res += get_wheel_rect(m_right_rear_cloud, rects[3], rot_angles[3], lengths[3], widths[3]) ? 0 : 1;

    // 只允许一个异常轮胎
    if(rect_res > 1)
    {
        // std::cout << "异常轮胎过多 " << rect_res << std::endl;
        return false;
    }
    // 计算轮宽
    double avg_width = 0.0;
    double avg_length = 0.0;
    for (size_t i = 2; i < 4; i++)
    {
        avg_width += widths[i];
        avg_length += lengths[i];
    }
    avg_width /= 2.0;
    avg_length /= 2.0;
    // std::cout << "avg width: " << avg_width << ", avg length: " << avg_length << std::endl;

    // 以投影方式计算轮宽
    Eigen::Vector2d car_angle_vec(cos(-car_angle), sin(-car_angle));
    double left_min=INT_MAX, left_max=-INT_MAX, right_min=INT_MAX, right_max=-INT_MAX;
    if(m_left_rear_cloud.size()==0 || m_right_rear_cloud.size()==0)
    {
        ratio = 1.0f;
        return false;
    }
    for (size_t i = 0; i < m_left_rear_cloud.size(); i++)
    {
        Eigen::Vector2d point_vec(m_left_rear_cloud[i].x, m_left_rear_cloud[i].y);
        double res = sqrt(point_vec.squaredNorm() - pow(car_angle_vec.dot(point_vec),2));
        // std::cout<<"lres: "<<res<<std::endl;
        if(res<left_min)
            left_min=res;
        if(res>left_max)
            left_max=res;
    }
    for (size_t i = 0; i < m_right_rear_cloud.size(); i++)
    {
        Eigen::Vector2d point_vec(m_right_rear_cloud[i].x, m_right_rear_cloud[i].y);
        double res = sqrt(point_vec.squaredNorm() - pow(car_angle_vec.dot(point_vec),2));
        // std::cout<<"rres: "<<res<<std::endl;
        if(res<right_min)
            right_min=res;
        if(res>right_max)
            right_max=res;
    }
    double new_wheel_width = (left_max+right_max-left_min-right_min)/2.0;
    // std::cout << "avg width: " << avg_width << ", new width: " << new_wheel_width << std::endl;
    
    wheel_width = new_wheel_width;
    wheel_length = avg_length;

    // ratio = 1.0f;

    // 轮宽范围，实际0.185-0.3，通常会少0.03-0.04，（新方法不会少），因此设定范围0.175-0.275
    // 比例以半径0.64为模板，范围0.6-0.7
    if(wheel_width<0.18||wheel_width>0.3)
    {
        ratio = 1.0f;
    }else{
        float t_model_ratio = float((0.6 + ((wheel_width - 0.175) * 0.1 / 0.1)) / 0.64);
        t_model_ratio = std::max(0.6f/0.64f, t_model_ratio);
        t_model_ratio = std::min(0.7f/0.64f, t_model_ratio);
        ratio = 1.0f / t_model_ratio;
    }
    return true;
}

// 使用外接矩形，车轮越大，多条线照到车轮的概率与车轮半径都越大，导致外接矩形向内侧收拢的趋势更明显，轴距偏小得越厉害
bool detect_wheel_ceres3d::wheebase_estimate(double &wheelbase, double &wheel_width, double &wheel_length)
{
    // 分别对四轮寻找外接矩形
    int rect_res = 0;
    cv::RotatedRect rects[4]; // t_left_front_rect, t_right_front_rect, t_left_rear_rect, t_right_rear_rect;
    double rot_angles[4], lengths[4], widths[4];
    rect_res += get_wheel_rect(m_left_front_cloud, rects[0], rot_angles[0], lengths[0], widths[0]) ? 0 : 1;
    rect_res += get_wheel_rect(m_right_front_cloud, rects[1], rot_angles[1], lengths[1], widths[1]) ? 0 : 1;
    rect_res += get_wheel_rect(m_left_rear_cloud, rects[2], rot_angles[2], lengths[2], widths[2]) ? 0 : 1;
    rect_res += get_wheel_rect(m_right_rear_cloud, rects[3], rot_angles[3], lengths[3], widths[3]) ? 0 : 1;

    // 只允许一个异常轮胎
    if(rect_res > 1)
    {
        // std::cout << "异常轮胎过多 " << rect_res << std::endl;
        return false;
    }
    // 只用后轮计算轮宽
    double avg_width = 0.0;
    double avg_length = 0.0;
    for (size_t i = 2; i < 4; i++)
    {
        avg_width += widths[i];
        avg_length += lengths[i];
    }
    avg_width /= 2.0;
    avg_length /= 2.0;
    // std::cout << "avg width: " << avg_width << ", avg length: " << avg_length << std::endl;
    wheel_width = avg_width;
    wheel_length = avg_length;

    // 检查角度,后轮不可超过5度，前轮不可超过30度
    int rear_limit_count = 0, front_limit_count = 0;
    int rear_abnormal_index = -1, front_abnormal_index = -1;
    for (size_t i = 2; i < 4; i++)
    {
        if (90-rot_angles[i] > 5)
        {
            // std::cout << "后轮角度 " << rot_angles[i] << std::endl;
            rear_limit_count++;
            rear_abnormal_index = i;
        }
        // else
        // {
        //     // 后轮记录
        //     rear_index = i;
        // }
    }
    for (size_t i = 0; i < 2; i++)
    {
        if (90-rot_angles[i] > 30)
        {
            // std::cout << "前轮角度 " << rot_angles[i] << std::endl;
            front_limit_count++;
            front_abnormal_index = i;
        }
        // else
        // {
        //     // 前轮记录
        //     front_index = i;
        // }
    }
    // 异常矩形超过一个
    if (rear_limit_count + front_limit_count > 1)
    {
        // std::cout << "异常矩形过多 " << rear_limit_count<<", "<<front_limit_count << std::endl;
        return false;
    }
    // 剔除可能异常数据，计算轴距
    Eigen::Vector2d wb_vector;
    if (rear_abnormal_index != 2 && front_abnormal_index != 0)
    {
        wb_vector << rects[0].center.x - rects[2].center.x, rects[0].center.y - rects[2].center.y;
    }else if(rear_abnormal_index != 3 && front_abnormal_index != 1){
        wb_vector << rects[1].center.x - rects[3].center.x, rects[1].center.y - rects[3].center.y;
    }else{
        return false;
    }

    wheelbase = wb_vector.norm();
    double alpha = 0.2;
    wheelbase = wheelbase + alpha * (wheelbase - 2.55);

    return true;
}

bool detect_wheel_ceres3d::get_wheel_rect(pcl::PointCloud<pcl::PointXYZ> cloud, cv::RotatedRect &rect, double &theta, double &length, double &width)
{
    std::vector<cv::Point2f> t_point_vec;
    for (size_t i = 0; i < cloud.size(); i++)
    {
        t_point_vec.push_back(cv::Point2f(cloud[i].x, cloud[i].y));
    }
    if(t_point_vec.size()<=0)
        return false;
    rect = cv::minAreaRect(t_point_vec);

    length = std::max(rect.size.width, rect.size.height);
    width = std::min(rect.size.width, rect.size.height);
    // 车轮范围，轮胎直径600-800，宽度165-245
    if (length < 0.3 || length >0.9 || width < 0.1 || width > 0.3)
    {
        // std::cout << "车轮范围越界 0.3-0.9, 0.1-0.3: "<<length<<", "<<width << std::endl;
        return false;
    }

    // 获得真实旋转角,前轮y更大
    cv::Point2f corners[4];
    rect.points(corners);
    for (size_t i = 0; i < 4; i++)
    {
        for (size_t j = 0; j < 3-i; j++)
        {
            if(corners[j].y > corners[j+1].y)
            {
                std::swap(corners[j], corners[j + 1]);
            }
        }
    }
    Eigen::Vector2d rear2front;
    rear2front << corners[2].x + corners[3].x - corners[0].x - corners[1].x, corners[2].y + corners[3].y - corners[0].y - corners[1].y;
    rear2front /= 2;
    theta = std::atan2(rear2front.y(), rear2front.x()) * 180.0 / M_PI;

    return true;
}

bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info)
{
    // 20220118 added by yct. 计算比例传入模型
    float model_ratio = 1.0f;
    // bool ws_res = wheel_size_estimate(detect_result.single_wheel_width, detect_result.single_wheel_length, detect_result.theta, model_ratio);
    // std::cout << "change cloud with ratio: " << model_ratio << std::endl;

    double cx=detect_result.cx;
    double cy=detect_result.cy;
    double init_theta=detect_result.theta;
    double init_wheel_base=2.7;
    if(detect_result.wheel_base>2.55 && detect_result.wheel_base<3.2)
        init_wheel_base = detect_result.wheel_base;
    double init_width = 1.75;// detect_result.width;
    double init_theta_front = 0*M_PI/180.0;

    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, 0, 0, 0, 0};
    // printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
    //         variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    CostFunctor3d *cost_func = new CostFunctor3d(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
            *mp_left_grid,*mp_right_grid, model_ratio);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 10>(
                    cost_func,
                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。

    // 设置参数边界
    problem.SetParameterLowerBound(variable, 3, 2.00);    // 最小轴距
    problem.SetParameterUpperBound(variable, 3, 3.50);    // 最大轴距
    problem.SetParameterLowerBound(variable, 4, 1.0);     // 最小车宽
    problem.SetParameterUpperBound(variable, 4, 2.5);     // 最大车宽
    problem.SetParameterLowerBound(variable, 6, -0.1);
    problem.SetParameterUpperBound(variable, 6, 0.1);
    problem.SetParameterLowerBound(variable, 7, -0.1);
    problem.SetParameterUpperBound(variable, 7, 0.1);
    problem.SetParameterLowerBound(variable, 8, -0.1);
    problem.SetParameterUpperBound(variable, 8, 0.1);
    problem.SetParameterLowerBound(variable, 9, -0.1);
    problem.SetParameterUpperBound(variable, 9, 0.1);

    //第三部分： 配置并运行求解器
    ceres::Solver::Options options;
    // options.use_nonmonotonic_steps=false;
    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
    //options.logging_type = ceres::LoggingType::SILENT;
    options.max_num_iterations=500;
    options.num_threads=2;
    options.minimizer_progress_to_stdout = false;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

    // printf("after solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
    //         variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);

    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());

    detect_result.cx=variable[0];
    detect_result.cy=variable[1];
    detect_result.theta=(-variable[2])*180.0/M_PI;
    detect_result.wheel_base=variable[3];
    detect_result.wheel_width=variable[4];
    detect_result.front_theta=-(variable[5]*180.0/M_PI);

    if(detect_result.theta>180.0)
        detect_result.theta=detect_result.theta-180.0;
    if(detect_result.theta<0)
        detect_result.theta+=180.0;

    if(summary.iterations.size()<=1){
        error_info.append(std::string("仅迭代一轮，优化异常"));
        return false;
    }
    if(summary.iterations[summary.iterations.size()-1].iteration<=1){
        error_info.append(std::string("最终仅迭代一轮，优化异常"));
        return false;
    }

    if(detect_result.theta > 120 || detect_result.theta < 60)
    {
        error_info.append("车身角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
        return false;
    }
    if(fabs(detect_result.front_theta)>35)
    {
        error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta)).append("\t");
        return false;
    }
    // 将loss传出
    if(cost_func != nullptr)
    {
        double costs_lf, costs_rf, costs_lr, costs_rr;
        get_costs(variable,costs_lf, costs_rf, costs_lr, costs_rr);

        loss_result.lf_loss = costs_lf/float(m_left_front_cloud.size()+1e-6);
        loss_result.rf_loss = costs_rf/float(m_right_front_cloud.size()+1e-6);
        loss_result.lb_loss = costs_lr/float(m_left_rear_cloud.size()+1e-6);
        loss_result.rb_loss = costs_rr/float(m_right_rear_cloud.size()+1e-6);
        loss_result.total_avg_loss = loss;
        detect_result.loss=loss_result;
        if(loss_result.lf_loss>0.4 || loss_result.rf_loss>0.4
        ||loss_result.lb_loss>0.4||loss_result.rb_loss>0.4)
        {
            error_info.append("loss过大 lf").append(std::to_string(loss_result.lf_loss)).append("，rf").append(std::to_string(loss_result.rf_loss)).append(", lr").
            append(std::to_string(loss_result.lb_loss)).append("，rb").append(std::to_string(loss_result.rb_loss));
            return false;
        }

    }
    else
    {
        return false;
    }

    return true;
}