CT_project/测量节点/wanji_lidar/detect_wheel_ceres.cpp

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

#include "detect_wheel_ceres.h"
#include <ceres/cubic_interpolation.h>
#include <pcl/common/transforms.h>


constexpr float kMinProbability = 0.01f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
constexpr int kPadding = INT_MAX / 4;
class GridArrayAdapter {
public:
    enum { DATA_DIMENSION = 1 };

    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}

    void GetValue(const int row, const int column, double* const value) const {
        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
            column >= NumCols() - kPadding) {
            *value = kMaxCorrespondenceCost;
        } else {
            *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
        }
    }

    int NumRows() const {
        return grid_.rows + 2 * kPadding;
    }

    int NumCols() const {
        return grid_.cols + 2 * kPadding;
    }
private:
    const cv::Mat& grid_;
};

class CostFunctor {
private:
    cv::Mat  m_map;
    double m_scale;
    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:
    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front, pcl::PointCloud<pcl::PointXYZ> right_front,
                pcl::PointCloud<pcl::PointXYZ> left_rear, pcl::PointCloud<pcl::PointXYZ> right_rear,
                cv::Mat &map, double scale)
    {
        m_map = map;
        m_scale = scale;
        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 costs[4]={T(0),T(0),T(0),T(0)};
	std::vector<std::vector<T> > deviation_calc_vector;
	deviation_calc_vector.resize(4);
	T deviation_weight = T(0.05);

        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 theta_front_weight = T(0.8);
//        T wheel_length_img_ratio = variable[6];
        T wheel_width_length_ratio = variable[6];
        // [1/3, 5/3]
        T limited_wheel_length_img_ratio = T(0.85);// = T(0.7) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.85);
        // [0.25, 0.35]
        T limited_wheel_length_ratio = T(0.25) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);

        //整车旋转
        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();


        const GridArrayAdapter adapter(m_map);
        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);

        double rows = m_map.rows;
        double cols = m_map.cols;
        //左前轮
        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;
            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[i]);
            //residual[i] += theta_front*theta_front_weight;
            costs[0] += residual[i];
	    deviation_calc_vector[0].push_back(residual[i]);
        }
        //右前轮
        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;
            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + i]);
            //residual[left_front_num + i] += theta_front*theta_front_weight;
            costs[1] += residual[left_front_num+i];
	    deviation_calc_vector[1].push_back(residual[left_front_num+i]);
        }
        //左后轮
        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;

            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + right_front_num + i]);
            costs[2] += residual[left_front_num + right_front_num + i];
	    deviation_calc_vector[2].push_back(residual[left_front_num + right_front_num + i]);
        }

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

            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
                                  &residual[left_front_num + right_front_num + left_rear_num + i]);
            costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
	    deviation_calc_vector[3].push_back(residual[left_front_num + right_front_num + left_rear_num + i]);
        }

	// 前轮旋转loss
	residual[left_front_num + right_front_num + left_rear_num + right_rear_num] = theta_front*theta_front_weight;
	// 四轮方差loss
	residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] = T(0);
	//printf("--------------\n");
	T avg_costs[4] = {T(costs[0]/T(left_front_num)), T(costs[1]/T(right_front_num)), T(costs[2]/T(left_rear_num)), T(costs[3]/T(right_rear_num))};
	for (int i = 0; i < deviation_calc_vector.size(); ++i) {
		T t_deviation = T(0);
		for (int j = 0; j < deviation_calc_vector[i].size(); ++j) {
			t_deviation += deviation_weight * ceres::abs(deviation_calc_vector[i][j] - avg_costs[i]);
		}
		residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] += t_deviation;
		//printf("??? dev: %.6f\n", t_deviation);
		//costs[i] = t_deviation;
		//printf("dev: %.6f\n", costs[i]);
	}

            char buf[30];
            memset(buf, 0, 30);
            sprintf(buf, "%.12f", costs[0]);
            double t_costs_lf = std::stod(buf) / left_front_num;

            memset(buf, 0, 30);
            sprintf(buf, "%.12f", costs[1]);
            double t_costs_rf = std::stod(buf) / right_front_num;

            memset(buf, 0, 30);
            sprintf(buf, "%.12f", costs[2]);
            double t_costs_lr = std::stod(buf) / left_rear_num;

            memset(buf, 0, 30);
            sprintf(buf, "%.12f", costs[3]);
            double t_costs_rr = std::stod(buf) / right_rear_num;
            if(t_costs_lf >= 0.001 && t_costs_rf >= 0.001 && t_costs_lr >= 0.001 && t_costs_rr >= 0.001)
            {
                m_costs_lf=t_costs_lf;
                m_costs_rf=t_costs_rf;
                m_costs_lr=t_costs_lr;
                m_costs_rr=t_costs_rr;
            }
        // m_costs_lf = costs[0];
	//printf("%.6f %.6f %.6f %.6f\n",m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr);
        return true;
    }
    void get_costs(double &lf, double &rf, double &lr, double &rr)
    {
        lf = m_costs_lf;
        rf = m_costs_rf;
        lr = m_costs_lr;
        rr = m_costs_rr;
    }
};

