基于反光柱特征的激光定位算法思路

1. 识别反光柱

反光柱是特殊材料制成，根据激光雷达对反光材料扫描得到的反射值来提取特征。反光柱是圆柱形，我们提取的特征就是反光柱圆心在当前帧下对应激光扫描线束的dist以及angle。提取方法如下：

大体思路是，根据激光雷达扫描到反光材料的反射值来确定扫到反光柱开始和结束的两条线束，计算两者之间的夹角，根据三角关系就能算出反光柱圆心到激光雷达的dist和angle。

2. 数据关联

2.1 基于几何形状寻找匹配

这里用到了一个寻找匹配的技巧。
我们在构建右边完整的多边形图的时候将边长的单位由米转换成了像素。比如我们设定右边多边形图的一个像素代表0.1米，则lidar扫描到的点之间的距离如果是3.132m，在右边图中就是31个像素（int）3.132/0.1。这个像素值作为多边形图中map数据结构的key，在根据真实的边长寻找匹配时，可以快速定位要寻找边的范围，比如3.132m的边在key为[31,32]范围内寻找匹配。
上一步可以认为是粗匹配，接着根据激光雷达的观测精度设置一个精匹配阈值来寻找最接近的边长。（这里反光柱布置较分散时也没有啥影响。）

2.2 暴力寻找匹配

3. 位姿估计（最小二乘求解）

定位结果展示：

运动轨迹展示：

4. 问题

4.1 精度问题

反光柱识别算法精度较差，静止agv，将识别到的反光柱坐标可视化，同一个反光柱，位置应该重合在一处。

agv静止不动识别反光柱的重复精度在厘米级，这个提取精度会传导到定位上去。从轨迹放大来看在厘米级，可接受。

4.2 快速旋转时定位较差

直线行驶较好：