已有地图的直线特征检测

任务动机：检测已有地图中的直线特征，以此为参照，旋转地图至“合理”的方向。

任务描述：栅格地图开始建立的时候，地图方向由机器人的位置决定，为了能在任意位置启动，得到一个人类看图习惯的地图，需要将地图进行一个旋转。

1. 前言

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

 

2. 思路

        目标是将墙壁对齐与显示区域，所以墙壁是要检测的目标，从三个方面考虑

1. 主方向检测；检测二维图像中两个主要的方向；
2. 检测图片中的墙壁直线；
3. 检测激光数据中的直线。

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

3. 方法

3.1 PCA分解图片

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

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

       

 

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

3.2 图片直线检测

        图片的直线检测方法很多，推荐一个综述比较详细的博客，对比了霍夫直线变换、LSD、EDLines等方法。OpenCV有集成的模块，可以直接使用上述的大部分方法。

       

        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库工具，效果不佳，提前设置的聚类数会很大影响结果。 自适应的聚类方法，使用投票的方法，统计出现最多的斜率。有了上一次的操作结论，可以再进行一次简化，我们需要的是最长的直线，进一步是最长直线的斜率，所以只需要统计斜率出现频率最高值即可。 将fld检测的直线划分成多个斜率区域,在[-3.2, 3.2]中按照区间长度，比如0.2，划分多个区间，其中一个区间[0.6, 0.8]中收纳该范围内的斜率值。遍历直线斜率进行投票，得到投票最多的区间，取平均值作为结果，效果如下。

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

3.3 激光数据检测直线

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

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

        直接说明方法：给定一个基准斜率k，计算当前点与基准点斜率，如果斜率小于阈值，则认为是同一条直线，效果如下。

4. 结论

1. pca求解主方向的方法简单明了，非常的轻量，缺点是适应的场景不够广；
2. 检测直线的方法，在地图构建比较全面的时候使用比较合适，对于初始构建地图，快速检测直线有可能会出错；
3. 激光数据模式，在初始构建地图时，可以很快的检测直线判断方向；缺点是信息太局部化，容易被局部符合条件直线迷惑。

5. 附图—fld直线检测的三种阈值结果

• length 10

• length 20

  

• length 30

  

        nav_msgs::OccupancyGrid 旋转方法。栅格地图的数据是以info为原点，使用data中的数据构建的，所以改变地图位置和方向可以通过改变基准点实现。 在改变基准点时，如果直接修改position or orientation会，出现这样的情况

        这是因为基准是绘图的左下角基准，正确的方式应该是以地图frame为基准进行旋转。 具体的方式是，先使用info构建一个位姿，然后使用需要旋转的角度，构建一个新的位移为0，旋转为对应角度的角轴或四元数，然后使用新的位姿左×原地图位姿，形成新的地图位姿。