diff --git a/fusion.cpp b/fusion.cpp
new file mode 100644
index 0000000..3894356
--- /dev/null
+++ b/fusion.cpp
@@ -0,0 +1,685 @@
+#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() < 4)	// 扩散物体雷达点应该很少
+				{
+					//cout << "******start draw circle******" << endl;
+					double d = dwha.at<float>(bbox_num, 0);
+					if (d > 0)
+					{
+						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 || class_index[box_class] == 10) && 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) || num_cluster > 4)
+			{
+				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.;
+}
+
+
diff --git a/fusion.h b/fusion.h
new file mode 100644
index 0000000..f3af461
--- /dev/null
+++ b/fusion.h
@@ -0,0 +1,100 @@
+#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}};
+};      // 类别映射，方便大小物体判断
diff --git a/lidar.cpp b/lidar.cpp
new file mode 100644
index 0000000..66ecfa3
--- /dev/null
+++ b/lidar.cpp
@@ -0,0 +1,583 @@
+/**
+ * @file         lipkg.cpp
+ * @author       LD Robot
+ * @version      V01
+ * @brief
+ * @note
+ * @attention    COPYRIGHT LDROBOT
+ **/
+
+#include "lidar.h"
+#include <math.h>
+#include <algorithm>
+
+// #ifdef USE_SLBI
+// #include "slbi.h"
+// #endif
+
+// #ifdef USE_SLBF
+// #include "slbf.h"
+// #endif
+//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000)
+#define MAX_ACK_BUF_LEN 2304000
+
+#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
+
+static const uint8_t CrcTable[256] =
+	{
+		0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
+		0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
+		0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
+		0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
+		0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
+		0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
+		0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
+		0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
+		0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
+		0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
+		0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
+		0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
+		0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
+		0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
+		0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
+		0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
+		0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
+		0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
+		0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
+		0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
+		0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
+		0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8};
+
+CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr),
+													mRxCount(0),
+													mReadCallback(nullptr),
+													version(ver)
+{
+	mComHandle = -1;
+}
+CmdInterfaceLinux::~CmdInterfaceLinux()
+{
+	Close();
+}
+bool CmdInterfaceLinux::Open(std::string &port_name)
+{
+	int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	mComHandle = open(port_name.c_str(), flags);
+	if (-1 == mComHandle)
+	{
+		std::cout << "CmdInterfaceLinux::Open " << port_name <<  " error !" << std::endl;
+		return false;
+	}
+
+	// get port options
+	struct termios options;
+	if (-1 == tcgetattr(mComHandle, &options))
+	{
+		Close();
+		std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl;
+		return false;
+	}
+
+	options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS);
+	options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD);
+	options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
+									ISIG | IEXTEN); //|ECHOPRT
+	options.c_oflag &= (tcflag_t) ~(OPOST);
+	options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK);
+
+	options.c_cc[VMIN] = 0;
+	options.c_cc[VTIME] = 0;
+
+	switch (version)
+	{
+	case 0:
+		cfsetispeed(&options, B115200);
+		break;
+	case 3:
+		cfsetispeed(&options, B115200);
+		break;
+	case 6:
+		cfsetispeed(&options, B230400);
+		break;
+	case 8:
+		cfsetispeed(&options, B115200);
+		break;
+	case 14:
+		cfsetispeed(&options, B115200);
+		break;
+	default:
+		cfsetispeed(&options, B115200);
+	}
+
+	if (tcsetattr(mComHandle, TCSANOW, &options) < 0)
+	{
+		std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl;
+		Close();
+		return false;
+	}
+
+	tcflush(mComHandle, TCIFLUSH);
+
+	mRxThreadExitFlag = false;
+	mRxThread = new std::thread(mRxThreadProc, this);
+	mIsCmdOpened = true;
+
+	return true;
+}
+bool CmdInterfaceLinux::Close()
+{
+	if (mIsCmdOpened == false)
+	{
+		return true;
+	}
+
+	mRxThreadExitFlag = true;
+
+	if (mComHandle != -1)
+	{
+		close(mComHandle);
+		mComHandle = -1;
+	}
+
+	if ((mRxThread != nullptr) && mRxThread->joinable())
+	{
+		mRxThread->join();
+		delete mRxThread;
+		mRxThread = NULL;
+	}
+
+	mIsCmdOpened = false;
+
+	return true;
+}
+bool CmdInterfaceLinux::GetCmdDevices(std::vector<std::pair<std::string, std::string>> &device_list)
+{
+	struct udev *udev;
+	struct udev_enumerate *enumerate;
+	struct udev_list_entry *devices, *dev_list_entry;
+	struct udev_device *dev;
+
+	udev = udev_new();
+	if (!udev)
+	{
+		return false;
+	}
+	enumerate = udev_enumerate_new(udev);
+	udev_enumerate_add_match_subsystem(enumerate, "tty");
+	udev_enumerate_scan_devices(enumerate);
+	devices = udev_enumerate_get_list_entry(enumerate);
+	udev_list_entry_foreach(dev_list_entry, devices)
+	{
+		const char *path;
+
+		path = udev_list_entry_get_name(dev_list_entry);
+		dev = udev_device_new_from_syspath(udev, path);
+		std::string dev_path = std::string(udev_device_get_devnode(dev));
+		dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device");
+		if (dev)
+		{
+			std::pair<std::string, std::string> p;
+			p.first = dev_path;
+			p.second = udev_device_get_sysattr_value(dev, "product");
+			device_list.push_back(p);
+			udev_device_unref(dev);
+		}
+		else
+		{
+			continue;
+		}
+	}
+	udev_enumerate_unref(enumerate);
+	udev_unref(udev);
+	return true;
+}
+bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len)
+{
+	static timespec timeout = {0, (long)(100 * 1e6)};
+	int32_t len = -1;
+
+	if (IsOpened())
+	{
+		fd_set read_fds;
+		FD_ZERO(&read_fds);
+		FD_SET(mComHandle, &read_fds);
+		int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL);
+		if (r < 0)
+		{
+			// Select was interrupted
+			if (errno == EINTR)
+			{
+				return false;
+			}
+		}
+		else if (r == 0) // timeout
+		{
+			return false;
+		}
+
+		if (FD_ISSET(mComHandle, &read_fds))
+		{
+			len = (int32_t)read(mComHandle, rx_buf, rx_buf_len);
+			if ((len != -1) && rx_len)
+			{
+				*rx_len = len;
+			}
+		}
+	}
+	return len == -1 ? false : true;
+}
+bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len)
+{
+	int32_t len = -1;
+
+	if (IsOpened())
+	{
+		len = (int32_t)write(mComHandle, tx_buf, tx_buf_len);
+		if ((len != -1) && tx_len)
+		{
+			*tx_len = len;
+		}
+	}
+	return len == -1 ? false : true;
+}
+void CmdInterfaceLinux::mRxThreadProc(void *param)
+{
+	CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param;
+	char *rx_buf = new char[MAX_ACK_BUF_LEN + 1];
+	uchar failedNum = 0;
+	while (!cmd_if->mRxThreadExitFlag.load())
+	{
+		uint32_t readed = 0;
+		bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed);
+		if (res && readed)
+		{
+			cmd_if->mRxCount += readed;
+			if (cmd_if->mReadCallback != nullptr)
+			{
+				cmd_if->mReadCallback(rx_buf, readed);
+			}
+			failedNum = 0;
+		}
+		else 
+		{
+			if(++failedNum == 255)
+				break;
+		}
+	}
+	cmd_if->mRxThreadExitFlag = true;
+	delete[] rx_buf;
+}
+
+LiPkg::LiPkg() : mTimestamp(0),
+				 mSpeed(0),
+				 mErrorTimes(0),
+				 mIsFrameReady(false),
+				 mIsPkgReady(false)
+{
+}
+double LiPkg::GetSpeed(void)
+{
+	return mSpeed / 360.0;
+}
+bool LiPkg::Parse(const uint8_t *data, long len)
+{
+	for (int i = 0; i < len; i++)
+	{
+		mDataTmp.push_back(*(data + i));
+	}
+
+	if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
+		return false;
+
+	if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100)
+	{
+		mErrorTimes++;
+		mDataTmp.clear();
+		return false;
+	}
+
+	uint16_t start = 0;
+
+	while (start < mDataTmp.size() - 2)
+	{
+		start = 0;
+		while (start < mDataTmp.size() - 2)
+		{
+			if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN))
+			{
+				break;
+			}
+
+			if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5))))
+			{
+				break;
+			}
+			start++;
+		}
+
+		if (start != 0)
+		{
+			mErrorTimes++;
+			for (int i = 0; i < start; i++)
+			{
+				mDataTmp.erase(mDataTmp.begin());
+			}
+		}
+
+		if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
+			return false;
+
+		LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data();
+		uint8_t crc = 0;
+
+		for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++)
+		{
+			crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff];
+		}
+
+		if (crc == pkg->crc8)
+		{
+			double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360;
+			if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2)
+			{
+				mErrorTimes++;
+			}
+			else
+			{
+				mSpeed = pkg->speed;
+				mTimestamp = pkg->timestamp;
+				uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000;
+				float step = diff / (POINT_PER_PACK - 1) / 100.0;
+				float start = (double)pkg->start_angle / 100.0;
+				float end = (double)(pkg->end_angle % 36000) / 100.0;
+				PointData data;
+				for (int i = 0; i < POINT_PER_PACK; i++)
+				{
+					data.distance = pkg->point[i].distance;
+					data.angle = start + i * step;
+					if (data.angle >= 360.0)
+					{
+						data.angle -= 360.0;
+					}
+					data.confidence = pkg->point[i].confidence;
+					mOnePkg[i] = data;
+					mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence));
+				}
+				// prevent angle invert
+				mOnePkg.back().angle = end;
+
+				mIsPkgReady = true;
+			}
+
+			for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++)
+			{
+				mDataTmp.erase(mDataTmp.begin());
+			}
+
+			if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
+			{
+				break;
+			}
+		}
+		else
+		{
+			mErrorTimes++;
+			/*only remove header,not all frame,because lidar data may contain head*/
+			for (int i = 0; i < 2; i++)
+			{
+				mDataTmp.erase(mDataTmp.begin());
+			}
+		}
+	}
+
+	return true;
+}
+bool LiPkg::AssemblePacket()
+{
+	float last_angle = 0;
+	std::vector<PointData> tmp;
+	int count = 0;
+	for (auto n : mFrameTemp)
+	{
+		/*wait for enough data, need enough data to show a circle*/
+		if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */
+		{
+			mFrameData.len = tmp.size();
+			Transform(tmp); /*transform raw data to stantard data */
+
+			if (tmp.size() == 0)
+			{
+				mFrameTemp.clear();
+				mIsFrameReady = false;
+				return false;
+			}
+
+#ifdef USE_SLBI
+			Slbi sb(mSpeed);
+			sb.FindBarcode(tmp);
+#endif
+
+#ifdef USE_SLBF
+			Slbf sb(mSpeed);
+			tmp = sb.NearFilter(tmp);
+#endif
+
+			std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b)
+					  { return a.angle < b.angle; });
+			mFrameData.angle_min = tmp[0].angle;
+			mFrameData.angle_max = tmp.back().angle;
+			mFrameData.distance.clear();
+			mFrameData.intensities.clear();
+			mFrameData.distance.resize(mFrameData.len);
+			mFrameData.intensities.resize(mFrameData.len);
+			float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len;
+			float angle_acc = mFrameData.angle_min;
+			int tmp_count = 0;
+			/* interpolation method */
+			for (uint32_t i = 0; i < mFrameData.len; i++)
+			{
+				if (angle_acc >= tmp[tmp_count].angle)
+				{
+					mFrameData.distance[i] = tmp[tmp_count].distance;
+					mFrameData.intensities[i] = tmp[tmp_count].confidence;
+					tmp_count++;
+					if (tmp_count == tmp.size())
+					{
+						break;
+					}
+				}
+				else
+				{
+					mFrameData.distance[i] = 0;
+					mFrameData.intensities[i] = 0;
+				}
+				angle_acc += step;
+			}
+			std::vector<PointData> tmp2;
+			for (uint32_t i = count; i < mFrameTemp.size(); i++)
+			{
+				tmp2.push_back(mFrameTemp[i]);
+			}
+			mFrameTemp.clear();
+			mFrameTemp = tmp2;
+			mIsFrameReady = true;
+			return true;
+		}
+		else
+		{
+			tmp.push_back(n); /* getting data */
+		}
+
+		count++;
+		last_angle = n.angle;
+	}
+
+	return false;
+}
+const std::array<PointData, POINT_PER_PACK> &LiPkg::GetPkgData(void)
+{
+	mIsPkgReady = false;
+	return mOnePkg;
+}
+
+std::vector<cv::Mat> LiPkg::scan_to_pointcloud(FrameData mFrameData)
+{
+	u_int32_t len = mFrameData.len;
+	float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min);
+	float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max);
+	cv::Mat pointcloud2(len, 3, CV_32F);
+	cv::Mat intensities(1, len, CV_32F);
+	cv::Mat pointdis(1, len, CV_32F);
+	float a_increment = (angle_max - angle_min) / (float)(len - 1);
+	for (int i = 0; i < len; i++)
+	{
+		float dis = mFrameData.distance[i] / 1000.f;
+		intensities.at<float>(0, i) = mFrameData.intensities[i];
+		pointdis.at<float>(0, i) = dis;
+
+		float x_temp = cos(angle_min + i * a_increment) * dis;
+		float y_temp = sin(angle_min + i * a_increment) * dis;
+		pointcloud2.at<float>(i, 0) = x_temp;
+		pointcloud2.at<float>(i, 1) = y_temp;
+		pointcloud2.at<float>(i, 2) = 0.0;
+	}
+
+	return std::vector<cv::Mat>{pointcloud2, intensities, pointdis};
+}
+
+/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
+
+SlTransform::SlTransform(LDVersion version, bool to_right_hand)
+{
+	switch (version)
+	{
+	case LDVersion::LD_ZERO:
+	case LDVersion::LD_NINE:
+		offset_x = 8.1;
+		offset_y = -22.5156;
+		break;
+
+	case LDVersion::LD_THREE:
+	case LDVersion::LD_EIGHT:
+		offset_x = 5.9;
+		offset_y = -20.14;
+		break;
+
+	case LDVersion::LD_FOURTEENTH:
+		offset_x = 5.9;
+		offset_y = -20.14;
+		break;
+
+	default:
+		break;
+	}
+}
+Points2D SlTransform::Transform(const Points2D &data)
+{
+	Points2D tmp2;
+	for (auto n : data)
+	{
+		/*Filter out invalid data*/
+		if (n.distance == 0)
+		{
+			continue;
+		}
+		/*transfer the origin to the center of lidar circle*/
+		/*The default direction of radar rotation is clockwise*/
+		/*transfer to the right-hand coordinate system*/
+		float right_hand = (360.f - n.angle);
+		double x = n.distance + offset_x;
+		double y = n.distance * 0.11923 + offset_y;
+		double d = sqrt(x * x + y * y);
+		double shift = atan(y / x) * 180.f / 3.14159;
+		/*Choose whether to use the right-hand system according to the flag*/
+		double angle;
+		if (to_right_hand)
+			angle = right_hand + shift;
+		else
+			angle = n.angle - shift;
+
+		if (angle > 360)
+		{
+			angle -= 360;
+		}
+		if (angle < 0)
+		{
+			angle += 360;
+		}
+
+		tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y));
+	}
+
+	return tmp2;
+}
+SlTransform::~SlTransform()
+{
+}
+void LD14_LiPkg::Transform(std::vector<PointData> &tmp)
+{
+	SlTransform trans(LDVersion::LD_FOURTEENTH);
+	tmp = trans.Transform(tmp);
+	// std::cout << "Transform LD_FOURTEENTH !!" << std::endl;
+}
\ No newline at end of file
diff --git a/lidar.h b/lidar.h
new file mode 100644
index 0000000..49b1d25
--- /dev/null
+++ b/lidar.h
@@ -0,0 +1,174 @@
+/**
+* @file         lidar.h
+* @author       LD Robot
+* @version      V01
+
+* @brief
+* @note
+* @attention    COPYRIGHT LDROBOT
+**/
+
+#ifndef __LIPKG_H
+#define __LIPKG_H
+#include <stdint.h>
+#include <vector>
+#include <array>
+#include <iostream>
+#include <functional>
+#include <thread>
+#include <atomic>
+#include <inttypes.h>
+#include <mutex>
+#include <string>
+#include <condition_variable>
+#include <sys/file.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <termios.h>
+#include <memory.h>
+#include <libudev.h>
+#include<opencv2/opencv.hpp>
+#include <ros/ros.h>
+
+struct PointData
+{
+	//�������ʾ��ʽ
+	float angle;
+	uint16_t distance;
+	uint8_t confidence;
+	//ֱ�������ʾ��ʽ
+	double x;
+	double y;
+	PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0)
+	{
+		this->angle = angle;
+		this->distance = distance;
+		this->confidence = confidence;
+		this->x = x;
+		this->y = y;
+	}
+	PointData() {}
+	friend std::ostream& operator<<(std::ostream& os, const PointData& data)
+	{
+		os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y;
+		return  os;
+	}
+};
+typedef std::vector<PointData> Points2D;
+
+
+enum
+{
+	PKG_HEADER = 0x54,
+	PKG_VER_LEN = 0x2C,
+	POINT_PER_PACK = 12,
+};
+typedef struct  __attribute__((packed))
+{
+	uint16_t distance;
+	uint8_t  confidence;
+}LidarPointStructDef;
+typedef struct  __attribute__((packed))
+{
+	uint8_t           header;
+	uint8_t           ver_len;
+	uint16_t          speed;
+	uint16_t          start_angle;
+	LidarPointStructDef point[POINT_PER_PACK];
+	uint16_t          end_angle;
+	uint16_t          timestamp;
+	uint8_t           crc8;
+}LiDARFrameTypeDef;
+struct FrameData
+{
+	ros::Time timestamp;
+	float angle_min;
+	float angle_max;
+	uint32_t len;
+	std::vector<uint16_t> distance;
+	std::vector<uint8_t> intensities;
+};
+class LiPkg
+{
+public:
+	LiPkg();
+	double GetSpeed(void);	/*Lidar spin speed (Hz)*/
+	uint16_t GetTimestamp(void) { return mTimestamp; }   /*time stamp of the packet */
+	bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */
+	bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/
+	long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/
+	const std::array<PointData, POINT_PER_PACK>& GetPkgData(void);/*original data package*/
+	bool Parse(const uint8_t* data, long len);/*parse single packet*/
+	virtual void Transform(std::vector<PointData>& tmp) = 0;/*transform raw data to stantard data */
+	bool AssemblePacket();/*combine stantard data into data frames and calibrate*/
+	const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; }
+	std::vector<cv::Mat> scan_to_pointcloud(FrameData mFrameData);
+private:
+	uint16_t mTimestamp;
+	double mSpeed;
+	std::vector<uint8_t> mDataTmp;
+	long mErrorTimes;
+	std::array<PointData, POINT_PER_PACK>mOnePkg;
+	std::vector<PointData> mFrameTemp;
+	FrameData mFrameData;
+	bool mIsPkgReady;
+	bool mIsFrameReady;
+};
+class LD14_LiPkg : public LiPkg
+{
+public:
+	virtual void Transform(std::vector<PointData>& tmp);
+};
+
+enum class LDVersion
+{
+	LD_ZERO,  /*Zero  generation lidar*/
+	LD_THREE, /*Third generation lidar*/
+	LD_EIGHT, /*Eight generation radar*/
+	LD_NINE,  /*Nine  generation radar*/
+	LD_FOURTEENTH /*Fourteenth generation radar*/
+};
+
+class SlTransform
+{
+private:
+	bool to_right_hand = true;
+	double offset_x;
+	double offset_y;
+
+public:
+	SlTransform(LDVersion version, bool to_right_hand = false);
+	Points2D Transform(const Points2D& data);
+	~SlTransform();
+};
+
+
+
+class CmdInterfaceLinux
+{
+public:
+	CmdInterfaceLinux(int32_t ver);
+	~CmdInterfaceLinux();
+
+	bool Open(std::string& port_name);
+	bool Close();
+	bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len);
+	bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len);
+	bool GetCmdDevices(std::vector<std::pair<std::string, std::string> >& device_list);
+	void SetReadCallback(std::function<void(const char*, size_t length)> callback) { mReadCallback = callback; }
+	bool IsOpened() { return mIsCmdOpened.load(); };
+	bool IsExited() { return mRxThreadExitFlag.load(); };
+
+private:
+	std::thread* mRxThread;
+	static void mRxThreadProc(void* param);
+	long long mRxCount;
+	int32_t version;
+	int32_t mComHandle;
+	std::atomic<bool> mIsCmdOpened, mRxThreadExitFlag;
+	std::function<void(const char*, size_t length)> mReadCallback;
+};
+
+#endif
+/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
diff --git a/m28.rknn b/m28.rknn
new file mode 100644
index 0000000..fb3a6d4
Binary files /dev/null and b/m28.rknn differ
diff --git a/main.cpp b/main.cpp
new file mode 100644
index 0000000..3796d0d
--- /dev/null
+++ b/main.cpp
@@ -0,0 +1,167 @@
+#include <iostream>
+#include <stdio.h>
+#include "lidar.h"
+#include <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+#include <opencv2/opencv.hpp>
+#include "ranging.h"
+#include "fusion.h"
+#include <string>
+#include <pthread.h>
+#include <mutex>
+#include <sys/time.h>
+/*#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>*/
+#include "opencv2/imgcodecs/imgcodecs.hpp"
+#include <ctime>
+
+#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
+
+Ranging r(9, 640, 480);
+std::queue<std::vector<Mat>> frameInfo;
+std::queue<FrameData> laser_queue;
+std::mutex g_mutex;
+
+void *ranging(void *args)		// ranging线程
+{
+	while (true)
+	{
+		std::cout<<" ************ enter ranging *********** "<<std::endl;
+		std::vector<Mat> result = r.get_range();
+		g_mutex.lock();
+		for (uchar i = 0; i < frameInfo.size(); i++)		// 只保存当前最新的图片帧信息
+		frameInfo.pop();
+		frameInfo.push(result);
+		g_mutex.unlock();
+	}
+}
+
+int main(int argc, char **argv)
+{
+	uint8_t laser_queue_size = 2;		// 雷达帧队列大小参数
+	LD14_LiPkg *pkg = new LD14_LiPkg;
+	CmdInterfaceLinux cmd_port(9);
+	std::string port_name = "/dev/ttyUSB0";
+	if (argc > 1)
+		port_name = argv[1];
+// 雷达读取回调函数，一直在执行雷达读取和处理数据
+	cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) {
+		if (pkg->Parse((uint8_t*)byte, len))
+		{
+			pkg->AssemblePacket();
+		} });
+	if (!cmd_port.Open(port_name))
+		return -1;
+	int type = VideoWriter::fourcc('M','J','P','G');
+	char name[256] = {0};
+	FILE *fp=NULL;
+	time_t timep;
+	struct tm *p;
+	time(&timep);
+	p = localtime(&timep);
+	sprintf(name,"%d.%d.%d %d:%02d.avi",1900+p->tm_year,1+p->tm_mon,p->tm_mday,p->tm_hour,p->tm_min);
+	VideoWriter writer(name, type, 30, Size(640, 480), true);
+	ros::init(argc, argv, "publish");
+	ros::NodeHandle nh; /* create a ROS Node */
+	ros::Publisher uplidar_pub = nh.advertise<sensor_msgs::LaserScan>("up_scan", 1);
+	//ros::Publisher downlidar_pub = nh.advertise<sensor_msgs::LaserScan>("down_scan", 1);
+	sensor_msgs::LaserScan upscan;
+	// sensor_msgs::LaserScan downscan;
+
+
+	/*image_transport::ImageTransport it(nh);
+        image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/
+	pthread_t tids[1];		// 执行ranging线程
+	int ret = pthread_create(&tids[0], NULL, ranging, NULL);
+	if (ret != 0)
+	{
+		std::cout << "Multithreading error !" << std::endl;
+		return -2;
+	}
+
+	LCFusion lcFusion(&r);
+	namedWindow("camera");
+
+	while (ros::ok() && !cmd_port.IsExited())
+	{
+		int64 t = getTickCount();
+		if (pkg->IsFrameReady())		// 若一帧雷达处理好，执行后续操作
+		{
+			FrameData laserData_queue = pkg->GetFrameData();		// 得到雷达数据
+			laserData_queue.timestamp = ros::Time::now();
+			laser_queue.push(laserData_queue);
+			while(laser_queue.size() > laser_queue_size)		// 只保存两帧雷达数据
+			{
+				laser_queue.pop();
+			}
+		}
+		else 
+			continue;
+		if (laser_queue.size() == laser_queue_size)			// 若队列有两帧数据，执行后续操作
+		{
+			FrameData laserData = laser_queue.front();
+			laser_queue.pop();
+			upscan.header.stamp = laserData.timestamp;
+			// upscan.header.stamp = ros::Time::now();
+			// downscan.header.stamp = upscan.header.stamp;
+
+			// uint16_t times_stamp = pkg->GetTimestamp();
+			// FrameData laserData = pkg->GetFrameData();
+			char state = -1;
+
+			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.);			// 角度增量
+			lcFusion.filter_laser(laserData, angle_min, angle_inc);			// 视角同步，处理视角内雷达数据
+			if (!frameInfo.empty())
+			{
+				g_mutex.lock();
+				std::vector<Mat> camInfo = frameInfo.front();				// 读取图像帧数据
+				frameInfo.pop();
+				g_mutex.unlock();
+				double fusion_old,fusion_now;
+				fusion_old = ros::Time::now().toSec();
+				state = lcFusion.fusion(camInfo);							// 融合
+				fusion_now = ros::Time::now().toSec();
+				cout << "data_fusion: " << fusion_now - fusion_old << endl;
+				if (state != -2)
+				{
+					lcFusion.set_laser_data(upscan);						// 设置雷达发布数据
+					uplidar_pub.publish(upscan);							// 发布
+				}
+				// 调试使用，将雷达点映射到图像中显示
+				if (!lcFusion.projectedPoints.empty())
+				{
+					for (int i = 0; i < lcFusion.projectedPoints.size(); i++)
+					{
+						Point2f p = lcFusion.projectedPoints[i];
+						circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8);
+					}
+				}
+				 /* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg();
+    				img_pub.publish(msg);*/
+				if (!camInfo[0].empty())
+				{
+					cv::Mat pic;
+					cv::Size dsize= Size(int(640*0.75),int(480*0.75));
+					cv::resize(camInfo[0],pic,dsize,0,0,INTER_AREA);
+					// cv::pyrDown(camInfo[0],pic);
+					writer.write(camInfo[0]);
+					imshow("camera", pic);
+				}
+				t = getTickCount() - t;
+				char fps[50];
+				std::cout << "fps:  " << int(1 / (t / getTickFrequency())) << std::endl;
+				if (waitKey(1) == 81)
+					break;
+			}
+			//lcFusion.set_laser_data(downscan, laserData);
+			//downlidar_pub.publish(downscan);
+		}
+	}
+	std::cout << "ROS or lidar error !" << std::endl;
+	delete pkg;
+	cmd_port.Close();
+	return 0;
+}
diff --git a/main.out b/main.out
new file mode 100644
index 0000000..02cf85f
Binary files /dev/null and b/main.out differ
diff --git a/ranging.cpp b/ranging.cpp
new file mode 100644
index 0000000..749b393
--- /dev/null
+++ b/ranging.cpp
@@ -0,0 +1,444 @@
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <iostream>
+#include <stdio.h>
+#include "ranging.h"
+#include <math.h>
+#include <stdlib.h>
+
+#include "rknn_sdk.h"
+#include <string.h>
+#include <sys/time.h>
+#include <dlfcn.h>
+#include "opencv2/core.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/imgproc.hpp"
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <vector>
+#include <tuple>
+#include <string>
+#include <string.h>
+#include <algorithm>
+#include <ros/ros.h>
+
+using namespace cv;
+
+
+void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数
+{
+    remap(oriImgL, oriImgL, mapX1, mapX2, INTER_LINEAR);
+    remap(oriImgR, oriImgR, mapY1, mapY2, INTER_LINEAR);
+}
+
+std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
+{
+    //(u,v)->(x,y)"(loc[0],loc[1])"
+    std::vector<float> loc;
+    loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
+    loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
+    return loc;
+}
+
+std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配
+{
+    int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
+    // cv::Mat right_image_,left_image_;
+    // cv::pyrDown(right_image, right_image_);
+    // cv::pyrDown(left_image, left_image_);
+    Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框
+    Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域
+    // Mat tpl = right_image_.rowRange(max(int(0.5*y1 - 2), 0), min(int(0.5*y2 + 2), 479)).colRange(int(0.5*x1), int(0.5*x2));
+    // Mat target = left_image_.rowRange(max(int(0.5*y1 - 10), 0), min(int(0.5*y2 + 10), 239)).colRange(0, 319);
+    int th = tpl.rows, tw = tpl.cols;
+    Mat result;
+    // methods = TM_CCOEFF_NORMED, TM_SQDIFF_NORMED, TM_CCORR_NORMED]
+    matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关
+    double minVal, maxVal;
+    Point minLoc, maxLoc; 
+    minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标
+    Point tl = maxLoc, br;
+
+    br.x = min(maxLoc.x + tw, 639); //转为像素坐标系
+    br.y = min(maxLoc.y + th, 479); //转为像素坐标系
+    ////展示匹配结果
+    // br.x = min(maxLoc.x + tw, 319);
+    // br.y = min(maxLoc.y + th, 239);
+    //rectangle(target, tl, br, (0, 255, 0), 3);
+    //imshow("match-", target);
+    //waitKey(2);
+    std::vector<int> maxloc;
+    maxloc.push_back(maxLoc.x); 
+    maxloc.push_back(maxLoc.y); 
+    return maxloc;
+}
+
+void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k)
+{
+    //保证摄像头与地面平行
+	int x1 = bboxs.at<float>(k, 0);
+	int x2 = bboxs.at<float>(k, 2);
+	int y1 = bboxs.at<float>(k, 1);
+	int y2 = bboxs.at<float>(k, 3);
+    float Conf = bboxs.at<float>(k, 4);
+    int cls = bboxs.at<float>(k, 5);
+	float Y_B, Y_H;
+	Mat edge, grayImage;
+	vector<cv::Point> idx;
+    Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围
+    //Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639);
+	Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1);
+	cvtColor(tpl, grayImage, COLOR_BGR2GRAY); 
+	GaussianBlur(grayImage,grayImage,Size(5,5),0);
+	Canny(grayImage, edge, 120, 180, 3);  //提取边缘，获取与地面接触点
+    //cv::imshow("1",edge);
+    //cv::waitKey(1);
+    float cluster[650];
+	for (int i = 0;i<650;i++)
+	{
+	    cluster[i] = 0;
+	}
+	int y_b, y_h;
+    int j = 0;
+	for (int i = 0; i < x2-x1; i++)
+	{
+		//Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2);
+		Mat temp = edge.col(i); //取第i列
+        // std::cout << "temp: " <<temp << std::endl;
+		cv::findNonZero(temp, idx);
+        std::vector<float> point_b = pic2cam(x1 + i, 240);  //转为相机坐标系
+		std::vector<float> point_H = pic2cam(320, 240);
+        float alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
+        if (idx.size() < 1)
+        {
+            Z.at<float>(0, i) = 0;
+		    Z.at<float>(1, i) = alfa;
+            continue;
+        }
+		int y_b = idx[idx.size() - 1].y + y1; //
+        y_b = int(y_b + y_b*0.03);
+		int y_h = 240;
+		point_b = pic2cam(x1 + i, y_b);  //转为相机坐标系
+		point_H = pic2cam(320, y_h);
+		Y_B = point_b[1];
+		Y_H = point_H[1];
+		// std::cout << "y_b:   " << y_b << std::endl;
+        float H_c = 60; //摄像头离地高度，单位mm
+        float theta = 0; //摄像头与地面夹角，弧度
+		float d = (1/cos(theta)* cos(theta)) * q.at<double>(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta);
+		alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
+        //cout << "d: " << d << endl;
+        if (d > 700)
+        {d = 0;}
+		Z.at<float>(0, i) = d/cos(alfa);
+		Z.at<float>(1, i) = alfa;
+	}
+ 	    /*if (i > 0)
+	    {
+		    if (abs(Z.at<float>(0, i) - Z.at<float>(0, i - 1)) < 40)       //聚类
+		    {
+		        cluster[j] = cluster[j] + 1;
+		    }
+		    else
+		    {
+		        j = j + 1;
+		        cluster[j] = 1;
+		        //std::cout<<"j :  "<< j<< std::endl;
+		    }
+	    }
+	}
+    //int max_loc = distance(cluster,max_element(cluster,cluster + sizeof(cluster)/sizeof(cluster[0]))); //只保留数量最多的一类，其余置0
+    //std::cout<<" max_loc :  "<< max_loc<<std::endl;
+    int temp = 0;
+    for (int i = 0; i < j; i++)
+    {
+        if (cluster[i] < 3)
+        {
+            for (int t = temp; t < temp + cluster[i]; t++)
+            {
+                Z.at<float>(0, t) = 0;
+            }
+        }
+ 	    temp = temp + cluster[i];
+    }*/
+    // std::cout << "z :  " <<Z << std::endl;
+    this->Z = Z.clone();
+}
+
+void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
+{
+    // ֱ��ͼ���⻯
+    Mat imgGrayL, imgGrayR;
+    cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY);
+    cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY);
+    Mat imgR_weight = imgR.clone();
+    Mat infoRow;
+    for (uchar i = 0; i < detBoxes.rows; i++)
+    {
+        int x1 = detBoxes.at<float>(i, 0);
+        int y1 = detBoxes.at<float>(i, 1);
+        int x2 = detBoxes.at<float>(i, 2);
+        int y2 = detBoxes.at<float>(i, 3);
+	//std::cout<<"x1: "<<x1<<"x2: "<<x2<<"y1: "<<y1<<"y2: "<<y2<<std::endl;
+        float Conf = detBoxes.at<float>(i, 4);
+        int cls = detBoxes.at<float>(i, 5);
+                
+        if (cls == 6 || cls == 10 || cls == 11) //体重秤，地毯和毛巾采用单目测距方法
+        {
+            horizon_estimate(imgR_weight, detBoxes, i);
+        }
+        if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体
+        {
+	
+            //char cm[15];
+            //sprintf(cm, "cannot match !");
+
+            char cm[15];
+            //sprintf(cm, "cannot match !");
+            sprintf(cm, "%d , %.2f", cls,Conf);
+            putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
+            infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+            infoRow.copyTo(info.row(i));
+            rectangle(imgR, Point(int(x1), int(y1)),
+            Point(int(x2), int(y2)), Scalar(0, 0, 255));
+            continue;
+        }
+        if (cls!=0 && cls!=1 && cls!=2 && cls!= 6 && cls!= 7 && cls!= 10 && cls!=11 && cls!=14 && cls!=15) //大物体不进行测距
+        {
+            if (x1 < 30 || x2 < 80 || x1>580 || x2 > 610) //删除边缘的目标框
+            {
+                detBoxes.at<float>(i, 5) = -1;
+                cls = detBoxes.at<float>(i, 5);
+            }
+                //char cm[15];
+                //sprintf(cm, "cannot match !");
+
+            char cm[15];
+            //sprintf(cm, "cannot match !");
+            sprintf(cm, "%d , %.2f", cls,Conf);
+            putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2);
+            infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+            infoRow.copyTo(info.row(i));
+            rectangle(imgR, Point(int(x1), int(y1)),
+            Point(int(x2), int(y2)), Scalar(0, 0, 255));
+            continue;
+        }
+
+        rectangle(imgR, Point(int(x1), int(y1)),
+                  Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框
+
+        int coordinates[4] = {x1, y1, x2, y2};
+        std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
+        float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差
+        float disp_pixel_y = disp_pixel[1] - y1; //
+        disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到
+
+        //Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz;
+        Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息
+        Mat threed_pixel_xyz, threedImage; 
+        reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
+        threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方，
+        std::vector<Mat> channels;
+        split(threed_pixel_xyz.clone(), channels);
+        threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离
+        threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d
+
+        int mid_pixel = int((x1 + x2) / 2);
+        std::vector<float> mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标
+        std::vector<float> loc_tar = pic2cam(mid_pixel, imgGrayR.rows);
+        float alfa = atan((loc_tar[0] - mid[0]) / q.at<double>(2, 3));
+
+
+        if (disp_pixel_x > 240)        // 距离太近，视差过大
+        {
+            char cm[15];
+            //sprintf(cm, "cannot match !");
+            sprintf(cm, "%d , %.2f", cls,Conf);
+            putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
+            infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+            infoRow.copyTo(info.row(i));
+            continue;
+        }
+        else
+        {
+            float median = threed_pixel_xyz.at<float>((int)(y1 + y2) / 2, (int)(x1 + x2) / 2);
+
+            std::vector<float> ltPoint = pic2cam(x1, y1);
+            std::vector<float> rbPoint = pic2cam(x2, y2);
+            float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高
+            float f = q.at<double>(2, 3);
+            float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出
+            //float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1;
+            float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1;
+            float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f);
+            //float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2;
+            float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2;
+            float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
+                            pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
+                            pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
+
+            w1 = w1 / 10;
+            h1 = (h1 + h2) / 10;
+            median /= 10;
+            if (median > 120) //过远测距误差较大
+            {
+                //char tf[9];
+                //sprintf(tf, "Too far!");
+                char cm[15];
+                //sprintf(cm, "cannot match !");
+                sprintf(cm, "%d , %.2f", cls,Conf);
+                putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
+                infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
+                infoRow.copyTo(info.row(i));
+                continue;
+            }
+            // ��ͼ���ϻ�����Ϣ
+            char dc[50], wh[50];
+            std::string cname = className[cls + 1];
+            sprintf(dc, "dis:%.2fcm  %d", median, cls);
+            sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa);
+            putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2);
+            putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5);
+
+
+            //返回数据
+            infoRow = (Mat_<float>(1, 4) << median, w1, h1, alfa);
+            infoRow.copyTo(info.row(i));
+        };
+    }
+}
+
+Ranging::Ranging(int index, int imgw, int imgh) : //初始化
+    mapX1(imgh, imgw, CV_64F),   //初始化矩阵  ，用于计算无畸变和修正转换映射。                                
+    mapX2(imgh, imgw, CV_64F),                                     
+    mapY1(imgh, imgw, CV_64F),
+    mapY2(imgh, imgw, CV_64F),
+    q(4, 4, CV_64F),
+    imgw(imgw),
+    imgh(imgh)
+{
+    // Z = Mat::zeros(2, 1, CV_32FC1);
+    if (!vcapture.open(index))
+    {
+        std::cout << "Open camera failed !" << std::endl;
+        exit(EXIT_FAILURE);
+    }
+    else
+    {
+        //vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
+        vcapture.set(CAP_PROP_FPS, 30);
+        vcapture.set(CAP_PROP_FRAME_WIDTH, imgw * 2);
+        vcapture.set(CAP_PROP_FRAME_HEIGHT, imgh);
+	    vcapture.set(CAP_PROP_BUFFERSIZE, 1);
+
+        auto imgSize = Size(imgw, imgh);
+        Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F);
+
+        stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,
+                      imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正
+
+        initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射
+        initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射
+		
+        RKNN_Create(&hdx, modelPath); // 初始化检测模型
+	std::cout<< " ******************* CAMERA  initialization ********************" << std::endl;
+    }
+}
+
+// std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
+// {
+// 	std::vector<float> loc;
+// 	loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
+// 	loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
+// 	return loc;
+// }
+
+
+
+std::vector<Mat> Ranging::get_range() 
+{
+	double rang_old, rang_now;
+	rang_old = ros::Time::now().toSec(); //测试运行时间
+    Mat frame, lframe, rframe;
+    vcapture.read(frame); //获取视频帧
+    
+    if (!frame.empty())
+    {
+	int64 t = getTickCount();
+    Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图
+    Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图
+    //imshow("lframe",lframe); 
+    //waitKey(1);
+    rectifyImage(lframe, rframe); //
+
+    DetRet *det_ret = NULL; //
+    InputData input_data; //定义输入数据
+    int shape[4] = {1, 3, rframe.rows, rframe.cols}, det_num = 0;
+    memcpy(input_data.shape, shape, sizeof(shape));
+	Mat Rframe = rframe.clone();
+
+	double detect_old, detect_now;
+	detect_old = ros::Time::now().toSec();
+        input_data.data = Rframe.data;
+	//std::cout<<"Rframe.data.shape"<<Rframe.rows<<std::endl;
+        RKNN_ObjDet(hdx, &input_data, &det_ret, &det_num);
+	detect_now = ros::Time::now().toSec();
+
+	//cout << "data_detect_time:" << detect_now-detect_old << endl;
+	//std::cout<<"det_num  "<<det_num<<std::endl;
+        if (!det_num)
+        {
+	    //std::cout<<"return NULL"<<std::endl;
+            return std::vector<Mat>{rframe}; //没有检测框时，退出
+        }
+        std::cout << "det_num:  " << det_num << std::endl;
+        // detction box transfor to our format
+        Mat detBoxes(det_num, 6, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls)
+        for (int i = 0; i < det_num; i++) //存储目标检测内容 (x,y,x,y,conf,cls)
+        {
+            DetRet det_result = det_ret[i];
+            // xyxy conf cls
+            float xmin = rframe.cols * det_result.fLeft;
+            float ymin = rframe.rows * det_result.fTop;
+            float xmax = rframe.cols * det_result.fRight;
+            float ymax = rframe.rows * det_result.fBottom;
+
+            detBoxes.at<float>(i, 0) = xmin;
+            detBoxes.at<float>(i, 1) = ymin;
+            detBoxes.at<float>(i, 2) = xmax;
+            detBoxes.at<float>(i, 3) = ymax;
+            detBoxes.at<float>(i, 4) = det_result.fConf;
+            // 实验测试，过滤过大的误检框
+            float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
+            if (ratio > 0.7)
+            {
+                detBoxes.at<float>(i, 5) = -1;
+                continue;
+            }
+            detBoxes.at<float>(i, 5) = det_result.nLabel;
+            if (det_result.nLabel == 1 || det_result.nLabel == 2) //检测目标为体重秤和风扇底座时，将目标框拉高，使杆子的雷达点能与目标框对应
+            {
+                detBoxes.at<float>(i, 1) = 100;
+            }
+        }
+        Mat info(det_num, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
+        if (det_num)
+        {
+		// double rang_old, rang_now;
+		// rang_old = ros::Time::now().toSec();
+            getInfo(lframe, rframe, detBoxes, info);
+		// rang_now = ros::Time::now().toSec();
+		// cout << "data_dis_time: " << rang_now-rang_old << endl;
+        }
+        t = getTickCount() - t;
+        char fps[50];
+        sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency())));
+        putText(rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5);
+	    // rang_now = ros::Time::now().toSec();
+	    // cout << "data_rang_time" << rang_now - rang_old << endl;
+        return std::vector<Mat>{rframe, detBoxes, info};
+    }
+    return std::vector<Mat>{rframe};
+}
diff --git a/ranging.h b/ranging.h
new file mode 100644
index 0000000..25d0f9f
--- /dev/null
+++ b/ranging.h
@@ -0,0 +1,134 @@
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/calib3d.hpp>
+
+#include <cstdlib>
+#include <vector>
+#include "rknn_sdk.h"
+#include "fusion.h"
+
+using namespace cv;
+class LCFusion;
+class Ranging
+{
+private:
+    friend class LCFusion;
+    VideoCapture vcapture;
+    rknn_handle hdx;
+    const char *modelPath = "m28.rknn";
+    std::vector<std::string> className = {
+        "Unknown",      //未知
+        "MaoGouWan",    //猫碗/狗碗
+        "BTYDiZuo",     //吧台怅底座
+        "FSDiZuo",      //风扇底座
+        "XiYiJi",       //掗衣机
+        "BingXiang",    //冰箱
+        "MaTong",       //马桶
+        "TiZhongCheng", //䜓重秀
+        "DianXian",     //电线
+        "DianShiJi",    //电视机
+        "CanZhuo",      //逐桌
+        "DiTan",        //地毯
+        "MaBu",         //抹垃
+        "ChaJi",        //茶几
+        "DianShiGu",    //电视柜
+        "XieZi",        //拖鞋/鞋子
+        "WaZi",         //袜子
+        "YiGui",        //衣柜
+        "Chuang",       //床
+        "ShaFa",        //沙发
+        "YiZi",         //怅子
+        "Men",          //闹
+        "DLS",          //倧理石
+        "DiBan",        //地板
+    };
+
+    // 双目参数
+/*
+   Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+        4.700950170847520e+02, 0, 0,
+        0, 4.709244004725060e+02, 0,
+        3.220628601465352e+02, 2.265265667516499e+02, 1);
+
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+        4.696016526986375e+02, 0, 0,
+        0, 4.702371464881610e+02, 0,
+        3.216994731643572e+02, 2.267449827655198e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) << 0.088023418049400, -0.047968406803285, 0,
+        0, 0);
+
+    Mat distortion_r = (Mat_<double>(1, 5) << 0.097018738959281, -0.081707209740704, 0,
+        0, 0);
+
+    Mat rotate = (Mat_<double>(3, 3) << 
+        0.999895538560624, 1.514179660491009e-04, 0.014452994124399,
+        -1.935031776135467e-04, 0.999995745723263, 0.002910514026079,
+        -0.014452491933249, -0.002913006689884, 0.999891314028152);
+    Mat trans = (Mat_<double>(3, 1) << 
+        -59.978995009996730, 0.050886852679934, -0.088565378305175);*/
+
+   Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+       4.860907245880164e+02, 0, 0,
+       0, 4.899923700242200e+02, 0,
+       2.986108795628009e+02, 2.423211232345708e+02, 1);
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+        4.833477325585679e+02, 0, 0,
+        0, 4.868370803438677e+02, 0,
+        2.976599964943864e+02, 2.310278327659757e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) << 0.126100629219048, -0.152357884696085, 0, 0, 0);
+
+    Mat distortion_r = (Mat_<double>(1, 5) << 0.117495161060377, -0.131936017409618, 0, 0, 0);
+
+    Mat rotate = (Mat_<double>(3, 3) << 
+        0.999964066385317, 0.001108038258875, 0.008404652839816,
+        -0.001043883137806, 0.999970316987207, -0.007633836027844,
+        -0.008412861946779, 0.007624788241143, 0.999935541101596);
+    Mat trans = (Mat_<double>(3, 1) << 
+        -60.419864984062440, -0.059256838559736, -0.609155889994525);
+
+
+/*
+
+    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
+        3.627346593660044e+02, 0, 0,
+        0, 3.629168542317134e+02, 0,
+        3.093231127433517e+02, 2.409353074183058e+02, 1);
+
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
+        3.615909129180773e+02, 0, 0,
+        0, 3.617772267770084e+02, 0,
+        3.062363746645480e+02, 2.298948842159400e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) << 0.117219440226075, -0.148594691012994, 0,
+        0, 0);
+    Mat distortion_r = (Mat_<double>(1, 5) << 0.116412295415583, -0.146817143572705, 0,
+        0, 0);
+    Mat rotate = (Mat_<double>(3, 3) << 
+        0.999962353120719, 4.648106081008926e-04, 0.008664657660520,
+        -4.493098874759400e-04, 0.999998295540464, -0.001790820145135,
+        -0.008665475284162, 0.001786859609987, 0.999960857569352);
+    Mat trans = (Mat_<double>(3, 1) << 
+        -60.097178758944190, -0.033859553542635, -0.423965574285253);
+*/
+/*
+    Mat cam_matrix_left = (Mat_<double>(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1);
+    Mat cam_matrix_right = (Mat_<double>(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1);
+    Mat distortion_l = (Mat_<double>(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682);
+    Mat distortion_r = (Mat_<double>(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122);
+    Mat rotate = (Mat_<double>(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437);
+    Mat trans = (Mat_<double>(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760);
+*/
+
+    Mat mapX1, mapX2, mapY1, mapY2, q, Z;
+
+    int imgw, imgh;
+
+public:
+    Ranging(int index, int imgw, int imgh);
+    void rectifyImage(Mat &oriImgL, Mat &oriImgR);
+    void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info);
+    std::vector<float> pic2cam(int u, int v);
+    std::vector<int> muban(Mat &left_image, Mat &right_image, const int *coordinates);
+    std::vector<Mat> get_range();
+    void horizon_estimate(Mat& img, Mat& bboxs,int k);
+};
diff --git a/rknn_sdk.h b/rknn_sdk.h
new file mode 100644
index 0000000..cda1b0f
--- /dev/null
+++ b/rknn_sdk.h
@@ -0,0 +1,110 @@
+#ifndef __RKNN_SDK_H__
+#define __RKNN_SDK_H__
+
+#include <stdint.h>
+
+// 返回错误码
+enum XErrorCode {
+	RKNN_SUCCEED         =  0,         
+	RKNN_NOT_INIT        = -1,  //SDK未初始化
+	RKNN_INVAILD_IMGSIZE = -2,  //输入图像尺寸不匹配模型输入要求
+	RKNN_INVAILD_PARAM   = -3,  //参数无效                            
+};
+
+enum ObjCls {
+	OBJ_Unknown      = -1,  //未知
+	OBJ_MaoGouWan    = 0,   //猫碗/狗碗      
+	OBJ_BTYDiZuo     = 1,   //吧台椅底座
+	OBJ_FSDiZuo      = 2,   //风扇底座
+	OBJ_XiYiJi       = 3,   //洗衣机
+	OBJ_BingXiang    = 4,   //冰箱    
+	OBJ_MaTong       = 5,   //马桶
+	OBJ_TiZhongCheng = 6,   //体重秤
+	OBJ_DianXian     = 7,   //电线
+	OBJ_DianShiJi    = 8,   //电视机
+	OBJ_CanZhuo      = 9,   //餐桌
+	OBJ_DiTan        = 10,  //地毯
+	OBJ_MaBu         = 11,  //抹布
+	OBJ_ChaJi        = 12,  //茶几
+	OBJ_DianShiGui   = 13,  //电视柜
+	OBJ_XieZi        = 14,  //拖鞋/鞋子
+	OBJ_WaZi         = 15,  //袜子
+	OBJ_YiGui        = 16,  //衣柜
+	OBJ_Chuang       = 17,  //床
+	OBJ_ShaFa        = 18,  //沙发
+	OBJ_YiZi         = 19,  //椅子
+	OBJ_Men          = 20,  //门
+	OBJ_DLS          = 21,   //da li shi
+	OBJ_DiBan        = 22   //di ban
+	//OBJ_Xiang        = 21   //宠物粪便
+};
+
+
+// 图像信息与buffer
+typedef struct _InputData {   
+	int    shape[4]; // Num, Channel, Height, Width       
+	void*  data;     // img.data
+} InputData;
+
+// 检测结果
+typedef struct _DetRet
+{
+	int    nID;       //图像ID
+	int    nLabel;    //物体类别 ObjCls nLabel;    //物体类别
+	float  fConf;     //置信度
+	float  fLeft;     //检测框左上角比例坐标x_min -> int(x_min*width)  获取像素坐标
+	float  fTop;      //检测框左上角比例坐标y_min -> int(y_min*height) 获取像素坐标
+	float  fRight;    //检测框右下角比例坐标x_max -> int(x_max*width)  获取像素坐标
+	float  fBottom;   //检测框右下角比例坐标y_max -> int(y_max*height) 获取像素坐标
+} DetRet;
+
+
+
+#define RKNNSDK_API(retType) retType
+
+///
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef uint64_t rknn_handle;
+
+
+/*  RKNN_Create 
+    SDK加载模型，仅初始化一次;  
+	input:
+		rknn_handle hd                 句柄
+		const char* model_path         模型路径
+	return:
+	    int                            错误码
+*/
+RKNNSDK_API(int) RKNN_Create(rknn_handle* hd, const char* model_path);
+
+/*  RKNN_ObjDet
+	输入图像，获取检测结果和检测目标数量;
+	input:
+		rknn_handle hd                 句柄
+		const InputData* input_data    输入图像数据
+		DetRet** det_ret               获取检测结果
+		int* det_num                   获取检测目标数量
+	return:
+	    int                            错误码
+*/
+RKNNSDK_API(int) RKNN_ObjDet(rknn_handle hd, const InputData* input_data, DetRet** det_ret, int* det_num);
+
+/*  RKNN_Release 
+    SDK资源释放，仅在程序退出前调用;
+	input:
+		rknn_handle hd                 句柄
+	return:
+	    int                            错误码
+*/
+RKNNSDK_API(int) RKNN_Release(rknn_handle hd);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif //__RKNN_SDK_H__
diff --git a/zhixing-1 b/zhixing-1
new file mode 100644
index 0000000..cb03575
--- /dev/null
+++ b/zhixing-1
@@ -0,0 +1 @@
+g++  -std=c++11 ./*.cpp -o main.out -I /usr/local/include -I /usr/local/include/opencv4 -I /usr/include -I /opt/ros/melodic/include -I /home/firefly/Desktop/object_detect_sdk_1804/install/include  -L /usr/local/lib -l opencv_core -l opencv_imgproc -l opencv_imgcodecs -l opencv_highgui -l opencv_dnn -l opencv_videoio -l opencv_calib3d -L /usr/lib/obj_sdk -l rknn_sdk -l rknnrt -l roscpp -l roslib -l rosconsole -l roscpp_serialization -L /opt/ros/melodic/lib -l rostime -l boost_system -l pthread -l udev
\ No newline at end of file
diff --git a/说明.txt b/说明.txt
new file mode 100644
index 0000000..319487e
--- /dev/null
+++ b/说明.txt
@@ -0,0 +1,5 @@
+V 2.0
+
+1. 添加了基于地平线的测距方法，稳定性待测试。
+2. 解决断连bug。原因是融合的判空与双目判空条件不一致。
+3. 
\ No newline at end of file