从激光数据点中提取直线

要做回航充电，又不想加其他的传感器，只能靠lidar来识别充电桩的形状定位。

显然，充电桩表面是平整的直线，那么就要从那么多点中提取直线，然后再识别哪一条直线是充电桩。

提取直线就成为了最初的一步。

在github上找了半天找到一个像样的

https://github.com/kam3k/laser_line_extraction

试了一把，提取效果还不错。

记录一下并把配置说明一下：

topics

image.png

订阅的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，如下，这不是主要的。

image.png

每个segment的类型LineSegment才是最主要的：

image.png

这其中的start, end都好理解：

1. start 和end分别是这个线段的起点坐标和终点坐标

3. 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轴+，

image.png

按照ros中的坐标系来说，机器人正前方为0度，逆时针为正，激光数据的排列是从-135~135度（我的激光扫描范围是270度），那么上面4个线段从左往右依次是1,2,3,4。

这样的话我们就能理解 radius和angle的意思了。官方给的解释是极坐标下参数，我们能够理解到的是

angle： 原点到直线的垂线的角度，角度以机器人为坐标系计算，范围是-PI~PI
radius： 原点到直线的距离

根据这两个参数，完全不能确定什么，这两个参数完全能够根据start和end点计算出来。不理解这两个参数存在的意义。

4. covariance是2X2的矩阵，是radius和angle的协方差