图片直线提取的一些尝试

任务动机 检测已有地图中的直线特征，以此为参照，旋转地图至“合理”的方向
任务描述 栅格地图开始建立的时候，地图方向由机器人的位置决定，为了能在任意位置启动，得到一个人类看图习惯的地图，需要将地图进行一个旋转。

1. 前言

在使用激光构建地图时，形成的地图在rviz中一般是这样的

这跟初始的建图位姿有关，如果手动的调整建图起始位置的朝向，将激光雷达正对墙壁或者其他参考，建图后效果，会是墙壁对齐于视图，当然在rviz中可以直接用鼠标中键旋转，如果想让地图在建图中自己调整至垂直的方向，就需要做一点工作。

2.思路

目标是将墙壁对齐与显示区域，所以墙壁是要检测的目标，从两个方面考虑
a) 主方向检测；检测二维图像中两个主要的方向；
b) 检测图片中的墙壁直线；
c) 检测激光数据中的直线。

---

a)简单明了，直接使用PCA方法，SVD分解后的U矩阵向量，即为主向量；
b)方法需要识别直线，然后找到一堆直线中的所需直线；
c)需要定义激光点中什么样的数据点是直线。

3. 方法

3.1 PCA分解图片

PCA分解方法是图像降维的一种方法，在二维点的特殊情况下，可以这样理解

1. 去中心化：将图像中心的影响排除，避免中心点对主方向的影响；或者说将所有的点平移到坐标原点附近；
2. 特殊技巧，只求所需的矩阵。
3. 得到需要的主向量，及U中的列向量。

2的说明

$$\begin{gathered} X=U_r\Sigma V^T \\ {X}^T=V{\Sigma }^T{U}_r^T \\ H=X{X}^T=U_r\Sigma {\Sigma }^T{U}_r^T \end{gathered}$$

结论
PCA方法在地图尚未形成闭环时比较有效，一旦形成封闭的区域，对于主方向的检测，会失效；
原因在于PCA检测的是方差最大和最小的方向，当形成一个封闭区域后，方差最大的方向已经无法正确识别。

3.2 图片直线检测

图片的直线检测方法很多，推荐一个综述比较详细的博客，对比了霍夫直线变换、LSD、EDLines等方法。

OpenCV有集成的模块，可以直接使用上述的大部分方法。

Hough&HoughP
LSD&FLD

OpenCV中有对几种方法做一个比较，也有霍夫变换的原理Hough Tutorial。

对比几种方法后，选择了FLD的方法，同样的条件处理图片FLD速度大概比LSD快了4~5倍。

有了直线后，开始检测所需要的直线。

直接给出要求：最长的直线

上一张检测直线的图片

这里fld的阈值使用默认阈值，可以调节

// length_threshold 最短直线长度
// do_merge 是否合并短直线
Ptr<FastLineDetector> fld = createFastLineDetector(length_threshold,
          distance_threshold, canny_th1, canny_th2, canny_aperture_size,
          do_merge);

下面的参数，改变直线的数量和长度。
结论
最长的直线并不是想要的直线，反而是图像上部的激光边界线。

使用kmeans聚类直线

kmeans也可以使用OpenCV库工具，效果不佳，提前设置的聚类数会很大影响结果。
[todo]自适应的聚类方法

使用投票的方法，统计出现最多的斜率

有了上一次的操作结论，可以再进行一次简化，我们需要的是最长的直线，进一步是最长直线的斜率，所以只需要统计斜率出现频率最高值即可。
将fld检测的直线划分成多个斜率区域,在[-3.2, 3.2]中按照区间长度，比如0.2，划分多个区间，其中一个区间[0.6, 0.8]中收纳该范围内的斜率值。遍历直线斜率进行投票，得到投票最多的区间，取平均值作为结果。

效果

在图建的比较完整时，正确率很高(没统计具体值)。

3.3 激光数据检测直线

首先要将sensor_msgs/LaserScan数据转换成对应的三维点，这里我直接使用了建图处理后的点云数据

Vector<Eigen::Vector3f> position;
float time;

直接说明方法

给定一个基准斜率k，计算当前点与基准点斜率，如果斜率小于阈值，则认为是同一条直线。

效果