682 lines
23 KiB
C++
682 lines
23 KiB
C++
|
#include <set>
|
|||
|
|
#include "fusion.h"
|
|||
|
|
// 角度 -> 弧度转换
|
|||
|
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
|||
|
|
// fusion函数
|
|||
|
|
char LCFusion::fusion(std::vector<Mat> &camInfo)
|
|||
|
|
{
|
|||
|
|
projectedPoints.clear();
|
|||
|
|
if (camInfo.size() < 3) // 判断返回的双目测距信息是否为空，正常为3
|
|||
|
|
{
|
|||
|
|
std::cout << "There is no detection box!" << std::endl;
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
Mat boxes = camInfo[1], dwha = camInfo[2];
|
|||
|
|
projectedPoints.clear();
|
|||
|
|
|
|||
|
|
if (this->upScan.pointCloud.empty())
|
|||
|
|
{
|
|||
|
|
std::cout << "There is no lidar data!" << std::endl;
|
|||
|
|
return -2;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (boxes.empty())
|
|||
|
|
{
|
|||
|
|
std::cout << "There is no detection box!" << std::endl;
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
draw_point(camInfo); // 画出雷达映射到图像中的点
|
|||
|
|
std::cout << "Fusion start!" << std::endl;
|
|||
|
|
// 大物体
|
|||
|
|
vector<int> clusterlable = clusters_bboxs(boxes); // 得到每簇雷达映射的检测框标签（检测框索引值）
|
|||
|
|
optimize_clusterlable(clusterlable, boxes); // 优化检测框（针对镂空物体）
|
|||
|
|
big_fusion(clusterlable, boxes, dwha); // 大物体赋语义
|
|||
|
|
|
|||
|
|
//小物体
|
|||
|
|
small_box_without_dis(boxes, dwha); // 无距离信息的检测框映射
|
|||
|
|
small_fusion(boxes, dwha); // 小物体赋语义
|
|||
|
|
}
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::big_fusion(vector<int> &clusterlable, Mat &bboxs, Mat &dwha) // 大物体赋语义
|
|||
|
|
{
|
|||
|
|
std::cout << "big_fusion" << std::endl;
|
|||
|
|
for (int i = 0; i < clusterlable.size(); i++) // 遍历每簇雷达
|
|||
|
|
{
|
|||
|
|
int bbox_num = clusterlable[i];
|
|||
|
|
|
|||
|
|
if (bbox_num != -1) // -1代表无检测框与之对应
|
|||
|
|
{
|
|||
|
|
int bbox_cls = bboxs.at<float>(bbox_num, 5);
|
|||
|
|
if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) // 排除扩散物体（风扇底座、吧台椅）
|
|||
|
|
{
|
|||
|
|
//std::cout << "bboxcls:" << bbox_cls << std::endl;
|
|||
|
|
for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) // 遍历一簇雷达的所有点赋语义
|
|||
|
|
{
|
|||
|
|
int laser_index = this->upScan.clustedCloud[i][j].at<float>(0, 3);
|
|||
|
|
this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) // 扩散物体雷达点应该很少
|
|||
|
|
{
|
|||
|
|
//cout << "******start draw circle******" << endl;
|
|||
|
|
draw_circle(i, bbox_num, bboxs, dwha); // 扩散物体赋语义
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) // 小物体u赋语义
|
|||
|
|
{
|
|||
|
|
std::cout << "small_fusion" << std::endl;
|
|||
|
|
for (int i = 0; i < bboxs.rows; i++)
|
|||
|
|
{
|
|||
|
|
int c = bboxs.at<float>(i, 5);
|
|||
|
|
float x_mim = bboxs.at<float>(i, 0);
|
|||
|
|
float y_min = bboxs.at<float>(i, 1);
|
|||
|
|
float x_max = bboxs.at<float>(i, 2);
|
|||
|
|
float y_max = bboxs.at<float>(i, 3);
|
|||
|
|
std::cout << "cls:" << c << " xmin:" << x_mim << " ymin:" << y_min << " xmax:" << x_max << " ymax:" << y_max << std::endl;
|
|||
|
|
if (class_index[c] == 0 || class_index[c] == 2 || (class_index[c] == 4 || class_index[c] == 5)) // 判断小物体u
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
Mat temp(2, 1, CV_32F);
|
|||
|
|
double d = dwha.at<float>(i, 0);
|
|||
|
|
double a = dwha.at<float>(i, 3); // 取距离和角度
|
|||
|
|
if (d == -1)
|
|||
|
|
continue;
|
|||
|
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
|||
|
|
temp.at<float>(0, 0) = float((dwha.at<float>(i, 0) / 100.0) * cos(dwha.at<float>(i, 3)));
|
|||
|
|
temp.at<float>(1, 0) = float((dwha.at<float>(i, 0) / 100.0) * sin(dwha.at<float>(i, 3)));
|
|||
|
|
temp = c2lR * temp + c2lT; // 相机坐标系-> 雷达坐标系转换
|
|||
|
|
double y = temp.at<float>(0, 0);
|
|||
|
|
double x = temp.at<float>(1, 0);
|
|||
|
|
float dis = sqrt(x * x + y * y); // 转极坐标
|
|||
|
|
|
|||
|
|
float angle = 0.0; // 得到雷达点索引
|
|||
|
|
if (y >= 0)
|
|||
|
|
angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
else
|
|||
|
|
angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
|
|||
|
|
double data_angle = atan((dwha.at<float>(i, 1)) / (200 * dis));
|
|||
|
|
data_angle /= this->upScan.angleInc; // 由测距宽度信息得到需更改的雷达点数的一半
|
|||
|
|
angle = (int)angle - (int)data_angle; // 起始雷达索引
|
|||
|
|
int num_point = 2 * (int)data_angle + 1; // 更改的雷达点数
|
|||
|
|
|
|||
|
|
int len = this->upScan.distance.size();
|
|||
|
|
for (int i = 0; i < num_point; i++)
|
|||
|
|
{
|
|||
|
|
if ((angle + i) < 0 || (angle + i) > (len - 1)) // 防止越界
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
this->upScan.distance[angle + i] = dis;
|
|||
|
|
this->upScan.intensities[angle + i] = class_intensity[c + 1];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (class_index[c] == 1 || class_index[c] == 3 ||class_index[c] == 6)
|
|||
|
|
{
|
|||
|
|
int len = this->upScan.distance.size();
|
|||
|
|
Mat z = r->Z.clone();
|
|||
|
|
if (countNonZero(z) < 1)
|
|||
|
|
{
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
if (z.cols >= 2)
|
|||
|
|
{
|
|||
|
|
int begin_index = 0;
|
|||
|
|
for(int j = z.cols-1; j >=0; j-- )
|
|||
|
|
{
|
|||
|
|
int d = z.at<float>(0, j);
|
|||
|
|
if (d != 0)
|
|||
|
|
{
|
|||
|
|
begin_index = j;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
cout << "tizhongcheng " << z.cols << endl;
|
|||
|
|
float d_begin = z.at<float>(0, begin_index)/1000.;
|
|||
|
|
float a_begin = z.at<float>(1, begin_index);
|
|||
|
|
cout << "d_begin: " << d_begin << " a_begin: " << a_begin << endl;
|
|||
|
|
Mat temp(2, 1, CV_32F);
|
|||
|
|
temp.at<float>(0, 0) = float(d_begin * cos(a_begin));
|
|||
|
|
temp.at<float>(1, 0) = float(d_begin * sin(a_begin));
|
|||
|
|
temp = c2lR * temp + c2lT;
|
|||
|
|
double y = temp.at<float>(0, 0);
|
|||
|
|
double x = temp.at<float>(1, 0);
|
|||
|
|
float dis = sqrt(x * x + y * y);
|
|||
|
|
float angle_index = 0.0; // 得到雷达点索引
|
|||
|
|
if (y >= 0)
|
|||
|
|
angle_index = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
else
|
|||
|
|
angle_index = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
angle_index = (int)angle_index;
|
|||
|
|
if (angle_index >= 0 && angle_index <len) // 防止越界
|
|||
|
|
{
|
|||
|
|
this->upScan.distance[angle_index] = dis;
|
|||
|
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
|||
|
|
}
|
|||
|
|
cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
|||
|
|
angle_index += 1;
|
|||
|
|
|
|||
|
|
for (int i = begin_index-1; i >= 0; i--)
|
|||
|
|
{
|
|||
|
|
float temp_d = z.at<float>(0, i)/1000.;
|
|||
|
|
float temp_a = z.at<float>(1, i);
|
|||
|
|
temp.at<float>(0, 0) = float(temp_d * cos(temp_a));
|
|||
|
|
temp.at<float>(1, 0) = float(temp_d * sin(temp_a));
|
|||
|
|
temp = c2lR * temp + c2lT;
|
|||
|
|
y = temp.at<float>(0, 0);
|
|||
|
|
x = temp.at<float>(1, 0);
|
|||
|
|
dis = sqrt(x * x + y * y);
|
|||
|
|
float temp_angle = 0.0; // 得到雷达点索引
|
|||
|
|
if (y >= 0)
|
|||
|
|
temp_angle = acos(x / dis);
|
|||
|
|
else
|
|||
|
|
temp_angle =2 * M_PI - acos(x / dis);
|
|||
|
|
float angle_now = this->upScan.angleMin + this->upScan.angleInc*angle_index;
|
|||
|
|
if (temp_angle >= angle_now)
|
|||
|
|
{
|
|||
|
|
if (angle_index >= 0 && angle_index < len) // 防止越界
|
|||
|
|
{
|
|||
|
|
this->upScan.distance[angle_index] = dis;
|
|||
|
|
// cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
|||
|
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
|||
|
|
}
|
|||
|
|
angle_index += 1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
r->Z = Mat::zeros(2, 1, CV_32FC1);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::optimize_clusterlable(vector<int> &clusterlable, Mat &bboxs) // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
|
|||
|
|
{
|
|||
|
|
for (int i = 0; i < bboxs.rows; i++) // 寻找每个框对应的所有雷达簇
|
|||
|
|
{
|
|||
|
|
vector<int> temp_cluster_num;
|
|||
|
|
Mat temp_bbox = bboxs.row(i).clone();
|
|||
|
|
for (int j = 0; j < clusterlable.size(); j++)
|
|||
|
|
{
|
|||
|
|
if (clusterlable[j] == i)
|
|||
|
|
{
|
|||
|
|
temp_cluster_num.push_back(j);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if (temp_cluster_num.size() == 0)
|
|||
|
|
continue;
|
|||
|
|
choose_forceground(temp_cluster_num, temp_bbox, clusterlable); // 筛选镂空物体的前景
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::choose_forceground(vector<int> &cluster_num, Mat &bbox, vector<int> &clusterlable) // 筛选镂空物体的前景
|
|||
|
|
{
|
|||
|
|
int box_class = bbox.at<float>(0, 5);
|
|||
|
|
if ((class_index[box_class] >= 15 && class_index[box_class] <= 18 || class_index[box_class] == 12)&& cluster_num.size() > 1)
|
|||
|
|
{
|
|||
|
|
cout << "class " << box_class << " have " << cluster_num.size() << " clusters" << endl;
|
|||
|
|
int total_point = 0;
|
|||
|
|
for (int i = 0; i < cluster_num.size(); i++) // 计算框内雷达点总数
|
|||
|
|
{
|
|||
|
|
total_point += (this->upScan.clustedCloud[cluster_num[i]].size());
|
|||
|
|
}
|
|||
|
|
for (int j = 0; j < cluster_num.size(); j++) // 计算每个雷达簇占比，大于0.2认为是背景
|
|||
|
|
{
|
|||
|
|
float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size();
|
|||
|
|
if (num_cluster / total_point > 0.2)
|
|||
|
|
{
|
|||
|
|
clusterlable[cluster_num[j]] = -1;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::draw_point(std::vector<Mat> &camInfo) // 画出映射到图像的雷达点
|
|||
|
|
{
|
|||
|
|
projectedPoints.clear();
|
|||
|
|
for (int i = 0; i < this->upScan.pointCloud.rows; i++)
|
|||
|
|
{
|
|||
|
|
Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone();
|
|||
|
|
Mat uv = pointcloud_pixel(temp_pointcloud);
|
|||
|
|
int x = uv.at<float>(0, 0);
|
|||
|
|
int y = uv.at<float>(1, 0);
|
|||
|
|
if (x > 0 && x < 640 && y > 0 && y < 480)
|
|||
|
|
{
|
|||
|
|
Point2d pt_uv(x, y);
|
|||
|
|
projectedPoints.push_back(pt_uv);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc)
|
|||
|
|
// {
|
|||
|
|
// upScan.distance.clear();
|
|||
|
|
// upScan.intensities.clear();
|
|||
|
|
// float pointAngle;
|
|||
|
|
// upScan.angleInc = angleInc;
|
|||
|
|
|
|||
|
|
// for (int i = 0; i < laserData.len; i++)
|
|||
|
|
// {
|
|||
|
|
// pointAngle = angle_min + i * upScan.angleInc;
|
|||
|
|
// if (pointAngle > cameraAnglemin)
|
|||
|
|
// {
|
|||
|
|
// if (pointAngle > cameraAnglemax)
|
|||
|
|
// {
|
|||
|
|
// upScan.angleMax = pointAngle - upScan.angleInc;
|
|||
|
|
// break;
|
|||
|
|
// }
|
|||
|
|
// upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
|||
|
|
// upScan.intensities.push_back(0);
|
|||
|
|
// if (i == (laserData.len - 1))
|
|||
|
|
// upScan.angleMax = pointAngle;
|
|||
|
|
// // upScan.intensities.push_back(laserData.intensities[i]);
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
// upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc;
|
|||
|
|
// this->scan_to_pointcloud();
|
|||
|
|
// this->cluster();
|
|||
|
|
// }
|
|||
|
|
void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) // 雷达点和相机视野同步
|
|||
|
|
{
|
|||
|
|
upScan.distance.clear();
|
|||
|
|
upScan.intensities.clear();
|
|||
|
|
upScan.angleMin = angle_min;
|
|||
|
|
upScan.angleInc = angleInc;
|
|||
|
|
upScan.angleMax = ANGLE_TO_RADIAN(laserData.angle_max);
|
|||
|
|
float pointAngle;
|
|||
|
|
for (int i = 0; i < laserData.len; i++)
|
|||
|
|
{
|
|||
|
|
upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
|||
|
|
pointAngle = angle_min + i * upScan.angleInc;
|
|||
|
|
if (pointAngle > cameraAnglemin && pointAngle < cameraAnglemax)
|
|||
|
|
upScan.intensities.push_back(1); // 视野范围内强度赋1
|
|||
|
|
else
|
|||
|
|
upScan.intensities.push_back(0); // 视野范围外强度赋0
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
this->scan_to_pointcloud(); // 雷达极坐标到二维坐标转换
|
|||
|
|
this->cluster(); // 雷达聚类
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::scan_to_pointcloud() // 雷达极坐标到二维坐标转换
|
|||
|
|
{
|
|||
|
|
u_int32_t len = upScan.distance.size();
|
|||
|
|
Mat pointcloud(len, 4, CV_32F);
|
|||
|
|
|
|||
|
|
for (int i = 0; i < len; i++)
|
|||
|
|
{
|
|||
|
|
float dis = this->upScan.distance[i];
|
|||
|
|
float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis;
|
|||
|
|
float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis;
|
|||
|
|
pointcloud.at<float>(i, 0) = x_temp;
|
|||
|
|
pointcloud.at<float>(i, 1) = y_temp;
|
|||
|
|
pointcloud.at<float>(i, 2) = 0;
|
|||
|
|
pointcloud.at<float>(i, 3) = i;
|
|||
|
|
}
|
|||
|
|
this->upScan.pointCloud = pointcloud.clone();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) // 无用
|
|||
|
|
{
|
|||
|
|
int x = uv.at<float>(0, 0);
|
|||
|
|
int y = uv.at<float>(1, 0);
|
|||
|
|
// 实验测试，过滤类别
|
|||
|
|
std::set<uchar> filterCls = {0, 7, 11, 14, 15, 21};
|
|||
|
|
for (uchar i = 0; i < boxes.rows; i++)
|
|||
|
|
{
|
|||
|
|
int clsIdx = boxes.at<float>(i, 5);
|
|||
|
|
if (filterCls.find(clsIdx) != filterCls.end())
|
|||
|
|
continue;
|
|||
|
|
|
|||
|
|
float xmin = boxes.at<float>(i, 0);
|
|||
|
|
float xmax = boxes.at<float>(i, 2);
|
|||
|
|
float ymin = boxes.at<float>(i, 1);
|
|||
|
|
float ymax = boxes.at<float>(i, 3);
|
|||
|
|
|
|||
|
|
// 实验测试，过滤过大的误检框
|
|||
|
|
float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
|||
|
|
if (ratio > 0.7)
|
|||
|
|
{
|
|||
|
|
boxflag[i] += 1;
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况
|
|||
|
|
if (x >= xmin && x <= xmax && y >= ymin && y <= ymax)
|
|||
|
|
{
|
|||
|
|
boxflag[i] += 1;
|
|||
|
|
return class_intensity[clsIdx + 1];
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::vector<int> LCFusion::clusters_bboxs(Mat &bboxs) // 所有的聚类的雷达簇和所有的检测框的匹配
|
|||
|
|
{
|
|||
|
|
std::vector<int> clusterlable;
|
|||
|
|
for (int i = 0; i < this->upScan.clustedCloud.size(); i++) // 遍历每簇雷达
|
|||
|
|
{
|
|||
|
|
std::vector<Mat> temp_cluster = this->upScan.clustedCloud[i];
|
|||
|
|
int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); // 为一簇雷达寻找匹配的检测框
|
|||
|
|
clusterlable.push_back(temp_clusters_bbox);
|
|||
|
|
}
|
|||
|
|
return clusterlable;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int LCFusion::cluster_bboxs(vector<Mat> &one_cluster, Mat &bboxs) // 为一簇雷达寻找匹配的检测框
|
|||
|
|
{
|
|||
|
|
vector<int> temp_result;
|
|||
|
|
Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]);
|
|||
|
|
Mat end = pointcloud_pixel(one_cluster[0]); // 一簇雷达起始点和终止点在像素坐标系的坐标
|
|||
|
|
for (int i = 0; i < bboxs.rows; i++)
|
|||
|
|
{
|
|||
|
|
Mat bbox = bboxs.row(i).clone();
|
|||
|
|
int cls = bbox.at<float>(0, 5);
|
|||
|
|
float ratio = ratio_in_1box(begin, end, bbox); // 在框中占比
|
|||
|
|
if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) // 适用于大物体
|
|||
|
|
temp_result.push_back(i);
|
|||
|
|
}
|
|||
|
|
if (temp_result.size() < 1) // 若一簇雷达落入多个框里，选择宽度较小的框
|
|||
|
|
return -1;
|
|||
|
|
else if (temp_result.size() < 2)
|
|||
|
|
return temp_result[0];
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
int min = temp_result[0];
|
|||
|
|
int width_min = bboxs.at<float>(min, 2) - bboxs.at<float>(min, 0);
|
|||
|
|
for (int i = 0; i < temp_result.size(); i++)
|
|||
|
|
{
|
|||
|
|
int width_new = bboxs.at<float>(temp_result[i], 2) - bboxs.at<float>(temp_result[i], 0);
|
|||
|
|
if (width_new < width_min)
|
|||
|
|
{
|
|||
|
|
min = temp_result[i];
|
|||
|
|
width_min = width_new;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
return min;
|
|||
|
|
}
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
Mat LCFusion::pointcloud_pixel(Mat &pointcloud) // 二维雷达点到像素坐标系转换
|
|||
|
|
{
|
|||
|
|
Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3));
|
|||
|
|
Mat camPoint = rotate * cutPointCloud.t() + translation;
|
|||
|
|
// float ppp = camPoint.at<float>(2, 0);
|
|||
|
|
if (camPoint.at<float>(2, 0) <= 0)
|
|||
|
|
{
|
|||
|
|
uv = (Mat_<float>(3, 1) << -1, -1, -1);
|
|||
|
|
return uv;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float scaleX = camPoint.at<float>(0, 0) / camPoint.at<float>(2, 0);
|
|||
|
|
float scaleY = camPoint.at<float>(1, 0) / camPoint.at<float>(2, 0);
|
|||
|
|
float scaleD = scaleX * scaleX + scaleY * scaleY;
|
|||
|
|
float tempD = 1 + distCoeff.at<float>(0, 0) * scaleD + distCoeff.at<float>(0, 1) * scaleD * scaleD + distCoeff.at<float>(0, 4) * scaleD * scaleD * scaleD;
|
|||
|
|
|
|||
|
|
camPoint.at<float>(0, 0) = scaleX * tempD + 2 * distCoeff.at<float>(0, 2) * scaleX * scaleY +
|
|||
|
|
distCoeff.at<float>(0, 3) * (scaleD + 2 * scaleX * scaleX);
|
|||
|
|
|
|||
|
|
camPoint.at<float>(1, 0) = scaleY * tempD + distCoeff.at<float>(0, 2) * (scaleD + 2 * scaleY * scaleY) +
|
|||
|
|
2 * distCoeff.at<float>(0, 3) * scaleX * scaleY;
|
|||
|
|
|
|||
|
|
camPoint.at<float>(2, 0) = 1.0;
|
|||
|
|
uv = cameraMat * camPoint; // uv为像素坐标
|
|||
|
|
uv = p2r * uv + t; // 矫正
|
|||
|
|
return uv;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) // 计算一簇雷达落入检测框的比例
|
|||
|
|
{
|
|||
|
|
int x_begin = begin.at<float>(0, 0), x_end = end.at<float>(0, 0), y_begin = begin.at<float>(1, 0), y_end = end.at<float>(1, 0);
|
|||
|
|
int xmin = box.at<float>(0, 0), ymin = box.at<float>(0, 1), xmax = box.at<float>(0, 2), ymax = box.at<float>(0, 3);
|
|||
|
|
if (y_begin < ymin || y_begin > ymax)
|
|||
|
|
return 0;
|
|||
|
|
if (x_begin < xmin)
|
|||
|
|
{
|
|||
|
|
if (x_end < xmin)
|
|||
|
|
return 0;
|
|||
|
|
else if (x_end < xmax)
|
|||
|
|
return (float)(x_end - xmin) / (x_end - x_begin);
|
|||
|
|
else
|
|||
|
|
return (float)(xmax - xmin) / (x_end - x_begin);
|
|||
|
|
}
|
|||
|
|
else if (x_begin < xmax)
|
|||
|
|
{
|
|||
|
|
if (x_end < xmax)
|
|||
|
|
return 1;
|
|||
|
|
else
|
|||
|
|
return (float)(xmax - x_begin) / (x_end - x_begin);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LCFusion::cluster() // 聚类
|
|||
|
|
{
|
|||
|
|
this->upScan.clustedCloud.clear();
|
|||
|
|
int i = 0;
|
|||
|
|
|
|||
|
|
for (; i < this->upScan.distance.size();)
|
|||
|
|
{
|
|||
|
|
std::vector<Mat> temp;
|
|||
|
|
int j = i;
|
|||
|
|
temp.push_back(this->upScan.pointCloud.row(i));
|
|||
|
|
for (; j < this->upScan.distance.size() - 1; j++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
// 距离的6%作为分类的阈值
|
|||
|
|
if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 )
|
|||
|
|
{
|
|||
|
|
temp.push_back(this->upScan.pointCloud.row(j + 1));
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
i = j + 1;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
this->upScan.clustedCloud.push_back(temp);
|
|||
|
|
if (j == this->upScan.distance.size() - 1)
|
|||
|
|
break;
|
|||
|
|
if (i == upScan.distance.size() - 1)
|
|||
|
|
{
|
|||
|
|
temp.push_back(upScan.pointCloud.row(i));
|
|||
|
|
this->upScan.clustedCloud.push_back(temp);
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
this->clusterIdx.clear();
|
|||
|
|
int ii = 0, totoalNum = 0;
|
|||
|
|
for (auto c : this->upScan.clustedCloud)
|
|||
|
|
{
|
|||
|
|
for (int jj = 0; jj < c.size(); jj++)
|
|||
|
|
{
|
|||
|
|
this->clusterIdx.push_back(ii);
|
|||
|
|
}
|
|||
|
|
totoalNum += c.size();
|
|||
|
|
ii++;
|
|||
|
|
}
|
|||
|
|
//std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl;
|
|||
|
|
}
|
|||
|
|
void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) // 扩散物体生成圆弧状伪激光雷达
|
|||
|
|
{
|
|||
|
|
vector<float> center_point = find_circle_center(cluster_num);
|
|||
|
|
float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at<float>(bbox_num, 1)/200.f,
|
|||
|
|
d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc);
|
|||
|
|
int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at<float>(bbox_num, 5);
|
|||
|
|
float laser_index = center_point[1] - half_data_num;
|
|||
|
|
//cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl;
|
|||
|
|
for (int i = 0; i < num_laser_circle; i++)
|
|||
|
|
{
|
|||
|
|
float theta = upScan.angleMin + laser_index * upScan.angleInc;
|
|||
|
|
if (laser_index < 0 || laser_index > upScan.distance.size())
|
|||
|
|
{
|
|||
|
|
laser_index++;
|
|||
|
|
continue;
|
|||
|
|
}
|
|||
|
|
float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2);
|
|||
|
|
if (data >= 0)
|
|||
|
|
{
|
|||
|
|
//cout << "draw_one_point" << endl;
|
|||
|
|
upScan.distance[laser_index] = circle(d, alfa ,theta, data);
|
|||
|
|
upScan.intensities[laser_index] = class_intensity[cls + 1];
|
|||
|
|
}
|
|||
|
|
laser_index++;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
vector<float> LCFusion::find_circle_center(int cluster_num) // 寻找扩散物体圆弧的圆心
|
|||
|
|
{
|
|||
|
|
vector<float> center_point(2);
|
|||
|
|
int index = 0;
|
|||
|
|
float total_dis = 0;
|
|||
|
|
int num = upScan.clustedCloud[cluster_num].size();
|
|||
|
|
int no_zero_dis = 0; // 非零距离和
|
|||
|
|
for (int i = 0 ; i < num; i++)
|
|||
|
|
{
|
|||
|
|
index = upScan.clustedCloud[cluster_num][i].at<float>(0, 3);
|
|||
|
|
if (upScan.distance[index] != 0)
|
|||
|
|
no_zero_dis++;
|
|||
|
|
total_dis += upScan.distance[index];
|
|||
|
|
}
|
|||
|
|
center_point[0] = total_dis/ no_zero_dis;
|
|||
|
|
index -= (num/2);
|
|||
|
|
float center_laser_index = index;
|
|||
|
|
center_point[1] = center_laser_index;
|
|||
|
|
return center_point;
|
|||
|
|
}
|
|||
|
|
float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度
|
|||
|
|
{
|
|||
|
|
float ro = d * cos(theta - alfa) - sqrt(data);
|
|||
|
|
return ro > 0 ? ro:0;
|
|||
|
|
}
|
|||
|
|
vector<float> LCFusion::width_ladar(const int* coordinates)
|
|||
|
|
{
|
|||
|
|
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
|||
|
|
|
|||
|
|
std::vector<float> mid = r-> pic2cam(640 / 2, 480); //<2F>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<float> loc_tar_start = r-> pic2cam(x1, 480); // <20>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
|
|||
|
|
std::vector<float> loc_tar_end = r-> pic2cam(x2, 480); //<2F>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ֹλ<D6B9><CEBB>
|
|||
|
|
//ת<><D7AA><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>ֵ
|
|||
|
|
float alfa_start = atan((loc_tar_start[0] - mid[0]) / r-> q.at<double>(2, 3));
|
|||
|
|
float alfa_end = atan((loc_tar_end[0] - mid[0]) / r-> q.at<double>(2, 3));
|
|||
|
|
std::vector<float> ladar_range;
|
|||
|
|
ladar_range.push_back(alfa_start);
|
|||
|
|
ladar_range.push_back(alfa_end);
|
|||
|
|
return ladar_range;
|
|||
|
|
}
|
|||
|
|
void LCFusion::small_box_without_dis(Mat & bboxs, Mat & dwha) // 无距离检测框到像素坐标系映射，过程和small_fusion类似
|
|||
|
|
{
|
|||
|
|
std::cout << "small_box_without_dis" << std::endl;
|
|||
|
|
double set_dis = 2.0; // 设置距离为2米
|
|||
|
|
for (int i = 0; i < bboxs.rows; i++)
|
|||
|
|
{
|
|||
|
|
int c = bboxs.at<float>(i, 5);
|
|||
|
|
int x_mim = bboxs.at<float>(i, 0);
|
|||
|
|
int y_min = bboxs.at<float>(i, 1);
|
|||
|
|
int x_max = bboxs.at<float>(i, 2);
|
|||
|
|
int y_max = bboxs.at<float>(i, 3);
|
|||
|
|
double d = dwha.at<float>(i, 0);
|
|||
|
|
int coordinates[4] = {x_mim, y_min, x_max, y_max};
|
|||
|
|
if ((class_index[c] >= 0 && class_index[c] <= 1) || (class_index[c] >= 3 && class_index[c] <= 6))
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
Mat temp(2, 1, CV_32F);
|
|||
|
|
|
|||
|
|
|
|||
|
|
if (d == -1)
|
|||
|
|
{
|
|||
|
|
vector<float> box_angle = width_ladar(coordinates);
|
|||
|
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
|||
|
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[0]));
|
|||
|
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[0]));
|
|||
|
|
temp = c2lR * temp + c2lT;
|
|||
|
|
double y = temp.at<float>(0, 0);
|
|||
|
|
double x = temp.at<float>(1, 0);
|
|||
|
|
float dis = sqrt(x * x + y * y);
|
|||
|
|
float angle_end = 0.0;
|
|||
|
|
if (y >= 0)
|
|||
|
|
angle_end = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
else
|
|||
|
|
angle_end = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
|
|||
|
|
angle_end = int(angle_end);
|
|||
|
|
// std:: cout << "angle end: " << angle_end << std::endl;
|
|||
|
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[1]));
|
|||
|
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[1]));
|
|||
|
|
temp = c2lR * temp + c2lT;
|
|||
|
|
y = temp.at<float>(0, 0);
|
|||
|
|
x = temp.at<float>(1, 0);
|
|||
|
|
dis = sqrt(x * x + y * y);
|
|||
|
|
float angle_start = 0.0;
|
|||
|
|
if (y >= 0)
|
|||
|
|
angle_start = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
else
|
|||
|
|
angle_start = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
|||
|
|
|
|||
|
|
angle_start = int(angle_start);
|
|||
|
|
// std:: cout << "angle start: " << angle_start << std::endl;
|
|||
|
|
if (angle_end-angle_start <= 0)
|
|||
|
|
continue;
|
|||
|
|
int num_point = angle_end-angle_start+1;
|
|||
|
|
for (int i = 0; i < num_point; i++)
|
|||
|
|
{
|
|||
|
|
if ((angle_start + i) < 0 || (angle_start + i) > (upScan.distance.size() - 1))
|
|||
|
|
continue;
|
|||
|
|
this->upScan.intensities[angle_start + i] = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) // 设置雷达发布数据（原始雷达）
|
|||
|
|
{
|
|||
|
|
float angle_min = ANGLE_TO_RADIAN(laserData.angle_min);
|
|||
|
|
float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
|
|||
|
|
uint32_t len = laserData.len;
|
|||
|
|
float angle_inc = (angle_max - angle_min) / (len - 1.);
|
|||
|
|
scan.header.frame_id = "down_laser_link";
|
|||
|
|
scan.range_min = 0.15;
|
|||
|
|
scan.range_max = 8.0;
|
|||
|
|
scan.angle_min = angle_min;
|
|||
|
|
scan.angle_max = angle_max;
|
|||
|
|
scan.angle_increment = angle_inc;
|
|||
|
|
scan.ranges.resize(len);
|
|||
|
|
scan.intensities.resize(len);
|
|||
|
|
scan.time_increment = 1. / 2400.;
|
|||
|
|
for (int i = 0; i < len; i++)
|
|||
|
|
{
|
|||
|
|
scan.ranges[i] = laserData.distance[i] / 1000.f;
|
|||
|
|
scan.intensities[i] = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) // 设置雷达发布数据（语义雷达）
|
|||
|
|
{
|
|||
|
|
scan.header.frame_id = "up_laser_link";
|
|||
|
|
scan.range_min = 0.15;
|
|||
|
|
scan.range_max = 8.0;
|
|||
|
|
scan.angle_increment = upScan.angleInc;
|
|||
|
|
scan.ranges.resize(upScan.distance.size());
|
|||
|
|
scan.intensities.resize(upScan.distance.size());
|
|||
|
|
scan.angle_min = upScan.angleMin;
|
|||
|
|
scan.angle_max = upScan.angleMax;
|
|||
|
|
for (int i = 0; i < upScan.distance.size(); i++)
|
|||
|
|
{
|
|||
|
|
scan.ranges[i] = upScan.distance[i];
|
|||
|
|
scan.intensities[i] = upScan.intensities[i];
|
|||
|
|
}
|
|||
|
|
scan.time_increment = 1. / 2400.;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|