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

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

class CostDynFunc{
    std::vector<double>   line_l_;
    std::vector<double>   line_r_;
    std::vector<double>   line_y_;
    double length_;
    double width_;

    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云

public:
    CostDynFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
        pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
        std::vector<double> linel,std::vector<double> liner,std::vector<double> liney,double width,double length)
    {
      line_l_=linel;
      line_r_=liner;
      line_y_=liney;
      width_=width;
      length_=length;
      m_left_front_cloud=left_front;
      m_right_front_cloud=right_front;
      m_left_rear_cloud=left_rear;
      m_right_rear_cloud=right_rear;

    }
    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 theta_front = variable[3];

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

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

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


      ceres::Grid1D<double> grid_l(line_l_.data(),0,line_l_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_l(grid_l);
      ceres::Grid1D<double> grid_r(line_r_.data(),0,line_r_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_r(grid_r);
      ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);


      //左前轮
      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->points[i].x) - cx),
                                           (T(m_left_front_cloud->points[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_l.Evaluate(point_rotation[0]*T(1000.)+T(200.),&residual[2*i]);
        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*i+1]);

      }
      //右前轮
      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->points[i].x) - cx),
                                           (T(m_right_front_cloud->points[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_r.Evaluate(point_rotation[0]*T(1000.)+T(1000.),&residual[2*left_front_num + 2*i]);
        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*left_front_num+2*i+1]);

      }
      //左后轮
      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->points[i].x) - cx),
                                           (T(m_left_rear_cloud->points[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
        interpolator_l.Evaluate(tanslate_point[0]*T(1000.)+T(200.),&residual[2*(left_front_num + right_front_num + i)]);
        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num+right_front_num)+2*i+1]);

      }

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

        interpolator_r.Evaluate(tanslate_point[0]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num + right_front_num + left_rear_num + i)]);
        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num+right_front_num+left_rear_num)+2*i+1]);

      }

      return true;
    }

};

class CostRYFunc{
private:
    std::vector<double>   line_y_;

    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云

public:
    CostRYFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
        pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
        std::vector<double> liney)
    {
      line_y_=liney;
      m_left_front_cloud=left_front;
      m_right_front_cloud=right_front;
      m_left_rear_cloud=left_rear;
      m_right_rear_cloud=right_rear;

    }
    template <typename T>
    bool operator()(const T* const variable, T* residual) const {
      T cy = variable[0];
      T theta = variable[1];
      T length = variable[2];

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

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


      ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
      T weight=T(0.3);
//左前轮

      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->points[i].x)),
                                           (T(m_left_front_cloud->points[i].y)));
        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_left_front;
        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[i]);
        residual[i]=residual[i]*weight;

      }
      //右前轮
      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->points[i].x) ),
                                           (T(m_right_front_cloud->points[i].y)));
        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_right_front;

        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[left_front_num+i]);
        residual[left_front_num+i]=residual[left_front_num+i]*weight;
      }
      //左后轮
      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->points[i].x)),
                                           (T(m_left_rear_cloud->points[i].y)));
        const Eigen::Matrix<T, 2, 1> tp(T(0),-cy);
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_left_rear;
        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[(left_front_num+right_front_num)+i]);

      }

      //右后轮

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

        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[left_front_num+right_front_num+left_rear_num+i]);

      }

      return true;
    }

private:

};

class CostXYWFFunc{
    std::vector<double>   line_l_;
    std::vector<double>   line_r_;
    std::vector<double>   line_y_;
    double length_;
    double theta_;

    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云

public:
    CostXYWFFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
                 std::vector<double> linel,std::vector<double> liner,std::vector<double> liney,
                 double theta,double length)
    {
      line_l_=linel;
      line_r_=liner;
      line_y_=liney;
      length_=length;
      theta_=theta;

      m_left_front_cloud=left_front;
      m_right_front_cloud=right_front;
      m_left_rear_cloud=left_rear;
      m_right_rear_cloud=right_rear;

    }
    template <typename T>
    bool operator()(const T* const variable, T* residual) const {
      T cx = variable[0];
      T cy = variable[1];
      T width = variable[2];
      T theta_front = variable[3];
      T theta=T(theta_);

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

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

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

      T weight=T(0.5);
      ceres::Grid1D<double> grid_l(line_l_.data(),0,line_l_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_l(grid_l);
      ceres::Grid1D<double> grid_r(line_r_.data(),0,line_r_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_r(grid_r);

      ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);

      //左前轮
      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->points[i].x) -cx),
                                           (T(m_left_front_cloud->points[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_l.Evaluate(point_rotation[0]*T(1000.)+T(200.),&residual[2*i]);
        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*i+1]);

        residual[2*i]=residual[2*i]*weight;

      }
      //右前轮
      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->points[i].x) - cx),
                                           (T(m_right_front_cloud->points[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_r.Evaluate(point_rotation[0]*T(1000.)+T(1000.),&residual[2*left_front_num + 2*i]);
        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*left_front_num+2*i+1]);
        residual[2*left_front_num + 2*i]=residual[2*left_front_num + 2*i]*weight;

      }
      //左后轮
      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->points[i].x) - cx),
                                           (T(m_left_rear_cloud->points[i].y) - cy));
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
        interpolator_l.Evaluate(tanslate_point[0]*T(1000.)+T(200.),&residual[2*(left_front_num + right_front_num + i)]);
        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num+right_front_num)+2*i+1]);

      }

      //右后轮

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

        interpolator_r.Evaluate(tanslate_point[0]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num + right_front_num + left_rear_num + i)]);
        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
                                &residual[2*(left_front_num+right_front_num+left_rear_num)+2*i+1]);

      }

      return true;
    }

};

