puai_wj_2021/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 "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;     //右后点云


 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)
                  :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;
    }

    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];

        //整车旋转
        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;
            residual[i] = T(1.0) - m_left_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
                                T(m_left_front_cloud[i].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;

            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));
        }
        //左后轮
        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;

            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));
        }

        //右后轮
        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;

            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));
        }


        return true;
    }

};


detect_wheel_ceres3d::detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
                                           pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud)
{
    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;
    mp_left_grid=new HybridGrid(resolution);
    mp_right_grid=new HybridGrid(resolution);
    for(int i=0;i<left_grid_cloud->size();++i)
    {
// std::cout << "2222 "<< i << std::endl;
        pcl::PointXYZ pt=left_grid_cloud->points[i];
        Eigen::Vector3f u(pt.x,pt.y,pt.z);

        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_left_grid->resolution())
        {
            // std::cout << "aa" << std::endl;
            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_left_grid->resolution())
            {
                // std::cout << "bb" << std::endl;
                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_left_grid->resolution())
                {
                    // std::cout << "cc" << std::endl;
                    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);
                    // std::cout << "dd "<< index << std::endl;
                    if(mp_left_grid->GetProbability(index)<prob){
                        // std::cout << "e1 "<<index << std::endl;
                        mp_left_grid->SetProbability(index,prob);
                        // std::cout << "e2 "<<index << std::endl;
                    }
                    // std::cout << "ee "<<index << std::endl;

                }
            }
        }

    }
// std::cout << "3333" << std::endl;
    for(int i=0;i<right_grid_cloud->size();++i)
    {
        pcl::PointXYZ pt=right_grid_cloud->points[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);

                }
            }
        }

    }
// std::cout << "4444" << std::endl;
    // save_model("/home/zx/zzw/catkin_ws/src/feature_extra");

}

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");
}

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);
    //第二步, 将每份点云旋转回去,计算点云重心所在象限
    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;
        }
        if (x > 0 && y < 0)
        {
            //第四象限
            m_right_front_cloud = cloud;
        }
        if (x < 0 && y > 0)
        {
            //第二象限
            m_left_rear_cloud = cloud;
        }
        if (x < 0 && y < 0)
        {
            //第三象限
            m_right_rear_cloud = cloud;

        }

    }

    // 多次优化，获取最佳优化结果
    detect_result.cx = center_x;
    detect_result.cy = center_y;
    detect_result.wheel_base=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值对应的初始旋转角

    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;
    }

    return solve_result;

}

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];

    //整车旋转
    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));
        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)));
    }
    //左后轮
    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)));
    }

    //右后轮
    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)));
    }

    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::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info)
{
    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.85;
    double init_theta_front=0*M_PI/180.0;

    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front};
    // 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);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 6>(
                    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); //向问题中添加误差项，本问题比较简单，添加一个就行。



    //第三部分： 配置并运行求解器
    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=1;
    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.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;
    }
    /*printf("it : %d ,summary loss: %.5f \n",
           summary.iterations.size(),loss);*/

    if (detect_result.width < 1.25 || detect_result.width > 2.000 || detect_result.wheel_base > 3.15 || detect_result.wheel_base < 2.200)
    {
        error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.15): ").append(
                std::to_string(detect_result.width).append(", ").append(
                        std::to_string(detect_result.wheel_base)).append("\t")));
        return false;
    }

    if(detect_result.theta > 120 || detect_result.theta < 60)
    {
        error_info.append("车身角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
//        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
        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;
}