CleanRobot-UESTC/融合/fusion.h

#pragma once
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <stdint.h>
#include "lidar.h"
#include "ranging.h"

using namespace cv;
using namespace std;

struct ImgScan
{
    float angleMin;         //弧度
    float angleMax;
    float angleInc;
    std::vector<float> distance;    // 米
    std::vector<uint8_t> intensities;
    Mat pointCloud;
    std::vector<std::vector<Mat>> clustedCloud;
};
class Ranging;      //友元类
class LCFusion
{
public:
    LCFusion(Ranging* ranging)
    {
        r = ranging;
        rotate = cameraExtrinsicMat.rowRange(0, 3).colRange(0, 3).t();      // 雷达 -> 相机坐标系旋转矩阵
        translation = cameraExtrinsicMat.col(3).rowRange(0, 3);            
        translation = -rotate * translation;                     // 雷达 -> 相机坐标系平移矩阵
        p2r = r_y * r_c;                // 像素坐标系矫正矩阵（旋转）
    };
    char fusion(std::vector<Mat> &camInfo);     // fusion函数
    uchar lidar2box(Mat uv, Mat boxes, uchar *boxFlag);     // 无用
    void filter_laser(const FrameData &lidarInfo, float angle_min, float angle_inc);    // 雷达点和相机视野同步
    void scan_to_pointcloud();      // 雷达极坐标到二维坐标转换
    std::vector<int> clusters_bboxs(Mat &bboxs);    // 所有的聚类的雷达簇和所有的检测框的匹配
    int cluster_bboxs(std::vector<Mat> &one_cluster, Mat &bboxs);       // 寻找一簇雷达簇匹配的检测框
    void optimize_clusterlable(vector<int> & clusterlable, Mat & bboxs);        // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
    void choose_forceground(vector<int> & cluster_num, Mat & bbox, vector<int> & clusterlable);         // 针对镂空物体选择对应的雷达簇，筛选过程
    void big_fusion(vector<int> & clusterlable, Mat & bboxs,  Mat &dwha);           // 大物体赋语义
    void small_fusion(Mat & bboxs, Mat & dwha);         // 小物体赋语义
    Mat pointcloud_pixel(Mat &pointcloud);              // 二维雷达点到像素坐标系转换
    void cluster();         // 聚类
    float ratio_in_1box(Mat &begin, Mat &end, Mat &box);        // 计算一簇雷达落入检测框的比例
    void draw_point(std::vector<Mat> &camInfo);                 // 画出雷达在图像中的映射点
    void draw_circle(int cluster_num, int bbox_num, Mat &bboxs,  Mat &dwha);    // 扩散物体生成圆弧状伪激光雷达
    vector<float> find_circle_center(int cluster_num);              // 寻找扩散物体圆弧的圆心
    float circle(float d, float alfa ,float theta, float data);         // 圆弧角度和距离的数学关系
    vector<float> width_ladar(const int* coordinates);
    void small_box_without_dis(Mat & bboxs, Mat & dwha);            // 检测框到雷达坐标系的映射关系
    void set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData);            // 设置雷达发布数据（原始雷达）
    void set_laser_data(sensor_msgs::LaserScan& scan);              // 设置雷达发布数据（语义雷达）

    ImgScan upScan;
    // 调试使用，将雷达点映射到图像中显示
    vector<Point2f> projectedPoints;        // 雷达 -> 像素坐标系映射的点
    std::vector<uchar> clusterIdx;

private:
    Ranging* r;
    //float cameraAnglemin = 1.117, cameraAnglemax = 2.0071;
	float cameraAnglemin = 1.345, cameraAnglemax = 2.0595;          // 相机视野角度
    Mat rotate, translation, p2r; // 雷达点映射矫正
    Mat cameraExtrinsicMat = (Mat_<float>(4, 4) << 9.9710325100937192e-01, 5.9677185357037893e-02,
                              -4.7156551765403301e-02, 2.6591864512557874e-02,
                              4.7136795335845721e-02, 1.7395474156941537e-03,
                              9.9888692878636431e-01, 7.4034983851991365e-02,
                              5.9692791457662736e-02, -9.9821621281296091e-01,
                              -1.0784828889205400e-03, -9.6176066499866750e-02, 0., 0., 0., 1.);

    Mat cameraMat = (Mat_<float>(3, 3) << 5.0280958247840368e+02, 0., 3.1925413622163182e+02, 0.,
                     5.0206199606708202e+02, 2.1999125419411467e+02, 0., 0., 1.);

    Mat distCoeff = (Mat_<float>(1, 5) << 1.2903666948900971e-01, -9.2734018203156590e-02,
                     -7.0528508350750380e-03, 8.3183285124261049e-04,
                     7.2727934388070986e-02);

    // Mat c2lR = (Mat_<float>(2, 2) << 0.99664806, 0.0802792, -0.0802792, 0.99664806);
    Mat c2lR = (Mat_<float>(2, 2) << 0.98163, 0.19081, -0.19081, 0.98163);	//11
    //Mat c2lR = (Mat_<float>(2, 2) << 0.9703, 0.24192, -0.24192, 0.9703);	//13
    Mat c2lT = (Mat_<float>(2, 1) << 0.15, 0.01);
    //Mat c2lT = (Mat_<float>(2, 1) << 0.13693697, 0.05094143);
    Mat r_c = (Mat_<float>(3, 3) << 9.977e-1, 6.7744e-02, 0., -6.7744e-02, 9.977e-1, 0., 0., 0., 1.);
    Mat t = (Mat_<float>(3, 1) << 50., 0., 0.);
    Mat r_y = (Mat_<float>(3, 3) << 1., 0., 0., 0., 1.19444e+00, 0., 0., 0., 1.);
    /*int class_intensity[24] = {
        0, 120, 130, 130, 130, 130, 130, 130, 146, 146,
        146, 162, 162, 178, 178, 196, 196, 212, 212, 228,
        228, 0, 0, 0}; */
    int class_intensity[25] = {
        0, 115, 116, 117, 118, 119, 120, 121, 150, 151,
        152, 153, 154, 155, 156, 200, 201, 202, 203, 204,
        205, 206, 207, 208, 209};           // 物体类别和强度映射关系

    map<int, int> class_index = {{-1, 23}, {0, 0}, {1, 13}, {2, 14}, {3, 7}, {4, 8}, {5, 9}, {6, 1}, {7, 2}, {8, 19}, {9, 18}, {10, 6}, {11, 3}, {12, 15}, {13, 10}, {14, 4}, {15, 5}, {16, 11}, {17, 16}, {18, 12}, {19, 17}, {20, 20}, {21, 21}, {22, 22}};
};      // 类别映射，方便大小物体判断