492 lines
14 KiB
Plaintext
492 lines
14 KiB
Plaintext
#include <set>
|
|
#include "fusion.h"
|
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
|
char LCFusion::fusion(std::vector<Mat> &camInfo)
|
|
{
|
|
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();
|
|
std::cout << "Fusion start!" << std::endl;
|
|
// 大物体
|
|
vector<int> clusterlable = clusters_bboxs(boxes);
|
|
optimize_clusterlable(clusterlable, boxes);
|
|
big_fusion(clusterlable, 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)
|
|
{
|
|
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)
|
|
{
|
|
std::cout << "small_fusion" << std::endl;
|
|
for (int i = 0; i < bboxs.rows; i++)
|
|
{
|
|
int c = bboxs.at<float>(i, 5);
|
|
std::cout << "cls:" << c << std::endl;
|
|
if (class_index[c] >= 0 && class_index[c] <= 6)
|
|
{
|
|
|
|
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];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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 && cluster_num.size() > 3)
|
|
{
|
|
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++)
|
|
{
|
|
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()
|
|
{
|
|
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::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.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 = 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;
|
|
}
|
|
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.;
|
|
}
|
|
|
|
|