bool detect_wheel_ceres::update_mat(int rows, int cols)
{
    /////创建地图
    int L=std::min(rows,cols);
    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
    //map=map*255;
    cv::Point center(L/2,L/2);


    float K=L*0.08;
    // 从中心开始向外画矩形
    // 内层高斯，外层二次函数
    for(int n=0;n<L;n+=2)
    {
        cv::Size size(n+2,n+2);
        cv::Rect rect(center-cv::Point(n/2,n/2),size);
        double x = n / double(L);
        double sigma = 0.05;
        double miu = 0.0;
        double scale = 0.02;
        double translation = 0.03;
        double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
        double quadratic_value = std::max(2*x-0.25, 0.045);
        // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
        // 对内外分别取色
        cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));

        // if(n<K*2){
        //     cv::rectangle(map,rect,1.0*float(K*2-n)/float(L));
        // }
        // else{
        //     cv::rectangle(map,rect,float(n-K*2)/float(L));
        // }

    }
    // std::cout<<"map size: "<<map.size()<<std::endl;
    cv::resize(map,map,cv::Size(cols,rows));
    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
    return true;
}

detect_wheel_ceres::detect_wheel_ceres()
{
    int cols=800;
    int rows=800;
    update_mat(rows, cols);
}
detect_wheel_ceres::~detect_wheel_ceres(){}



bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> 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];
    //计算外接旋转矩形
    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.theta=-angle_x*M_PI/180.0;
    // Loss_result one_shot_loss_result;
    // return Solve(detect_result, one_shot_loss_result);

    // 多次优化，获取最佳优化结果
    detect_result.cx=center_x;
    detect_result.cy=center_y;
    // 已计算次数，初始传入值正常，之后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;
    // for (int j = 2; j < 4&&!stop_sign; j++)
    // {
    //     // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
    //     double map_rows = map_cols * 1.0 / j ;
    //     update_mat(map_rows, map_cols);

    // !!! 将车轮比例添加到优化模型中
        Detect_result t_detect_result = detect_result;
       // 寻找最小loss值对应的初始旋转角
        for (int i = -3; i < 4&&!stop_sign; i+=6)
        {
            t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
            // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
            Loss_result t_loss_result;
            t_loss_result.total_avg_loss = 1000;
            //输出角度已变化，需恢复成弧度
            if(calc_count > 0)
            {
                // t_detect_result.theta *= (-M_PI) / 180.0;
                t_detect_result.front_theta *= (-M_PI) / 180.0;
            }
            auto t1=std::chrono::system_clock::now();
	    std::string t_error_info;
            bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
    	    error_info = t_error_info;
            auto t2=std::chrono::system_clock::now();
            auto duration=t2-t1;
            static double second=0.0;
            second=std::chrono::duration_cast<std::chrono::milliseconds>(duration).count()/1000.0;
            total_solve_time+=second;
            // std::cout<<" time "<<second<<std::endl;
            // std::cout<<"current_result: "<<current_result<<std::endl;
            if(!current_result)
                continue;
            // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
            if (avg_loss > t_loss_result.total_avg_loss)
            {
                avg_loss = t_loss_result.total_avg_loss;
                // final_theta = -input_vars[2] * M_PI / 180.0;
                detect_result = t_detect_result;
                solve_result = current_result;
                loss_result = t_loss_result;
                // optimized_map_rows = map_rows;
                calc_count++;
                // // 新增，优化时间足够长则认为已找到正确结果
                // if(second > 0.02)
                // {
                //     stop_sign = true;
                //     break;
                // }
            }
        }
    // }
    // std::cout<<"solve time "<<total_solve_time<<std::endl;
    // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";