detect_wheel_ceres::detect_wheel_ceres()
{
  FILE* filel= fopen("./cuve_l.txt","w");
  for(int i=0;i<1200;i++){
    double x=double(i-200)*0.001;
    double y=std::max(1.0/(1.+exp(-400.*(-x-0.02))) , 1.-exp(-x*x));
    m_line_l.push_back(y);
    fprintf(filel,"%f %f 0\n",x,y);

  }
  fclose(filel);

  FILE* filer= fopen("./cuve_r.txt","w");
  for(int i=0;i<1200;i++){
    double x=double(i-1000)*0.001;
    double y=std::max(1.0/(1.+exp(-400.*(x-0.02))) , 1.-exp(-x*x));
    m_line_r.push_back(y);
    fprintf(filer,"%f %f 0\n",x,y);

  }
  fclose(filer);

  FILE* filey= fopen("./cuve_y.txt","w");
  for(int i=0;i<2000;i++){
    double x=double(i-1000)*0.001;
    m_line_y.push_back(1.-exp(-0.5*x*x));
    m_line_y_2stp.push_back(1.-exp(-2*x*x));
    fprintf(filey,"%f %f 0\n",x,1.-exp(-0.5*x*x));
    fprintf(filey,"%f %f 0\n",x,1.-exp(-2*x*x));

  }
  fclose(filey);


}
detect_wheel_ceres::~detect_wheel_ceres(){}

bool detect_wheel_ceres::detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                 float& cx,float& cy,float& theta,float& wheel_base,float& width,
                 float& front_theta,float& loss){
  m_left_front_cloud= filter(cl1,1.5);
  m_right_front_cloud=filter(cl2,1.5);
  m_left_rear_cloud= filter(cl3,1.5);
  m_right_rear_cloud= filter(cl4,1.5);

  double variable[] = {cx,cy,theta,front_theta};
  auto t1=std::chrono::steady_clock::now();
  // 第二部分：构建寻优问题
  ceres::Problem problem;
  ceres::CostFunction* cost_function =new
      ceres::AutoDiffCostFunction<CostDynFunc, ceres::DYNAMIC, 4>(
      new CostDynFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
                       m_line_l,m_line_r,m_line_y,width,wheel_base),
      2*(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=1000;
  options.num_threads=2;
  options.minimizer_progress_to_stdout = false;//输出到cout
  ceres::Solver::Summary summary;//优化信息
  ceres::Solve(options, &problem, &summary);//求解!!!

  auto t2=std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
  double time= double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

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

  cx=variable[0];
  cy=variable[1];
  theta=variable[2];
  front_theta=variable[3];
  if(loss>0.05){
    return false;
  }

  //save(*m_left_front_cloud,*m_right_front_cloud,*m_left_rear_cloud,*m_right_rear_cloud,cx,cy,theta,wheel_base,width,front_theta);

  return true;
}

bool detect_wheel_ceres::detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                     float& cx,float& cy,float& theta,float& wheel_base,
                                     float& width,float& front_theta,float& loss){
    pcl::PointXYZ tp1,tp2,tp3,tp4;
    float yaw1=0.0;
    float yaw2=0.0;
    float yaw3=0.0;
    float yaw4=0.0;
    pcl::PointCloud<pcl::PointXYZ>::Ptr out1,out2,out3,out4;
    FallModel::fall(cl1,out1,tp1,yaw1,eNagtive);
    FallModel::fall(cl2,out2,tp2,yaw2,ePositive);
    FallModel::fall(cl3,out3,tp3,yaw3,eNagtive);
    FallModel::fall(cl4,out4,tp4,yaw4,ePositive);

    printf("yaw: %f %f %f %f\n",yaw1*180.0/M_PI,yaw2*180.0/M_PI,yaw3*180.0/M_PI,yaw4*180.0/M_PI);

    float angle_max = 10 * M_PI / 180.0;
    if (fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max) {
        static int error_index = 0;
        LOG(ERROR) << "left front cloud result: tp " << tp1 << " yaw " << yaw1;
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl1_" + std::to_string(error_index) + ".txt", cl1);
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out1_" + std::to_string(error_index) + ".txt", out1);
        LOG(ERROR) << "left front cloud result: tp " << tp2 << " yaw " << yaw2;
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl2_" + std::to_string(error_index) + ".txt", cl2);
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out2_" + std::to_string(error_index) + ".txt", out2);
        LOG(ERROR) << "left front cloud result: tp " << tp3 << " yaw " << yaw3;
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl3_" + std::to_string(error_index) + ".txt", cl3);
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out3_" + std::to_string(error_index) + ".txt", out3);
        LOG(ERROR) << "left front cloud result: tp " << tp4 << " yaw " << yaw4;
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl4_" + std::to_string(error_index) + ".txt", cl4);
        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out4_" + std::to_string(error_index) + ".txt", out4);
        error_index++;
    }

    return true;

}

