Measure/Modules/MeasureNode/velodyne_lidar/chassis_ceres_solver.cpp

//
// Created by zx on 2021/9/17.
//

#include "chassis_ceres_solver.h"

const float CERES_PA = (100.);
const float CERES_PB = (300.);
const float CERES_PC = (300.);

class chassis_ceres_cost {
private:
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //点云

public:
    chassis_ceres_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
            : m_cloud(cloud) {
    }

    template<typename T>
    bool operator()(const T *const variable, T *residual) const {
        T cx = variable[0];
        T cy = variable[1];
        T w = variable[2];
        T h = variable[3];

        const T PA = T(CERES_PA);
        const T PB = T(CERES_PB);
        const T PC = T(CERES_PC);
        //
        for (int i = 0; i < m_cloud->size(); ++i) {
            //先平移后旋转
            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)),
                                               (T(m_cloud->points[i].z)));
            T kx = point(0, 0);
            T ky = point(1, 0);
            T fx = 1.0 / (1.0 + ceres::exp(-PA * (kx - cx + w / 2.0))) -
                   1.0 / (1.0 + ceres::exp(-PA * (kx - cx - w / 2.0)));
            T fy = 1.0 / (1.0 + ceres::exp(-PB * (ky - cy + h / 2.0))) -
                   1.0 / (1.0 + ceres::exp(-PC * ((ky - cy) - h / 2.0)));

            residual[i] = fx * fy; //+(50./m_cloud->size())/(w*h+1e-16);
        }
        residual[m_cloud->size()] = T(1.0) / (w * h + 1e-16);
        return true;
    }

};

// 公式转图片优化
#include <ceres/cubic_interpolation.h>

constexpr float kMinProbability = 0.01f;
constexpr float kMaxProbability = 1.f - kMinProbability;
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
constexpr int kPadding = INT_MAX / 4;
constexpr float resolutionx = 0.01;
constexpr float resolutiony = 0.005;
constexpr float inner_width = 1.2;
constexpr float chassis_height = 0.14;
constexpr float startx = -2.0;
constexpr float starty = -0.1;
constexpr float endx = 2.0;
constexpr float endy = 0.3;

class GridArrayAdapter {
public:
    enum {
        DATA_DIMENSION = 1
    };

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

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

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

    int NumCols() const {
        return grid_.cols + 2 * kPadding;
    }

private:
    const cv::Mat &grid_;
};

class chassis_mat_cost {
private:
    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //点云
    cv::Mat m_mat;

public:
    chassis_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat mat)
            : m_cloud(cloud), m_mat(mat) {
    }

    template<typename T>
    bool operator()(const T *const variable, T *residual) const {
        T cx = variable[0];
        T cy = variable[1];
        // T w_ratio = variable[2]/T(inner_width);
        // T h_ratio = variable[3]/T(chassis_height);

        const GridArrayAdapter adapter(m_mat);
        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
        //
        for (int i = 0; i < m_cloud->size(); ++i) {
            //先平移后旋转
            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x) - cx),
                                               (T(m_cloud->points[i].z) - cy));
            T kx = (point(0, 0) - T(startx)) / T(resolutionx) + T(0.5 + kPadding);
            T ky = (point(1, 0) - T(starty)) / T(resolutiony) + T(0.5 + kPadding);
            interpolator.Evaluate(kx, ky, &residual[i]);

            // residual[i] = fx * fy; //+(50./m_cloud->size())/(w*h+1e-16);
        }
        // residual[m_cloud->size()] = (w_ratio - T(1.0));
        // residual[m_cloud->size()+1] = (h_ratio - T(1.0));
        return true;
    }

    // 生成debug图片
    void generate_mat(cv::Mat &mat, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double *variables) {
        mat = m_mat.clone();
        // double w_ratio = variables[2] / inner_width;
        // double h_ratio = variables[3] / chassis_height;
        for (size_t i = 0; i < cloud->size(); i++) {
            const Eigen::Vector2d point(
                    ((cloud->points[i].x - variables[0]) - startx) / resolutionx,
                    ((cloud->points[i].z - variables[1]) - starty) / resolutiony);
            cv::circle(mat, cv::Point2d(point.y(), point.x()), 2, cv::Scalar(0.4), 1);
            // std::cout << point.y() << ", " << point.x() << std::endl;
        }
    }

};

cv::Mat chassis_ceres_solver::m_model = chassis_ceres_solver::create_mat();

chassis_ceres_solver::chassis_ceres_solver() {}

chassis_ceres_solver::~chassis_ceres_solver() {}


/**
 * @description: 使用sigmoid函数寻找xoz平面平移以及内宽、底盘高度
 */
