Vzense-Tof-3D/detect/ceres_detect/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>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h> // 拟合直线
#include <pcl/ModelCoefficients.h>          //导入ModelCoefficients（模型系数）结构
#include <pcl/filters/project_inliers.h>        //导入ProjectInliers滤波器

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 CostRXFunc {
private:
    std::vector<double> line_x_;

    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:
    CostRXFunc(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> linex) {
        line_x_ = linex;
        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 theta = variable[1];


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


        ceres::Grid1D<double> grid_x(line_x_.data(), 0, line_x_.size());
        ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_x(grid_x);

        T weight = T(1.0);
//左前轮

        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(cx, T(0));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point + tp;
            interpolator_x.Evaluate(point_rotation[0] * 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(cx, T(0));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point + tp;
            interpolator_x.Evaluate(point_rotation[0] * 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(cx, T(0));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point + tp;
            interpolator_x.Evaluate(tanslate_point[0] * 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(cx, T(0));
            //减去经过车辆旋转计算的左前中心
            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point + tp;
            interpolator_x.Evaluate(tanslate_point[0] * 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);

    FILE *filex = fopen("./cuve_x.txt", "w");
    for (int i = 0; i < 2000; i++) {
        double x = double(i - 1000) * 0.001;
        double y = 1.0 + 1.0 / (1.0 + exp(-49. * (x - 0.9))) - 1 / (1 + exp(-49. * (x + 0.9)));
        m_line_x.push_back(y);
        fprintf(filex, "%f %f 0\n", x, y);

    }
    fclose(filex);


}

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

float cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1, pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
           pcl::PointCloud<pcl::PointXYZ>::Ptr cl3, pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
           std::vector<double> line_y_,
           float cy, float wheelbase, float theta) {
    //整车旋转
    Eigen::Rotation2D<float> rotation(-theta);
    Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();

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


    ceres::Grid1D<double> grid_y(line_y_.data(), 0, line_y_.size());
    ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
    float weight = 1;
//左前轮
    float loss = 0.;
    int left_front_num = cl1->size();
    FILE *lf = fopen("./lf.txt", "w");
    FILE *rf = fopen("./rf.txt", "w");
    FILE *lb = fopen("./lb.txt", "w");
    FILE *rb = fopen("./rb.txt", "w");
    for (int i = 0; i < cl1->size(); ++i) {
        const Eigen::Matrix<float, 2, 1> point(cl1->points[i].x, cl1->points[i].y);
        const Eigen::Matrix<float, 2, 1> tp((0), -cy);
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<float, 2, 1> point_rotation = rotation_matrix * point + tp - wheel_center_normal_left_front;
        double cost = 0.0;
        interpolator_y.Evaluate(point_rotation[1] * float(1000.) + float(1000.), &cost);
        cost = cost * weight;
        loss += cost * cost;
        fprintf(lf, "%f %f %f\n", point_rotation[1], cl1->points[i].z, point_rotation[0]);

    }

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

        double cost = 0.0;
        interpolator_y.Evaluate(point_rotation[1] * float(1000.) + float(1000.), &cost);
        cost = cost * weight;
        loss += cost * cost;

        fprintf(rf, "%f %f %f\n", point_rotation[1], cl2->points[i].z, point_rotation[0]);

    }
    //左后轮
    int left_rear_num = cl3->size();

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

        double cost = 0.0;
        interpolator_y.Evaluate(tanslate_point[1] * float(1000.) + float(1000.), &cost);
        cost = cost * weight;
        loss += cost * cost;
        fprintf(lb, "%f %f %f\n", tanslate_point[1], cl3->points[i].z, tanslate_point[0]);

    }

    //右后轮

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

        double cost = 0.0;
        interpolator_y.Evaluate(tanslate_point[1] * float(1000.) + float(1000.), &cost);
        cost = cost * weight;
        loss += cost * cost;
        fprintf(rb, "%f %f %f\n", tanslate_point[1], cl4->points[i].z, tanslate_point[0]);

    }
    fclose(lf);
    fclose(rf);
    fclose(lb);
    fclose(rb);
    return loss;

}

