diff --git a/双目测距文档.docx b/双目测距文档.docx
new file mode 100644
index 0000000..db5dae1
--- /dev/null
+++ b/双目测距文档.docx
@@ -0,0 +1,425 @@
+                     双目测距算法文档V.2022_06_23
+一.算法概述
+1.算法解决问题
+   双目立体视觉，用于立体感知，利用相似三角形的原理就可以得到立体信息。双目摄像头的引入为了解决下列问题：
+oo   解决雷达盲区的问题：利用双目摄像头对盲区物体测量目标距离及宽度。
+oo   解决雷达绘图的问题：对于风扇等物体，雷达仅能探测细杆，风扇的底座处于雷达视野外，视觉通过给出宽度补充底座。
+ 输入，输出，数据格式
+输入双目摄像头数据，输出右图，检测框以及测距信息。
+输入
+                          双目摄像头视频数据
+输出
+右图(Mat)
+检测框(Mat)
+测距信息(Mat)
+检测框存放格式(x1,y1,x2,y2,fConf,cls)
+测距信息存放格式( median, w1, h1, alfa)
+ 目前效果
+                                    厘米
+                                    测试
+                                    真实
+                                       
+                                       
+                                    距离
+                                    宽度
+                                    距离
+                                    宽度
+                                       1
+                                    拖鞋
+                                      37
+                                     22.2
+                                     38.5
+                                     25.5
+                                       2
+                                    地毯
+                                       
+                                   49.6/76.2
+                                       
+                                     48/68
+                                       3
+                                    电线
+                                     48.05
+                                      10
+                                     48.5
+                                      11
+                                       4
+                                    抹布
+                                      42
+                                       
+                                      40
+                                       
+                                       5
+                                       
+                                     70.8
+                                       
+                                      73
+                                       
+                                       6
+                                    拖鞋
+                                      65
+                                      22
+                                     63.5
+                                      23
+                                       7
+                                    地毯
+                                       
+                                     55/74
+                                       
+                                     48/68
+                                       8
+                                   体重秤
+                                      37
+                                      25
+                                     35.5
+                                      28
+                                       9
+                                       
+                                     49.77
+                                      27
+                                     51.5
+                                      28
+                                      10
+                                       
+                                     78.65
+                                      26
+                                      80
+                                      28
+                                      11
+                                    狗盆
+                                     49.8
+                                     14.6
+                                      48
+                                      15
+                                      12
+                                       
+                                     80.9
+                                       
+                                     79.5
+                                       
+                                      13
+                                       
+                                     28.73
+                                       
+                                     25.1
+                                       
+                                      14
+                                       
+                                     94.5
+                                       
+                                     93.5
+                                       
+                                      15
+                                 风扇底座
+                                       
+                                      40
+                                       
+                                      39
+                                      16
+                                       
+                                       
+                                     39.28
+                                       
+                                      39
+                                      17
+                                       
+                                       
+                                     40.74
+                                       
+                                      39
+                                      18
+                                    电线
+                                     33.9
+                                     12.35
+                                      31
+                                      14
+                                      19
+                                       
+                                     49.83
+                                       
+                                      49
+                                       
+                                      20
+                                       
+                                     72.7
+                                       
+                                      73
+                                       
+                                      21
+                                       
+                                      97
+                                       
+                                      95
+                                       
+                                      22
+                                    袜子
+                                     97.53
+                                     19.8
+                                     104.5
+                                      21
+                                      23
+                                       
+                                     80.88
+                                     18.15
+                                     81.3
+                                      21
+                                      24
+                                       
+                                     39.13
+                                     18.49
+                                      42
+                                      21
+                                      25
+                                    拖鞋
+                                     74.4
+                                     23.6
+                                      79
+                                     23.5
+                                      26
+                                       
+                                     54.42
+                                       
+                                      58
+                                       
+                                      27
+                                       
+                                     63.35
+                                       
+                                     63.5
+                                       
+                                      28
+                                       
+                                     35.95
+                                       
+                                     38.5
+                                       
+                                      29
+                                   桌子腿
+                                    113.86
+                                       
+                                      113
+                                       
+                                      30
+                                       
+                                     141.6
+                                       
+                                      143
+                                       
+                                      31
+                                       
+                                     125.5
+                                       
+                                     124.5
+                                       
+                                      32
+                                       
+                                     150.3
+                                       
+                                      152
+                                       
+                                      33
+                                       
+                                       
+                                      102
+                                       
+                                      104
+                                      34
+                                   吧台椅
+                                      211
+                                       
+                                      212
+                                       
+                                      35
+                                    冰箱
+                                      224
+                                       
+                                      225
+                                       
+                                      36
+                                       
+                                       
+                                     39.2
+                                       
+                                     38.3
+                                      37
+                                   洗衣机
+                                      156
+                                       
+                                      156
+                                       
+                                      38
+                                       
+                                       
+                                      41
+                                       
+                                      37
+                                      39
+                                    马桶
+                                     198.5
+                                       
+                                     202.3
+                                       
+                                      40
+                                       
+                                       
+                                     28.3
+                                       
+                                      28
+                                      41
+                                      床
+                                     397.1
+                                       
+                                     397.3
+                                       
+                                      42
+                                       
+                                       
+                                     131.4
+                                       
+                                     130.7
+                                      43
+                                       
+                                       
+                                      190
+                                       
+                                      190
+                                      44
+                                    椅子
+                                     160.8
+                                       
+                                     161.7
+                                       
+                                      45
+                                       
+                                       
+                                     40.7
+                                       
+                                     40.7
+                                      46
+                                    桌子
+                                      247
+                                       
+                                      250
+                                       
+                                      47
+                                       
+                                       
+                                     86.4
+                                       
+                                     87.7
+                                      48
+                                       
+                                       
+                                     51.85
+                                       
+                                     53.3
+                                      49
+                                    衣柜
+                                     268.2
+                                       
+                                     271.5
+                                       
+                                      50
+                                       
+                                       
+                                      85
+                                       
+                                      80
+                                      51
+                                   电视柜
+                                       
+                                     31.7
+                                       
+                                      31
+                                      52
+                                    沙发
+                                       
+                                     73.17
+                                       
+                                      75
+
+二.详细描述
+1.框架图
+                                     false
+   2.1.1极线矫正
+   极线矫正包括相机内外参数标定，相机矫正参数计算以及立体矫正。
+  2.1.2 目标检测
+  目标检测检测出物体，返回目标类别和目标框位置。
+  2.1.3 基于模板匹配的测距方法
+  获得目标框在右图的位置，对左图搜索目标框的内容，搜索的y轴范围与右图目标框的位置一致，x轴范围为全部。返回搜索结果的目标框对应位置。
+  左右图目标框位置的水平位置作差，即为视差。
+  视差转为三维坐标。
+  基于目标框的像素宽度，目标物体的距离得到目标的宽。
+  基于目标框的中心点位置获得目标角度。
+  2.1.4 单目测距方法
+  获取目标框内图像。
+  高斯滤波，canny算子边缘检测。
+  取边缘检测结果的y方向的最大值，也就是最底边缘。
+  利用相似三角形原理，测量底边缘的距离及角度。
+                                       
+2.详细步骤
+1.   启动摄像头：输出左右图
+2.   立体矫正：输入左右图片，输出矫正左右图片
+   矫正过程如下：
+   false
+   Matlab立体矫正使用的是张氏标定法，因此标定时，目标板在图像上的位置以及与相机的距离，都会对后续测距产生影响，因此为了提高测距精度，标定板的角度和距离与后续测距使用场景需保持一致性。
+   目标检测：输入右图片，输出目标检测的坐标位置以及类别。
+   Sdk定义了RKNN_ObjDet函数，输入句柄、图像，返回目标框位置（结构体指针）和目标框数目（整型）。
+   RKNN_ObjDet(hdx, &input_data, &det_ret, &det_num);
+ 
+1.   模板测距：输入左右图，目标框位置，输出目标距离、宽度和角度
+ false
+1.   输出距离、宽度、角度、目标框坐标位置、类别
+
+三.算法记录 
+3.1实验遇到的问题
+ 3.1.1标定问题
+ Matlab使用的是张氏标定法，张氏标定得到的是 近似解，因此标定的视角与标定板的距离范围有限制。并且标定精度的影响因素有很多，现发现的因素有以下三点：
+oo   标定板使用打印的棋盘格，打印的精度不高，导致标定的精度降低
+oo   标定板的反光，会影响精度，甚至照片无法使用。
+oo   标定板的距离应当与测距范围一致。视角也应当与测距使用时保持一致
+ 3.1.2.测距算法
+    传统的测距算法可以分为稠密立体匹配与稀疏的立体匹配。稠密的立体匹配算法主要有SGBM、BM、ADCensus，稀疏的立体匹配方法主要有SIFT、SURF、ORB。模板匹配利用了块匹配的方法，通过整块像素的相似性计算，得到匹配块，进而得到视差。
+ 
+ 算法名
+                                   优缺点
+                                        
+                                     稀疏
+ SIFT
+ 速度慢，点数多，绘制形状难，精度高
+ 
+ SURF
+ 与SIFT类似，效率更高
+ 
+ ORB
+ 速度快，点数较少，以角点为主，不能保证在需要测量的物体上有ORB点。
+ 
+                                     稠密
+ BM
+ 速度快，精度低，视差图连续性差。
+ 
+ SGBM
+ 速度居中，精度好，视差图经过WLS滤波后有较好的连续性，稠密三维重建时不稳定，测距时距离不稳定。
+ 
+ ADCensus
+ 速度慢，精度高
+ 块匹配
+ 模板匹配
+ 速度快，稳定性好，精度高，实现难度低。
+ 3.1.3 镂空物体检测问题
+    镂空物体会导致雷达点语义信息附在背景上。
+ 3.1.4 风扇底座与吧台底座问题
+     雷达对于此类物体仅能探测细杆，但机器人需要对底座进行避障，因此底座需要在栅格地图上同时被建立，因此我们要求同时修改激光雷达数据完成底座的建立。
+ 2如何解决
+ 3.2.1标定问题解决方法
+    针对标定精度，主要思路是提高标定板棋盘格的精度，以及标定拍摄时图片清晰。另一种思路是对结果的处理，标定的精度最直接的影响是立体匹配，在视差转换为三维空间点时出现误差，因此直接对视差做修正或对深度拟合，即可有一个较高的精度。
+ 3.2.2测距问题
+ 在稠密匹配时，SGBM算法的视差图需要WLS滤波，能去除更多的空洞，SGBM的点云图一般，对地面和墙壁不能较好的还原，稳定性一般，在RK3566的耗时640*480的分辨率情况下大约是500ms。因此改用了模板测距与ORB测距，ORB测距的主要问题是目标物体不一定能找到较好的ORB特征点，模板测距可以解决该问题。但模板匹配针对大面积镂空物体如桌子等，效果较差，因此主要使用激光雷达测距。
+ 3.2.3 镂空物体检测问题
+ 3.2.4风扇底座与吧台底座问题
+ 现有信息为：目标框、底座尺寸（双目测距得到尺寸）、立杆位置（雷达点）。其中立杆的雷达点给出距离和角度。因此利用这些信息建立该圆弧的极坐标：
+ 
+四.实验环境及代码
+1.实验环境
+wls滤波需要opencv_Contrib，opencv版本为3.4.2.16，opencv_contrib版本为3.4.2.16。
+ 代码管理
+ 正在整理
diff --git a/雷达语义生成算法文档.docx b/雷达语义生成算法文档.docx
new file mode 100644
index 0000000..1209251
--- /dev/null
+++ b/雷达语义生成算法文档.docx
@@ -0,0 +1,102 @@
+                 雷达语义生成算法文档 --  -- 2022.6.20
+
+一.算法概述
+1 . 算法解决问题
+  该算法目的是为每束激光雷达赋予相应的语义信息，主要解决的问题是对镂空物体在正确的位置赋语义信息，减小背景因素的干扰。
+ 输入，输出，数据格式
+ 输入数据
+   camInfo：数据格式为vector<Mat>型，vector有三个元素，分别为原始图像、检测框信息和双目测距信息
+   pointcloud_cluster：数据格式为vector<vector<Mat>>，存储的为聚类后的数据
+   Laser：数据格式为自定义的FrameData，具体格式为：
+   struct FrameData
+   {
+   ros::Time timestamp;					//时间戳
+   	float angle_min;						//起始角度（单位：度）
+   	float angle_max;						//终止角度（单位：度）
+   	uint32_t len;							//每帧雷达点个数
+   	std::vector<uint16_t> distance;			//雷达点距离（单位：mm），大小为len
+   	std::vector<uint8_t> intensities;			//雷达点强度，大小为len
+   };
+   Pointcloud：数据格式为Mat，Mat大小为n*4，n为雷达点个数，前三列为雷达数据对应的三维坐标，第四列为雷达的索引（0-n-1）
+ 输出数据
+   修改输入的Laser的距离和强度（语义）信息后作为输出
+3.目前效果
+  以雷达聚类后的结果和检测框为依据对雷达赋予语义信息，可以抑制对背景的雷达点赋语义信息（特别是镂空物体）。
+二.详细描述
+1.框架图
+                                     false
+
+2.详细步骤
+  算法主要分为雷达簇与检测框的匹配、大物体赋语义信息和小物体赋语义信息三大步骤。
+2.1雷达簇与检测框的匹配
+  该步骤主要是为聚类后的每一组雷达匹配对应的检测框。
+   每一簇雷达在给定检测框中所占比例计算
+   调用函数为float LCFusion::ratio_in_1box(Mat & begin, Mat & end, Mat & box)。begin为一簇雷达最后一个雷达点对应的像素坐标，end为第一个雷达点对应的像素坐标，box为一个检测框的信息。根据输入的信息判断该簇雷达点落在检测框所占的比例并返回。
+ 将一簇雷达点和所有的检测框进行匹配
+   该过程实现为每一簇雷达匹配一个最优的检测框并赋予检测框索引标签，若无检测框匹配则赋予-1.
+   调用函数为int LCFusion::cluster_bboxs(vector<Mat> & one_cluster, Mat & bboxs)，在此函数中调用2.1.1所述函数分别计算一簇雷达点在每一个框中所占的比例，若比例大于阈值（0.8），则认为该簇雷达在检测框中并赋予该框的索引，若没有检测框则赋-1。当一簇雷达落在多个检测框中，则会为该簇雷达匹配较小的框。
+ 优化雷达簇和检测框匹配
+   该过程主要实现对镂空物体剔除背景区域的雷达簇。对于如桌子。椅子等镂空物体，会有大量的背景雷达簇落入检测框中，对每一簇落入检测框的雷达计算其相对于框内所有雷达点数量的比例，若大于阈值（0.2），则认为该簇雷达为背景，修改该簇雷达对应的检测框标签为-1。
+   
+2.2大物体赋语义信息
+  调用函数为void big_fusion(vector<int> & clusterlable, Mat & bboxs,  Mat &dwha)，其中clusterlable为每一簇雷达对应的检测框的索引（无检测框对应则为-1），bboxs为检测模型给出的检测框信息，dwha为测距信息。
+  	for (int i = 0; i < clusterlable.size(); i++)			//循环每一簇雷达
+  	{
+  		int bbox_num = clusterlable[i];				//得到该簇雷达对应的检测框														索引
+  		int bbox_cls = bboxs.at<float>(bbox_num, 5);	//得到实际的框类别
+  		if (bbox_num != -1)						//框索引9999的跳过
+  		{
+  			for (int j = 0; j < pointcloud_cluster[i].size(); j++)	//遍历一簇雷达的每																一个雷达点
+  			{
+  				int laser_index = pointcloud_cluster[i][j].at<float>(0, 4);	//映射原始																	雷达索引
+  				laser.intensities[laser_index] = class_intensity[bbox_cls+1]; //赋语义
+  			}
+  		}
+  	}
+2.3小物体赋语义信息
+  雷达扫描的平面和地面之间存在一个高度差，若地面上存在袜子、电线等物体会使雷达扫描不到导致机器人不能有效地避障，因此利用双目测距信息生成伪激光雷达解决上述问题。下图所示为小物体生成伪激光雷达及赋语义流程图。
+                                      false
+   2.3.1 标定计算相机到雷达坐标系的旋转矩阵c2lr和平移矩阵c2lt
+   2.3.2对每个框进行遍历，若为小物体的标签，则根据该框双目测距的角度、距离和宽度修改雷达帧的距离信息。设temp为二维坐标系下的物体坐标，映射到雷达坐标系下为temp= c2lr*temp+c2lt，之后将temp转为极坐标，再根据角度计算得到伪雷达点对应的索引值，代码如下：
+ 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) - laser.angle_min)/angle_inc ;		//得到角度后再转为索引angle
+ else
+   angle =  ((2 * M_PI - acos(x / dis)) -laser.angle_min)/angle_inc;
+   
+ double data_angle = atan((dwha.at<float>(i, 1))/(200 *dis));		//由宽度信息得到伪雷达角度的一半
+ data_angle /= angle_inc;
+ angle = (int)angle -(int)data_angle ;
+ int num_point = 2 * (int)data_angle + 1;					//得到伪雷达点的数量
+ int c = boxes.at<float>(i, 5);
+ 
+ for (int i = 0; i < num_point; i++)							//对雷达距离和强度进行更改
+ {
+   if ((angle+i) < 0 || (angle+i) > (laser.len-1))
+   	continue;
+   laser.distance[angle+i] = dis;
+   laser.intensities[angle+i] = class_intensity[c+1];		
+ }
+
+
+三.算法记录 
+1.实验遇到的问题
+  calibration_toolkit标定雷达和相机坐标转换后实际测试不准确。如下图所示：
+                                        
+ 如何解决
+   通过后期矫正减小标定的偏差，具体步骤如下：
+   3.1正对一面墙，发现映射在图像中的雷达点后，获取两端的像素坐标后求得旋转角度，之后转化为二维坐标系旋转矩阵r_c
+   3.2对雷达映射的高度进行拟合，在一个物体上标记雷达点的映射高度，之后将物体放在不同距离采集映射像素y轴坐标和理想的y像素坐标，采集十几组数据后利用matlab等软件进行拟合，之后换算成矩阵r_y
+   3.3经过上述转换后可能还存在微小的平移偏量（主要是x坐标），找一个有棱角的物体进行雷达点映射，采集一组映射的角点和实际的角点x坐标，得到偏移量t
+   假设一个雷达点经标定工具的参数矩阵计算得到的像素坐标为uv，加入矫正参数矩阵后的计算公式为uv = r_y*r_c*uv +t。
+   矫正后效果如下图所示：
+                                         
+
+四.实验环境及代码
+1.实验环境
+  操作系统为ubuntu18.04，ros版本为melodic，opencv版本为3.2.0
+2.代码管理
\ No newline at end of file