要做回航充电，又不想加其他的传感器，只能靠lidar来识别充电桩的形状定位。
显然，充电桩表面是平整的直线，那么就要从那么多点中提取直线，然后再识别哪一条直线是充电桩。
提取直线就成为了最初的一步。
在github上找了半天找到一个像样的
https://github.com/kam3k/laser_line_extraction
试了一把，提取效果还不错。
记录一下并把配置说明一下：
topics
订阅的topic显然是 scan，发布了两个topic，我们要用的是/line_segments，/line_markers是用来在rviz上显示的，可以在launch中关闭。
配置
配置列表如下，基本上都好理解，被我分成了三大类，便于理解。
frame_id (default: "laser") # The frame in which the line segments are published.
scan_topic (default: "scan") # The LaserScan topic.
publish_markers (default: false) # Whether or not markers are published.
-----------------------------------------------------------------------------------------------------
bearing_std_dev (default: 0.001) # The standard deviation of bearing uncertainty in the laser scans (rad).
least_sq_angle_thresh (default: 0.0001) # Change in angle (rad) threshold to stop iterating least squares (least_sq_radius_thresh must also be met).
least_sq_radius_thresh (default: 0.0001) # Change in radius (m) threshold to stop iterating least squares (least_sq_angle_thresh must also be met).
range_std_dev (default: 0.02) # The standard deviation of range uncertainty in the laser scans (m).
---------------------------------------------------------------------------------------------------------
max_line_gap (default: 0.4) # The maximum distance between two points in the same line (m).
min_line_length (default: 0.5) # Lines shorter than this are not published (m).
min_line_points (default: 9) # Lines with fewer points than this are not published.
min_range (default: 0.4) # Points closer than this are ignored (m).
min_split_dist (default: 0.05) # When performing "split" step of split and merge, a split between two points results when the two points are at least this far apart (m).
# 线段split的阈值，过大时很多线段被合并成一条，过小时，出现很多碎短的线段
outlier_dist (default: 0.05) # Points who are at least this distance from all their neighbours are considered outliers (m).
消息
最重要的是我们拿到 /line_segments的消息之后如何分析
消息类型是LineSegmentsList，如下，这不是主要的。
每个segment的类型LineSegment才是最主要的：
这其中的start, end都好理解：
start 和end分别是这个线段的起点坐标和终点坐标
radius和angle就不好理解了，于是我们echo了它的消息，如下
radius: 0.752869129181
angle: -0.797926008701
covariance: [1.8016297644862789e-06, 1.2701141258730786e-06, 1.2701141258730786e-06, 9.006992058857577e-07]
start: [-0.6120783090591431, -1.648557186126709]
end: [-0.35934978723526, -1.402082920074463]
-
radius: 1.1387783289
angle: -0.808640360832
covariance: [2.9355262540775584e-06, 2.777204372250708e-06, 2.777204372250708e-06, 2.649457655934384e-06]
start: [-0.08191638439893723, -1.6525081396102905]
end: [0.14225095510482788, -1.4385261535644531]
-
radius: 1.12730681896
angle: -0.787253201008
covariance: [1.4751535672985483e-06, -2.180588808187167e-06, -2.180588808187167e-06, 3.510540864226641e-06]
start: [1.011875033378601, -0.5831754803657532]
end: [1.459415078163147, -0.1372928023338318]
-
radius: 3.53375911713
angle: 0.791161239147
covariance: [3.9494702832598705e-06, -7.934396307973657e-06, -7.934396307973657e-06, 2.223481897090096e-05]
start: [2.499594211578369, 2.497988224029541]
end: [1.9661375284194946, 3.025331497192383]
我的地图总共拟合出4条直线，相应的结果如下，中间的frame是laser的frame，红色为x轴+，
按照ros中的坐标系来说，机器人正前方为0度，逆时针为正，激光数据的排列是从-135~135度（我的激光扫描范围是270度），那么上面4个线段从左往右依次是1,2,3,4。
这样的话我们就能理解 radius和angle的意思了。官方给的解释是极坐标下参数，我们能够理解到的是
angle： 原点到直线的垂线的角度，角度以机器人为坐标系计算，范围是-PI~PI
radius： 原点到直线的距离
根据这两个参数，完全不能确定什么，这两个参数完全能够根据start和end点计算出来。不理解这两个参数存在的意义。
- covariance是2X2的矩阵，是radius和angle的协方差