bool detect_wheel_ceres::ransicLine(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                    pcl::PointXYZ &center, double &theta) {
    if (cloud->size() < 10)
        return false;
    for (int i = 0; i < cloud->size(); ++i) {
        cloud->points[i].z = 0;
    }

    pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr model_line(
            new pcl::SampleConsensusModelLine<pcl::PointXYZ>(cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_line);
    ransac.setDistanceThreshold(0.02);    //内点到模型的最大距离
    ransac.setMaxIterations(1000);        //最大迭代次数
    ransac.computeModel();                //直线拟合

    //-------------------------根据索引提取内点--------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<int> inliers;                //存储内点索引的容器
    ransac.getInliers(inliers);            //提取内点索引
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_line);
    //----------------------------输出模型参数---------------------------
    Eigen::VectorXf coefficient;
    ransac.getModelCoefficients(coefficient);


    theta = atan(coefficient[3] / coefficient[4]);

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(6);
    for (int i = 0; i < 6; ++i) {
        coefficients->values[i] = coefficient[i];
    }
    // ------------------------------投影到平面---------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_LINE);   //创建投影类型，投影到直线上
    proj.setInputCloud(cloud_line);
    proj.setModelCoefficients(coefficients);
    proj.filter(*cloud_projected);

    Eigen::Vector4f centroid;  //质心
    pcl::compute3DCentroid(*cloud_projected, centroid); // 计算质心
    center = pcl::PointXYZ(centroid[0], centroid[1], 0);
    return true;
}

bool detect_wheel_ceres::detect_ransic(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) {
    // cl4->clear();

    pcl::PointXYZ center1, center2, center3, center4;
    double theta1, theta2, theta3, theta4;

    pcl::PointXYZ vertexs[4];
    std::vector<pcl::PointXYZ> correct_vertexs;
    for (int i = 0; i < 4; ++i) vertexs[i] = pcl::PointXYZ(0, 0, 0);

    if (ransicLine(cl1, center1, theta1)) {
        vertexs[0] = (center1);
        correct_vertexs.push_back(center1);
    }
    if (ransicLine(cl2, center2, theta2)) {
        vertexs[1] = (center2);
        correct_vertexs.push_back(center2);
    }
    if (ransicLine(cl3, center3, theta3)) {
        vertexs[2] = (center3);
        correct_vertexs.push_back(center3);
    }
    if (ransicLine(cl4, center4, theta4)) {
        vertexs[3] = (center4);
        correct_vertexs.push_back(center4);
    }
    std::vector<pcl::PointXYZ> rect_points = correct_vertexs;
    if (!isRect(rect_points))
        return false;
    if (correct_vertexs.size() == 4) {
        pcl::PointXYZ fcenter = pcl::PointXYZ((center1.x + center2.x) * 0.5, (center1.y + center2.y) * 0.5, 0);
        pcl::PointXYZ bcenter = pcl::PointXYZ((center3.x + center4.x) * 0.5, (center3.y + center4.y) * 0.5, 0);
        cx = (fcenter.x + bcenter.x) * 0.5;
        cy = (fcenter.y + bcenter.y) * 0.5;
        double thetal = atan((center1.x - center3.x) / (center1.y - center3.y));
        double thetar = atan((center2.x - center4.x) / (center2.y - center4.y));
        theta = (thetal + thetar) * 0.5;
        width = 0.5 * sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2)) +
                0.5 * sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
        wheel_base = sqrt(pow(fcenter.x - bcenter.x, 2) + pow(fcenter.y - bcenter.y, 2));
    } else if (correct_vertexs.size() == 3) {
        int errorId = 0;
        for (int i = 0; i < 4; ++i) {
            if (fabs(vertexs[i].y) < 1e-5) {
                errorId = i;
                break;
            }
        }
        if (errorId == 0) {
            cx = (center2.x + center3.x) * 0.5;
            cy = (center2.y + center3.y) * 0.5;
            theta = atan((center2.x - center4.x) / (center2.y - center4.y));
            wheel_base = sqrt(pow(center2.x - center4.x, 2) + pow(center2.y - center4.y, 2));
            width = sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
            front_theta = theta2;
        } else if (errorId == 1) {
            cx = (center1.x + center4.x) * 0.5;
            cy = (center1.y + center4.y) * 0.5;
            theta = atan((center1.x - center3.x) / (center1.y - center3.y));
            wheel_base = sqrt(pow(center1.x - center3.x, 2) + pow(center1.y - center3.y, 2));
            width = sqrt(pow(center3.x - center4.x, 2) + pow(center3.y - center4.y, 2));
            front_theta = theta1;
        } else if (errorId == 2) {
            cx = (center1.x + center4.x) * 0.5;
            cy = (center1.y + center4.y) * 0.5;
            theta = atan((center2.x - center4.x) / (center2.y - center4.y));
            wheel_base = sqrt(pow(center2.x - center4.x, 2) + pow(center2.y - center4.y, 2));
            width = sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2));
            front_theta = (theta1 + theta2) * 0.5;
        } else if (errorId == 3) {
            cx = (center2.x + center3.x) * 0.5;
            cy = (center2.y + center3.y) * 0.5;
            theta = atan((center1.x - center3.x) / (center1.y - center3.y));
            wheel_base = sqrt(pow(center1.x - center3.x, 2) + pow(center1.y - center3.y, 2));
            width = sqrt(pow(center1.x - center2.x, 2) + pow(center1.y - center2.y, 2));
            front_theta = (theta1 + theta2) * 0.5;
        }
        front_theta -= theta;
        return recorrect_width(cl3, cl4, theta, width);
    } else {
        return false;
    }
    
    front_theta = (theta1 + theta2) * 0.5 - theta;
    return recorrect_width(cl3, cl4, theta, width);
}

