99 lines
5.7 KiB
C++
99 lines
5.7 KiB
C++
#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}};
|
|
}; // 类别映射，方便大小物体判断
|