bool detect_wheel_ceres::detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                  pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                      float& cx,float& cy,float& theta,float& wheel_base,float& width,
                                      float& front_theta,float& loss){
  m_left_front_cloud= filter(cl1,1.5);
  m_right_front_cloud=filter(cl2,1.5);
  m_left_rear_cloud= filter(cl3,1.5);
  m_right_rear_cloud= filter(cl4,1.5);

  bool ryl=detect_RYL(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
  if(ryl==false) return ryl;
  bool xwf=detect_XWF(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
  width += 0.04;
  return xwf;
}

bool detect_wheel_ceres::detect_RYL(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                    float& cx,float& cy,float& theta,float& wheel_base,float& width,
                                    float& front_theta,float& loss){




  double variable[] = {cy,theta,wheel_base};
  auto t1=std::chrono::steady_clock::now();
  // 第二部分：构建寻优问题
  ceres::Problem problem;
  ceres::CostFunction* cost_function =new
      ceres::AutoDiffCostFunction<CostRYFunc, ceres::DYNAMIC, 3>(
      new CostRYFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_line_y_2stp),
      (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=100;
  options.num_threads=2;
  options.minimizer_progress_to_stdout = false;//输出到cout
  ceres::Solver::Summary summary;//优化信息
  ceres::Solve(options, &problem, &summary);//求解!!!

  auto t2=std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/

  //cy=variable[0];
  theta=variable[1];
  wheel_base=variable[2];
  if(loss>0.05){
    return false;
  }
return true;

}

bool detect_wheel_ceres::detect_XWF(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                                    pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                    float& cx,float& cy,float& theta,float& wheel_base,float& width,
                                    float& front_theta,float& loss){

  double variable[] = {cx,cy,width,front_theta};
  auto t1=std::chrono::steady_clock::now();
  // 第二部分：构建寻优问题
  ceres::Problem problem;
  CostXYWFFunc* costFuc=new CostXYWFFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
                                  m_line_l,m_line_r,m_line_y,theta,wheel_base);
  ceres::CostFunction* cost_function =new
      ceres::AutoDiffCostFunction<CostXYWFFunc, ceres::DYNAMIC, 4>(costFuc,
      2*(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=100;
  options.num_threads=2;
  options.minimizer_progress_to_stdout = false;//输出到cout
  ceres::Solver::Summary summary;//优化信息
  ceres::Solve(options, &problem, &summary);//求解!!!

  auto t2=std::chrono::steady_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;

  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/

  cx=variable[0];
  cy=variable[1];
  width=variable[2];
  front_theta=variable[3];
  if(loss>0.05){
    return false;
  }
  return true;

}

pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r){
  if(cloud->size()==0)
    return cloud;
  float sumx=0;
  for(int i=0;i<cloud->size();++i){
    sumx+=cloud->points[i].x;
  }
  float avg=sumx/float(cloud->size());
  float stdd=0.0;
  for(int i=0;i<cloud->size();++i){
    stdd+=pow(cloud->points[i].x-avg,2);
  }
  stdd=sqrt(stdd/(float(cloud->size())));
  if(stdd<0.15)
    return cloud;
  pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
  for(int i=0;i<cloud->size();++i){
    if(fabs(cloud->points[i].x-avg)<r*stdd)
      out->push_back(cloud->points[i]);
  }
  return out;
}