bool detect_wheel_ceres::recorrect_width(pcl::PointCloud<pcl::PointXYZ>::Ptr left_cl,
                                         pcl::PointCloud<pcl::PointXYZ>::Ptr right_cl,
                                         float theta, float &width) {
    int NUMS[] = {100, 200, 400, 800, 1600};
    float COVS[] = {0.04 * 0.04, 0.02 * 0.02, 0.01 * 0.01, 0.005 * 0.005, 0.01 * 0.01};
    float widthCov[2] = {0, 100.0};
    int correct_count = 0;
    for (int i = 0; i < 5; ++i) {
        float w = 0;
        if (compute_width(left_cl, right_cl, NUMS[i], theta, w)) {
            float K = widthCov[1] / (widthCov[1] + COVS[i]);
            widthCov[0] = widthCov[0] + K * (w - widthCov[0]);
            widthCov[1] = widthCov[1] * (1.0 - K);
            correct_count++;
        }
    }
    if (correct_count > 0) {
        width = widthCov[0];
        return true;
    }
    return false;
}

bool detect_wheel_ceres::compute_width(pcl::PointCloud<pcl::PointXYZ>::Ptr cl3, pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
                                       int NUM, float &theta, float &width) {
    auto t1 = std::chrono::steady_clock::now();
    if (cl3->size() == 0 && cl4->size() == 0) {
        return false;
    }

    double vx = cos(theta);
    double vy = sin(theta);

    double per = 4.0 / NUM;
    int *counts = new int[NUM];
    memset(counts, 0, NUM * sizeof(int));

    for (int i = 0; i < cl3->size(); ++i) {
        float dot = cl3->points[i].x * vx + cl3->points[i].y * vy;
        int id = int(dot / per + 0.5) + NUM / 2;
        if (id >= 0 && id < NUM) {
            counts[id] += 1;
        }
    }
    for (int i = 0; i < cl4->size(); ++i) {
        float dot = cl4->points[i].x * vx + cl4->points[i].y * vy;
        int id = int(dot / per + 0.5) + NUM / 2;
        if (id >= 0 && id < NUM)
            counts[id] += 1;
    }

    double *grade = new double[NUM];
    memset(grade, 0, NUM * sizeof(double));

    double ming = 0, maxg = 0;
    for (int i = 1; i < NUM; ++i) {
        grade[i - 1] = counts[i] - counts[i - 1];

        if (grade[i - 1] < ming && i > NUM / 2) ming = grade[i - 1];
        if (grade[i - 1] > maxg && i < NUM / 2) maxg = grade[i - 1];
    }
    int l = -1, r = -1;
    double weight = 0.3;
    for (int i = 0; i < NUM - 1; ++i) {
        if (grade[i] > maxg * weight) {
            l = i;
            break;
        }
    }
    for (int i = NUM - 1; i > 0; --i) {
        if (grade[i] < ming * weight) {
            r = i;
            break;
        }
    }
    delete[] counts;
    delete[] grade;

    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;


    if (l >= 0 && r >= 0) {
        width = (r - l) * per;
        return true;
    }
    return false;
}

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) {

    //cl3->clear();

    m_left_front_cloud = filter(cl1, 1);
    m_right_front_cloud = filter(cl2, 1);
    m_left_rear_cloud = filter(cl3, 1);
    m_right_rear_cloud = filter(cl4, 1);


    bool ryl = detect_RYL(m_left_front_cloud, m_right_front_cloud, m_left_rear_cloud, m_right_rear_cloud,
                          cx, cy, theta, wheel_base, width, front_theta, loss);
    if (ryl == false) return ryl;

    bool xwf = detect_XWF(m_left_front_cloud, m_right_front_cloud, m_left_rear_cloud, m_right_rear_cloud,
                          cx, cy, theta, wheel_base, width, front_theta, loss);
    return xwf;
}

