puai_wj_2021/velodyne_lidar/car_pose_detector.h

/*
 * @Description: 底盘z高度检测类,准确识别出平移,旋转,车宽,底盘高度，供3D车轮算法优化出车辆准确信息
 * @Author: yct
 * @Date: 2021-09-16 15:02:49
 * @LastEditTime: 2021-09-22 15:01:45
 * @LastEditors: yct
 */

#ifndef Z_DETECTOR_HH
#define Z_DETECTOR_HH

#include <iostream>
#include <chrono>
#include <ceres/ceres.h>
#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "../tool/singleton.h"

// 代价函数的计算模型
struct Car_pose_cost
{
    Car_pose_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr) : m_cloud_ptr(cloud_ptr) {}

    // 残差的计算
    template <typename T>
    bool operator()(
        const T *const vars, // 模型参数，x y theta w
        T *residual) const
    {
        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
        {
            std::cout << "error occured" << std::endl;
            return false;
        }
        const T norm_scale = T(0.002);
        //residual[0] = T(0);
        // residual[1] = T(0);
        // residual[m_cloud_ptr->size()] = T(0.0);
        // 点云loss
        const T width = T(2.0); //vars[3];
        for (int i = 0; i < m_cloud_ptr->size(); i++)
        {
            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
            Eigen::Rotation2D<T> rotation(vars[2]);
            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;

            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.2))));
            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.2))));
            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
            // if(left_loss > T(0.01))
            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
        }

        // // 参数L2正则化loss
        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
        return true;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
};

#include <opencv2/opencv.hpp>
// 代价函数的计算模型
struct Trans_mat_cost
{
    Trans_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat mat) : m_cloud_ptr(cloud_ptr), m_mat(mat) {}

    // 残差的计算
    template <typename T>
    bool operator()(
        const T *const vars, // 模型参数，x y theta w
        T *residual) const
    {
        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
        {
            std::cout << "error occured" << std::endl;
            return false;
        }
        const T norm_scale = T(0.002);
        //residual[0] = T(0);
        // residual[1] = T(0);
        // residual[m_cloud_ptr->size()] = T(0.0);
        // 点云loss
        const T width = T(2.0); //vars[3];
        for (int i = 0; i < m_cloud_ptr->size(); i++)
        {
            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
            Eigen::Rotation2D<T> rotation(vars[2]);
            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;

            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.4))));
            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.4))));
            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
            // if(left_loss > T(0.01))
            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
        }

        // // 参数L2正则化loss
        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
        return true;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
    cv::Mat m_mat;
};

class Car_pose_detector : public Singleton<Car_pose_detector>
{
    friend class Singleton<Car_pose_detector>;
public:
    Car_pose_detector(const Car_pose_detector &) = delete;
    Car_pose_detector &operator=(const Car_pose_detector &) = delete;
    ~Car_pose_detector() = default;

    // 变换点云
    void inv_trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);

    // 变换点云
    void trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);

    // 创建两轴具体曲线
    void create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double width);

    // 检测底盘z方向值，去中心，
    bool detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud=false);

private:
    // 父类的构造函数必须保护，子类的构造函数必须私有。
    Car_pose_detector() = default;
};
#endif // !Z_DETECTOR_HH