#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); //�ҵ���������
    std::vector<float> loc_tar_start = r-> pic2cam(x1, 480); // �ҵ�Ŀ�����ʼλ��
    std::vector<float> loc_tar_end = r-> pic2cam(x2, 480); //�ҵ�Ŀ�����ֹλ��
    //ת���ɻ���ֵ
    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.;
}