//    if(solve_result)
//        printf("final avg cost for four wheels: %7.6f %7.6f %7.6f %7.6f---%7.6f\n", loss_result.lf_loss, loss_result.rf_loss, loss_result.lb_loss, loss_result.rb_loss, loss_result.total_avg_loss);
    
    // // 生成并保存图片
    // update_mat(optimized_map_rows, map_cols);
    // Detect_result t_detect_result = detect_result;
    // t_detect_result.theta *= (-M_PI) / 180.0;
    // t_detect_result.front_theta *= (-M_PI) / 180.0;
    // Loss_result t_loss_result;
    // // LOG(INFO) <<"going to show img ";
    // Solve(detect_result, t_loss_result, true);

//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
//         detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
    if(solve_result)
        error_info="";
    return solve_result;

}

// 根据测量结果，生成四轮各中心十字星点云
void detect_wheel_ceres::transform_src(Detect_result detect_result)
{

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

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


    Eigen::Matrix<double, 2, 1> translate_car(detect_result.cx,detect_result.cy);

    const Eigen::Matrix<double, 2, 1> lt_center = rotation_matrix * wheel_center_normal_left_front+translate_car;
    const Eigen::Matrix<double, 2, 1> rt_center = rotation_matrix * wheel_center_normal_right_front+translate_car;
    const Eigen::Matrix<double, 2, 1> lb_center = rotation_matrix * wheel_center_normal_left_rear+translate_car;
    const Eigen::Matrix<double, 2, 1> rb_center = rotation_matrix * wheel_center_normal_right_rear+translate_car;

    create_mark(lt_center(0,0),lt_center(1,0),-detect_result.theta-detect_result.front_theta,m_left_front_cloud_out);
    create_mark(rt_center(0,0),rt_center(1,0),-detect_result.theta-detect_result.front_theta,m_right_front_cloud_out);
    create_mark(lb_center(0,0),lb_center(1,0),-detect_result.theta,m_left_rear_cloud_out);
    create_mark(rb_center(0,0),rb_center(1,0),-detect_result.theta,m_right_rear_cloud_out);
}

// 创建车轮中心十字星点云
void detect_wheel_ceres::create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud_out)
{
    cloud_out.clear();
    pcl::PointCloud<pcl::PointXYZ> cloud;

    //std::cout<<"1111111111"<<std::endl;
    int width=30;
    int height=10;
    double step=0.015;

    for(int i=0;i<width;++i)
    {
        pcl::PointXYZ point((i-width/2)*step,0,0);
        cloud.push_back(point);
    }
    for(int i=0;i<height;++i)
    {
        pcl::PointXYZ point(0,(i-height/2)*step,0);
        cloud.push_back(point);
    }
    //std::cout<<"22222222222"<<std::endl;
    //整车旋转
    Eigen::Rotation2D<double> rotation(theta);
    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();

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

        const Eigen::Matrix<double, 2, 1> point((double(cloud[i].x)),
                                                (double(cloud[i].y)));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point;
        pcl::PointXYZ point_out(tanslate_point(0,0)+x,tanslate_point(1,0)+y,0);
        cloud_out.push_back(point_out);

    }
}

bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
{
    if(m_left_front_cloud.size()<=1 || m_right_front_cloud.size()<=1 || m_left_rear_cloud.size()<=1 || m_right_rear_cloud.size()<=1)
    {
        return false;
    }
    double SCALE=300.0;
    double cx=detect_result.cx;
    double cy=detect_result.cy;
    double init_theta=detect_result.theta;
    double init_wheel_base=2.7;
    double init_width=1.55;
    double init_theta_front=0*M_PI/180.0;

    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
                    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.max_num_iterations=60;
    options.num_threads=1;
    options.minimizer_progress_to_stdout = false;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息

    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/

    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(loss>0.01)
        return false;
    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
    {
        return false;
    }

    detect_result.width+=0.15;//车宽+10cm
    // printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);

    // //added by yct
    avg_loss = loss; // 将loss传出

    Detect_result t_detect_result = detect_result;
    t_detect_result.theta *= -M_PI/180.0;
    t_detect_result.front_theta *= -M_PI/180.0;
    transform_src(t_detect_result);

    return true;

}


bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img)
{
    double SCALE=300.0;
    double cx=detect_result.cx;
    double cy=detect_result.cy;
    double init_theta=detect_result.theta;
    double init_wheel_base=2.7;
    double init_width=1.55;
    double init_theta_front=0*M_PI/180.0;
    // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
//    double init_wheel_length_img_ratio = 0.1;
    // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
    double init_wheel_width_length_ratio = -0.5;

    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_width_length_ratio};
    // printf("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;
    CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction* cost_function =new
            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 7>(
                    cost_func,
                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size()+2);
    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=60;
    options.num_threads=3;
    options.minimizer_progress_to_stdout = false;//输出到cout
    ceres::Solver::Summary summary;//优化信息
    ceres::Solve(options, &problem, &summary);//求解!!!

//    if(!summary.IsSolutionUsable())
//    {
//        error_info.append("检测失败\t");
//        return false;
//    }
    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
    // debug_img(detect_result, loss_result, true);
    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;

    //检验
    // printf("loss: %.5f\n", loss);
    // printf("middle 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]);
    if(loss>0.0115)
    {
        error_info.append("总loss过大 ").append(std::to_string(loss)).append("\t");
//        LOG(WARNING) <<"总loss过大 "<<loss;
        return false;
    }
    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")));
//        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.30): "<<detect_result.width<<", "<<detect_result.wheel_base;
        return false;
    }

    detect_result.width += 0.22; //车宽一边+11cm
//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
//             detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);

    // //added by yct
    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");
//        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
        return false;
    }
    // 将loss传出
    if(cost_func!=nullptr)
    {
        
        double costs_lf, costs_rf, costs_lr, costs_rr;
        cost_func->get_costs(costs_lf, costs_rf, costs_lr, costs_rr);
        // std::cout << "111"<< std::endl;
        loss_result.lf_loss = costs_lf;
        loss_result.rf_loss = costs_rf;
        loss_result.lb_loss = costs_lr;
        loss_result.rb_loss = costs_rr;
        loss_result.total_avg_loss = loss;
        // std::cout << "222"<< std::endl;
        if (costs_lf == costs_rf && costs_lf == costs_lf && costs_lf == costs_rr)// (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
        {
	    return false;
            loss_result.lf_loss = loss;
            loss_result.rf_loss = loss;
            loss_result.lb_loss = loss;
            loss_result.rb_loss = loss;
        }
        // 判断每个轮子平均loss是否满足条件
        double single_wheel_loss_threshold = 0.135;
        if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
        {
            error_info.append("四轮loss过大 ").append(std::to_string(loss_result.lf_loss)).append(", ")
                    .append(std::to_string(loss_result.rf_loss)).append(", ")
                    .append(std::to_string(loss_result.lb_loss)).append(", ")
                    .append(std::to_string(loss_result.rb_loss)).append("\t");
//            LOG(WARNING)<<error_info;
//            LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
            return false;
        }

        // std::cout<<"save img: "<<save_img<<std::endl;
//        debug_img(detect_result, loss_result, save_img);
    }
    else
    {
        return false;
    }

    Detect_result t_detect_result = detect_result;
    t_detect_result.theta *= -M_PI/180.0;
    t_detect_result.front_theta *= -M_PI/180.0;
    t_detect_result.width -= 0.22;
    transform_src(t_detect_result);

    return true;
}