Error_manager
chassis_ceres_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &y, double &w, double &h) {
    double variable[] = {x, y, w, h};
    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    chassis_ceres_cost *cost_func = new chassis_ceres_cost(cloud);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction *cost_function = new
            ceres::AutoDiffCostFunction<chassis_ceres_cost, ceres::DYNAMIC, 4>(
            cost_func, cloud->size() + 1);
    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。



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

    double loss = summary.final_cost / (cloud->size());

    x = variable[0];
    y = variable[1];
    w = variable[2];
    h = variable[3];
//     printf(" loss : %f    x:%.5f   y:%.5f  w:%.5f  h:%.5f\n",
//             loss,variable[0],variable[1],variable[2],variable[3]);



    return SUCCESS;
}

/**
 * @description: 图片代替函数十倍提升优化速度
 */
Error_manager
chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &y, double &w, double &h,
                                bool update_debug_img) {
    // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
    // create_mat(m_model);
    // cv::imshow("model", m_model);
    // cv::waitKey(30);

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    // std::chrono::duration<double> time_used_mat = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0);

    double variable[] = {x, y, w, h};
    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/

    // 第二部分：构建寻优问题
    ceres::Problem problem;
    chassis_mat_cost *cost_func = new chassis_mat_cost(cloud, m_model);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    ceres::CostFunction *cost_function = new
            ceres::AutoDiffCostFunction<chassis_mat_cost, ceres::DYNAMIC, 2>(
            cost_func, cloud->size());
    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。

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

    // std::cout << summary.BriefReport() << std::endl;

    double loss = summary.final_cost / (cloud->size());

    x = variable[0];
    y = variable[1];
    // w = variable[2];
    // h = variable[3];
    // printf(" loss : %f    x:%.5f   y:%.5f  w:%.5f  h:%.5f\n",
    //         loss,variable[0],variable[1],variable[2],variable[3]);

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used_solve = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);

    if (update_debug_img) {
        // printf("midx:%.3f, midz:%.3f\n", x, y);
        cost_func->generate_mat(m_projected_mat, cloud, variable);
        cv::namedWindow("win", cv::WINDOW_FREERATIO);
        cv::imshow("win", m_projected_mat);
        cv::waitKey();
    }


    std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used_disp = std::chrono::duration_cast<std::chrono::duration<double>>(t3 - t2);
    // std::cout << "\ntime used for solve, display in ms: " << time_used_solve.count() << ", " << time_used_disp.count() << std::endl
    //           << std::endl;

    return SUCCESS;
}

cv::Mat chassis_ceres_solver::create_mat() {
    int rows = (endx - startx) / resolutionx + 1;
    int cols = (endy - starty) / resolutiony + 1;
    if (rows <= 1 || cols <= 1)
        return cv::Mat();
    cv::Mat t_mat(rows, cols, CV_32FC1);

    for (size_t i = 0; i < rows; i++) {
        for (size_t j = 0; j < cols; j++) {
            double x = i * resolutionx + startx;
            double y = j * resolutiony + starty;
            double fx = 1.0 / (1.0 + std::exp(-CERES_PA * (x + inner_width / 2.0))) -
                        1.0 / (1.0 + std::exp(-CERES_PA * (x - inner_width / 2.0)));
            double fy = 1.0 / (1.0 + std::exp(-CERES_PB * (y + chassis_height / 2.0))) -
                        1.0 / (1.0 + std::exp(-CERES_PC * (y - chassis_height / 2.0)));
            t_mat.at<float>(i, j) = fx * fy;
            // std::cout << "x,y: " << x << ", " << y << "-----value: " << 1.0 / (1.0 + std::exp(-CERES_PA * (x + default_width / 2.0)))<<", "<<
            //   1.0 / (1.0 + std::exp(-CERES_PA * (x - default_width / 2.0))) << std::endl;
            // if(fx*fy>0)
            //     std::cout << "x,y: " << x << "," << y << ", fx,fy: " << fx << ", " << fy << ", " << fx * fy << std::endl;
        }
    }
    cv::normalize(t_mat, t_mat, 1.0, 0.1, cv::NORM_MINMAX);
    // std::cout << t_mat << std::endl;

    // cv::namedWindow("win", cv::WINDOW_FREERATIO);
    // cv::imshow("win", t_mat);
    // cv::waitKey(0);

    return t_mat;
}


// 返回投影地图转换的点云, 便于投影到pcl视野中
pcl::PointCloud<pcl::PointXYZ>::Ptr chassis_ceres_solver::get_projected_cloud() {
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
            new pcl::PointCloud<pcl::PointXYZ>);
    if (m_projected_mat.empty())
        return t_cloud;
    for (size_t i = 0; i < m_projected_mat.rows; i++) {
        for (size_t j = 0; j < m_projected_mat.cols; j++) {
            double x0 = (i * resolutionx + startx);
            t_cloud->push_back(pcl::PointXYZ());
        }
    }
    return t_cloud;
}