bool detect_wheel_ceres::detect_RX(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);
    m_right_front_cloud = filter(cl2, 1);
    m_left_rear_cloud = filter(cl3, 1);
    m_right_rear_cloud = filter(cl4, 1);

    double variable[] = {cx, theta};
    auto t1 = std::chrono::steady_clock::now();
    // 第二部分：构建寻优问题
    ceres::Problem problem;
    ceres::CostFunction *cost_function = new
            ceres::AutoDiffCostFunction<CostRXFunc, ceres::DYNAMIC, 2>(
            new CostRXFunc(m_left_front_cloud, m_right_front_cloud, m_left_rear_cloud, m_right_rear_cloud, m_line_x),
            (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.function_tolerance = 1e-10;
    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];
    theta = variable[1];
    if (loss > 0.05) {
        return false;
    }
    return true;

}

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.function_tolerance = 1e-10;
    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());

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

    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::transformRY(
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float theta, float y) {
    //整车旋转
    Eigen::Rotation2D<float> rotation(theta);
    Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
    pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i = 0; i < cloud->size(); ++i) {
        const Eigen::Matrix<float, 2, 1> point(cloud->points[i].x, cloud->points[i].y);
        const Eigen::Matrix<float, 2, 1> tp(0.0, y);
        //减去经过车辆旋转计算的左前中心
        const Eigen::Matrix<float, 2, 1> point_transform = rotation_matrix * point + tp;
        out->push_back(pcl::PointXYZ(point_transform[0], point_transform[1], cloud->points[i].z));
    }
    return out;
}

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

/*
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);

  save_cloud_txt(out1,"./src1.txt");
  save_cloud_txt(out2,"./src2.txt");
  save_cloud_txt(out3,"./src3.txt");
  save_cloud_txt(out4,"./src4.txt");

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

  return true;

}*/