// 显示/保存图片供调试用
void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, std::string out_img_path)
{
    double SCALE=300.0;
    cv::Mat lf = m_map.clone();
    cv::Mat rf = m_map.clone();
    cv::Mat lb = m_map.clone();
    cv::Mat rb = m_map.clone();
    //整车旋转
    Eigen::Rotation2D<double> rotation(detect_result.theta);

    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();

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

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

    // 左前轮
    for (size_t i = 0; i < m_left_front_cloud.size(); i++)
    {
        Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - detect_result.cx),
                                          (double(m_left_front_cloud[i].y) - detect_result.cy));
        //减去经过车辆旋转计算的左前中心
        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
        //旋转
        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
        cv::circle(lf, cv::Point(c, r), 1, cv::Scalar(150));
        // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
        // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
        // lf.at<uchar>(r, c) = 150;
    }
    //右前轮
    for (int i = 0; i < m_right_front_cloud.size(); ++i)
    {
        Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - detect_result.cx),
                                          (double(m_right_front_cloud[i].y) - detect_result.cy));
        //减去经过车辆旋转计算的左前中心
        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
        //旋转
        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
        cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
    }
    //左后轮
    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
    {
        Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - detect_result.cx),
                                          (double(m_left_rear_cloud[i].y) - detect_result.cy));
        //减去经过车辆旋转计算的左前中心
        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
        cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
    }

    //右后轮
    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
    {
        Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - detect_result.cx),
                                          (double(m_right_rear_cloud[i].y) - detect_result.cy));
        //减去经过车辆旋转计算的左前中心
        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
        cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
    }

    // cv::Mat rot90;// = (cv::Mat_<double>(2, 3)<<cos(90), -sin(90), 0, sin(90), cos(90), 0);
    // rot90 = cv::getRotationMatrix2D(cv::Point2f(lf.cols/2, lf.rows/2), 90, 1);
    // // std::cout<<rot90<<std::endl;
    // cv::warpAffine(lf, lf, rot90, cv::Size(lf.cols, lf.rows));
    // cv::flip()
    // cv::namedWindow("left front", cv::WINDOW_FREERATIO);
    // cv::namedWindow("right front", cv::WINDOW_FREERATIO);
    // cv::namedWindow("left back", cv::WINDOW_FREERATIO);
    // cv::namedWindow("right back", cv::WINDOW_FREERATIO);
    cv::flip(lf, lf, -1);
    cv::flip(rf, rf, -1);
    cv::flip(lb, lb, -1);
    cv::flip(rb, rb, -1);
    cv::Mat lft = lf.t(), rft = rf.t(), lbt = lb.t(), rbt = rb.t();
    // cv::imshow("left front", lf.t());
    // cv::imshow("right front", rf.t());
    // cv::imshow("left back", lb.t());
    // cv::imshow("right back", rb.t());
    // 写入各轮平均误差
    cv::putText(lft, (boost::format("lf %.6f") % (loss_result.lf_loss)).str().c_str(),
                cv::Point(lft.cols / 6, lft.rows / 4),
                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
    cv::putText(rft, (boost::format("rf %.6f") % (loss_result.rf_loss)).str().c_str(),
                cv::Point(rft.cols / 6, rft.rows / 4),
                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
    cv::putText(lbt, (boost::format("lb %.6f") % (loss_result.lb_loss)).str().c_str(),
                cv::Point(lbt.cols / 6, lbt.rows / 4),
                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
    cv::putText(rbt, (boost::format("rb %.6f") % (loss_result.rb_loss)).str().c_str(),
                cv::Point(rbt.cols / 6, rbt.rows / 4),
                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));

    cv::Mat total_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, m_map.type());
    lft.copyTo(total_img(cv::Rect(0, 0, m_map.rows, m_map.cols)));
    rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
    lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
    rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
    // cv::namedWindow("total img", CV_WINDOW_FREERATIO);
    // cv::imshow("total img", total_img);
    // cv::waitKey(20);
    if(save_img)
    {
        cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
        int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
        std::string img_filename="";
        if(out_img_path.empty()){
            img_filename = std::string("/home/youchen/Documents/measure/MainStructure/elecfence_ws/img/img").append(std::to_string(total_milli)).append(".jpg");
        }
        else{
            img_filename = out_img_path;
        }
        LOG(INFO) << "write to " << img_filename.c_str();
        // cv::cvtColor(total_img*255, cvted_img, CV_8U);
        cv::convertScaleAbs(total_img * 255, cvted_img);
        cv::imwrite(img_filename, cvted_img);
    }
}