//
// Created by lz on 2023/4/3.
//
#pragma once

#include <random>
#include <ceres/ceres.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <opencv4/opencv2/opencv.hpp>
#include "define.pb.h"
#include "defines.hpp"
#include "tool/static_tool.hpp"
#include "third_src/interpolated_grid.hpp"

class CostFunctor3d {
private:
    const InterpolatedProbabilityGrid m_left_interpolated_grid;
    const InterpolatedProbabilityGrid m_right_interpolated_grid;
    pcl::PointCloud<pcl::PointXYZ> m_left_front_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ> m_right_front_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ> m_left_rear_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ> m_right_rear_cloud;     //右后点云
    // 20220118 added by yct
    float m_ratio; // 模型变换比例，用于贴合模型，cloud / ratio --> model

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

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

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


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

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

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

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

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

        return true;
    }

};

class WheelCeres3D {
public:
    // public函数
    WheelCeres3D() = default;

    ~WheelCeres3D() = default;

    bool init(const std::map<std::string, float> &files) {
        if (!loadModule(files, grids)) {
            return false;
        }

        DLOG(INFO) << "cloud module grid is well, test (0,0,0) :"
                   << grids[0].GetProbability(grids[0].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
        DLOG(INFO) << "cloud module grid is well, test (0,0,0) :"
                   << grids[1].GetProbability(grids[1].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
        return true;
    }

    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &cloud_vec, DetectResult &detect_result,
                std::string &error_info) {
        error_info = "";
        //清理点云W
        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 (const auto &i: cloud_vec) {
            cloud_all += (*i);
        }

        if (cloud_all.size() < 20) {
            LOG(WARNING) << "The point cloud data has " << cloud_all.size()
                         << " remaining points after Euclidean clustering.";
            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 (auto &i: cloud_all) {
            points_cv.emplace_back(i.x, 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.0f / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));

        //第二步, 将每份点云旋转回去,计算点云重心所在象限
        pcl::PointCloud<pcl::PointXYZ> t_cloud_left, t_cloud_right, t_cloud_front, t_cloud_rear;
        for (const auto &i: cloud_vec) {
            pcl::PointCloud<pcl::PointXYZ> cloud = (*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);

            //计算重心
            pcl::compute3DCentroid(transformed_cloud, centroid);
            double x = centroid[0];
            double y = centroid[1];
            //计算象限
            if (x > 0 && y > 0) {
                //第一象限
                m_left_front_cloud = cloud;
                t_cloud_left += (m_left_front_cloud);
                t_cloud_front += (m_left_front_cloud);
            }
            if (x > 0 && y < 0) {
                //第四象限
                m_right_front_cloud = cloud;
                t_cloud_right += (m_right_front_cloud);
                t_cloud_front += (m_right_front_cloud);
            }
            if (x < 0 && y > 0) {
                //第二象限
                m_left_rear_cloud = cloud;
                t_cloud_left += (m_left_rear_cloud);
                t_cloud_rear += (m_left_rear_cloud);
            }
            if (x < 0 && y < 0) {
                //第三象限
                m_right_rear_cloud = cloud;
                t_cloud_right += (m_right_rear_cloud);
                t_cloud_rear += (m_right_rear_cloud);
            }
        }

        Eigen::Vector4f centroid_left, centroid_right, centroid_front, centroid_rear;
        // 计算x
        pcl::compute3DCentroid(t_cloud_left, centroid_left);
        pcl::compute3DCentroid(t_cloud_right, centroid_right);
        // 计算y
        pcl::compute3DCentroid(t_cloud_front, centroid_front);
        pcl::compute3DCentroid(t_cloud_rear, centroid_rear);
        // LOG(WARNING)<<"x,y: "<<(centroid_left.x()+centroid_right.x())/2.0f<<", "<<(centroid_front.y()+centroid_rear.y())/2.0f;

        // 使用外部第一步优化产生的x，以及当前计算后的y作为初值
        // 多次优化，获取最佳优化结果
        // detect_result.cx = center_x;
        detect_result.set_cx(detect_result.cx());
        detect_result.set_cy((centroid_front.y() + centroid_rear.y()) / 2.0f);
        detect_result.set_wheel_base(centroid_front.y() - centroid_rear.y() + 0.12);
        // 优化结果
        bool solve_result = false;

        DetectResult t_detect_result = detect_result;

        LossResult t_loss_result;
        t_loss_result.set_total_avg_loss(1000);
        std::string t_error_info;
        bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
        error_info = t_error_info;

        if (current_result) {
            detect_result = t_detect_result;
//            detect_result.set_allocated_loss(&t_loss_result);
            detect_result.mutable_loss()->CopyFrom(t_loss_result);
            solve_result = current_result;
        }
        detect_result.set_success(solve_result);

        return solve_result;
    }

protected:
    // protected函数

private:
    // private函数
    bool loadModule(const std::map<std::string, float> &files, std::vector<HybridGrid> &grids) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr module(new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto &file: files) {
            DLOG(INFO) << "Load module:" << file.first << ", raido:" << file.second;
            module->clear();
//            HybridGrid grid(0.01);
            grids.emplace_back(0.01);
            if (!ReadTxtCloud(file.first, module)) {
                return false;
            }
            update_model(module, grids.back(), file.second);
        }
        return true;
    }


// 根据点云与模型比例参数更新模型
    bool update_model(pcl::PointCloud<pcl::PointXYZ>::Ptr &points, HybridGrid &grid, float ratio) {
        float del_x = 0.05;
        float del_y = 0.03;
        float del_z = 0.05;
        Eigen::Matrix3f delta = Eigen::MatrixXf::Identity(3, 3);
        delta(0, 0) = del_x * del_x;
        delta(1, 1) = del_y * del_y;
        delta(2, 2) = del_z * del_z;
        Eigen::Matrix3f delta_inv = delta.inverse();


        Eigen::Vector3f d(0, 0, 0.02);
        Eigen::MatrixXf ce = -0.5 * ((d).transpose() * delta_inv * (d));
        float cut_p = exp(ce(0, 0));

        DLOG(INFO) << "grid.resolution() = " << grid.resolution();

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

        return true;
    }

    bool Solve(DetectResult &detect_result, LossResult &loss_result, std::string &error_info) {
        if (grids.size() != 2) {
            LOG(ERROR) << "Cloud module grid is nullptr, please check it's initialization process.";
            return false;
        }
//        LOG(INFO) << "cloud module grid is well, test (0,0,0) :" << grids[0].GetProbability(grids[0].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
//        LOG(INFO) << "cloud module grid is well, test (0,0,0) :" << grids[1].GetProbability(grids[1].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
        float model_ratio = 1.0f;
        double cx = detect_result.cx();
        double cy = detect_result.cy();
        double init_theta = detect_result.theta();
        double init_wheel_base = 2.7;
        if (detect_result.wheel_base() > 2.55 && detect_result.wheel_base() < 3.2)
            init_wheel_base = detect_result.wheel_base();
        double init_width = 1.85;
        double init_theta_front = 0 * M_PI / 180.0;

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



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

        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.set_cx(variable[0]);
        detect_result.set_cy(variable[1]);
        detect_result.set_theta((-variable[2]) * 180.0 / M_PI);
        detect_result.set_wheel_base(variable[3]);
        detect_result.set_width(variable[4]);
        detect_result.set_front_theta(-(variable[5] * 180.0 / M_PI));

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

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

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

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

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

        return true;
    }

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

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

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

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


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

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

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

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

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

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

    // 定义函数对象
    struct CostFunction {
        explicit CostFunction(const cv::Point3f &point, const HybridGrid &iter) : x_(point.x), y_(point.y), z_(point.z),
                                                                                  iter_grid(iter) {}

        template<typename JetT>
        bool operator()(const JetT *const input, JetT *residual) const {
            JetT r = ((input[0] + x_) * ceres::cos(input[2]) - (input[1] + y_) * ceres::sin(input[2]));
            JetT c = ((input[0] + x_) * ceres::sin(input[2]) + (input[1] + y_) * ceres::cos(input[2]));
            JetT t(z_);

            residual[0] = iter_grid.GetInterpolatedValue(r, c, t) + 1.0;

//            LOG(INFO) << r << c << t << residual[0];

            return true;
        }

    private:
        const double x_;
        const double y_;
        const double z_;
        const InterpolatedGrid<HybridGrid> iter_grid;
    };

public:
    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;     //右后点云
    pcl::PointCloud<pcl::PointXYZ> m_left_front_transformed_cloud;     //左前点云
    pcl::PointCloud<pcl::PointXYZ> m_right_front_transformed_cloud;    //右前点云
    pcl::PointCloud<pcl::PointXYZ> m_left_rear_transformed_cloud;      //左后点云
    pcl::PointCloud<pcl::PointXYZ> m_right_rear_transformed_cloud;     //右后点云

protected:
    // protected成员

private:
    // private成员
//    HybridGrid *mp_left_grid;
//    HybridGrid *mp_right_grid;
    std::vector<HybridGrid> grids;

    // 左右模型原始点云, 用于动态调整模型大小
    pcl::PointCloud<pcl::PointXYZ> m_left_model_cloud;
    pcl::PointCloud<pcl::PointXYZ> m_right_model_cloud;
};
