CT_project/测量节点/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;
}