目前，基于特征点提取与匹配的SLAM的方法研究日趋成熟。Davison等[4-5]实现了单目SLAM原型系统，为单目SLAM后续发展奠定了基础。Klein和Murray[6]提出了并行跟踪和地图构建 (Parallel Tracking and Mapping，PTAM) 方法，把跟踪和地图构建分为2个线程进行并行处理，用捆绑调整[7]替代了滤波方法，提高了系统的实时性。Tan等[8]通过在线选取关键帧提出了基于先验自适应随机采样一致 (prior-based adaptive ramdom sample consensus) 算法来提高相机的位姿跟踪准确性，实现了动态环境下基于特征点的单目SLAM。上述方法主要依靠单目图像帧间特征点匹配实现三维信息发现，由于图像特征点容易受到运动模糊及光照变化等噪声影响，基于图像特征点三维信息解算优化搜索计算负荷较重且容易出现不稳定现象[9]。
无人机及机器人操作的环境中往往蕴含着丰富的直线特征。由于直线的位置和方向信息不容易受到点噪声的影响，越来越多的研究者尝试用直线特征来优化SLAM制图和跟踪过程。Smith等[10]利用扩展卡尔曼滤波的直线提取方法实现单目SLAM，通过随机找出一个点作为一条直线段的起点，根据直线上像素点的曲率用扩展卡尔曼估算直线终点，进而得到完整直线段，由于卡尔曼滤波运算次数与像素点个数平方成正比，系统运算量大。Zhou等[11]提出了基于建筑结构直线特征单目SLAM方法，建筑结构直线实时约束了摄像头的位姿，消除了累积方位误差，在结构规范的室内环境下，三维信息发现效果较好，在建筑线条不明显的情况下，系统性能下降明显。Zhao等[12]减少直线描述参数，提高三维信息解算效率，由于仅使用了单一直线特征，空间环境信息描述不够完整，其直线匹配计算需要先验知识并通过手动输入参数完成。Jeong和Lee[13]提出了直线特征和角点特征相结合的单目SLAM制图方法，但未提供在实际环境中的测试结果，亦没有给出比较分析。Gee等[14]为单目SLAM系统引入了直线与平面特征，在小范围场景中采用随机抽样一致 (RANSAC) 算法提取平面，利用高层次几何特征提高了空间结构的描述效率，使用的RANSAC算法在有噪情况下生成的直线和平面空间误差较大，影响系统的鲁棒性。
引入蕴含高层次几何特征的直线和平面，可以有效降低单目SLAM地图构建过程中三维空间表达的信息冗余[15]。为实现高效稳定的单目SLAM三维信息解算，本文提出了一种改进的J-Linkage算法，通过引入直线特征，并结合图像点特征，实现鲁棒性较高的三维空间平面发现，从而建立单目SLAM地图构建过程中的点线面结合的算法框架。
单目SLAM点特征的提取详见参考文献[5]，这里不再赘述。本文主要内容包括单目SLAM制图过程中直线提取方法和直线增强的三维特征平面点线结合聚类发现方法，通过实验分析验证了上述方法的有效性。
1 单目SLAM制图直线提取 为了实现单目SLAM制图过程中帧间线特征匹配, 本文采用分步滤除端点对齐的直线匹配搜索方法。文献[9]在分步滤除的基础上使用捆绑调整 (Bundle Adjustment，BA) 方法，优化搜索过程速度较慢。为提高解算效率，基于单目SLAM帧间图像纹理局部几何特征变化相对较少的特点，利用对极几何解算方法[16]，以源关键帧候选直线中点为基准，将其投影到目标关键帧并得到相应对极线。在对极线周围形成矩形搜索区域，利用canny算子提取搜索区域中的直线边缘信息。通过对可能匹配的直线段进行分步滤除处理，滤除不符合匹配直线要求的边缘，得到最终的匹配直线。设变量Dt为搜索区域中边缘到对极线的距离，θt为搜索区域中直线与源直线所成的夹角，Ct为搜索区域中边缘曲率，Lt为搜索区域中直线长度。具体步骤如下：
1) 滤除与对极线的距离d≥Dt的直线。
2) 滤除与源直线纹理特征[9]不符的直线。
3) 滤除与源直线所成的夹角θ≥θt的直线。
4) 滤除边缘曲率c≥Ct与长度l≤Lt的直线。
在单目SLAM系统前后两帧关键帧中，通过分步滤除端点对齐的直线匹配搜索，为源关键帧的候选直线在目标关键帧中找到对应匹配直线，如图 1所示。
 |
| 图 1 帧间直线匹配 Fig. 1 Inter-frame matching of line segments |
| 图选项 |
由直线段对应端点的对极几何恢复直线的三维信息[16]，如图 2所示。
 |
| 图 2 对极几何三维直线生成 Fig. 2 Three-dimensional line generation based on epipolar geometry |
| 图选项 |
源关键帧s和目标关键帧t对应相机中心分别为c1和c2，ps1ps2为源关键帧线段，pt1pt2为目标关键帧对应的线段，pG1pG2为通过对极几何恢复出来的三维空间线段。完成匹配后，利用对极几何即可解算出对应的空间线段pG1pG2。由于相机移动可能存在旋转分量，导致二维直线段的匹配产生误差，影响到空间直线段深度信息生成精度。因此，需要利用平面信息对其进行校验，进一步提升几何建模的效率和稳定性。
2 单目SLAM三维平面发现 Toldo和Fusiello[17]提出J-Linkage算法，在单一参数空间中实现多模型聚类。J-Linkage算法能够在初始样本空间噪声较大且不需要手动输入聚类模型个数的情况下，实现多模型空间聚类。但算法的复杂度较高，不利于地图数据量较大的单目SLAM系统多模型聚类。为了有效实现单目SLAM地图构建过程中的三维平面发现，本文提出了基于直线匹配增强的J-Linkage平面聚类方法。利用直线对平面的强约束特性，通过地图中的点特征生成最小采样集 (Minimal Sample Sets，MSS)，用S{s1, s2, …, sm}表示，共包含M个模型，利用直线匹配增强进行平面发现，可以有效降低S中的样本个数。为了提高聚类有效性，对原始样本点集进行了空间低通滤波。
对样本点采样生成最小采样集S，如样本空间中某一点xi已经被选取，那么点xj被选取的概率为
 | (1) |
式中：Z为归一化常数；σ为经验值[18-20]。
假如一个样本空间维数为N，其中内点个数为Ni，那么生成一个基数为Q的最小采样集S，内点抽取概率为
 | (2) |
式中：Ei为在第i(i=1, 2，…，Q) 次抽取一个内点的概率事件。
均匀采样情况下，第i次抽到内点的概率为
 | (3) |
在实际应用中，首次抽取到内点的概率模型通常认为是均匀采样。
 | (4) |
但随后第i次抽取内点，则是以式 (1) 结合条件概率模型式 (2) 进行抽取，所以第i次抽取内点的概率为
 | (5) |
式中：α为内点间的平均欧氏距离；w为外点与内点的欧氏距离。
令δ=Ni/N，δ为一个给定模型的内点的比例，如果Ni?Q，则有
 | (6) |
如w?α情况下，样本集S抽取外点的概率相应增大。按照上述概率模型，结合空间低通滤波后的地图样本点集PG{pG1, pG2, …, pGn}，生成最小采样集S，具体步骤如下:
1) 从PG{pG1, pG2, …, pGn}中以式 (1) 为模型抽取3个不同的样本点 (pGi1, pGi2, pGi3)。
2) 判断3点是否共线，如果共线则返回到步骤1) 继续生成3个不同的样本点；否则，进入步骤3)。
3) 由 (pGi1, pGi2, pGi3) 得到一个平面标准方程：Ax+By+Cz+D=0。
4) 把对应平面标准方程的4个参数 (A, B, C, D) 加入到S。
5) 重复上述步骤直到完成最小采样集S的生成。
通过直线对平面的几何约束，利用直线特征几何特征对生成的最小采样集S进行滤除，降低S维数，加快聚类速度。如第i条直线方向向量为nli，线段起点为pG1i(x1, y1, z1)，终点为pG2i(x2, y2, z2)，S中第sj个法向量为npj，sj(Aj, Bj, Cj, Dj)，线段两点同时满足到sj距离dij < η，及直线方向向量与平面的法向量的夹角θ < φ。
 | (7) |
式中：η和φ为常数。
Vi为样本点pGi在最小采样集S上的倾向向量, Vi=[v1i, v2i, …, vMi], dij为pGi到sj的距离，计算公式为
 | (8) |
则
 | (9) |
式中：ε为常数。
样本点集
 |
进一步，聚类使用jaccard距离[17]作为判断2个向量相似程度的依据。设有倾向向量Vk和Vl, 则这2个倾向向量的jaccard距离为
 | (10) |
式中：
Vk与Vl与运算结果向量
 | (11) |
 | (12) |
在TN×M中，找出2个相似度最高的行向量合并为一个向量，直到所有行向量出现互斥，则聚类结束。
直线增强的J-Linkage算法步骤如下：
输入：
过程：
1:由式 (7)~式 (9) 生成直线增强的
2: for j={1, 2, …, N}:do
3: ?for i={1, 2, …, M}:do
4: ??计算pGj点到si的距离dji
5: ??根据dji结合式 (9) 得到每个点的倾向向量Vi
6: ??生成模型空间的倾向矩阵TN×M
7: ?end for
8: end for
9: repeat:
10: ??计算Vi、Vj的jaccard距离满足dijJ最小
11: if dijJ < 1:
12: ??删除Vi、Vj依据式 (11) 并添加Vi∪Vj
13: continue
14: end if
15: until dijJ≥1聚类结束
3 实验结果 本文实验场景为人工布置的实验台和室内设备堆积场景。系统硬件配置为Core i7 3.4 GHz四核CPU, 软件环境为Windows7操作系统，开发环境为Microsoft Visual Studio 2010，编程语言为C/C++。
为了体现直线特征在单目SLAM制图过程的作用，首先使用传统单目SLAM制图方法进行三维点特征提取，然后使用点线结合的单目SLAM制图方法对相同场景进行了三维信息提取，如图 3所示。图 3(a)为包含二维图像纹理信息的特征点提取结果，图中的特征点用黄色标出。图 3(b)为在三维空间中呈现的特征点相对位置关系，图中可观测到的红绿白三色直线构造的空间直角坐标系为单目SLAM传感器相邻2次观测对应相机不同位置，以及三维空间网格平面红绿黄三色直角坐标系。图 3(c)为包含二维图像纹理信息的特征点和特征线提取结果，直线用蓝色表示。图 3(d)为在三维空间中呈现的特征点与特征线相对位置关系。可以看出，更为直观的直线特征所蕴含的空间几何信息对单目SLAM三维地图绘制具有重要价值。
 |
| 图 3 点线特征对比示意图 Fig. 3 Schematic diagram of comparison between feature points and feature lines |
| 图选项 |
为了评估参数η、φ对直线增强的单目SLAM三维制图的作用，对这组变量不同取值状况下聚类解算效果进行统计分析。首先选定一个固定的φ，然后对η取不同值得到最小采样集样本内点概率曲线，以及最小采样集样本空间大小曲线；同理，固定η、φ取值不同而得到最小采样集样本内点概率及最小采样集样本空间大小曲线，如图 4所示。图中：P为内点概率；S为最小采样集维数。
 |
| 图 4 参数η和φ影响S生成 Fig. 4 Influences of parameters to S caused by η and φ |
| 图选项 |
由图 4曲线变化关系，η、φ取值越大，最小采样集S样本空间维数越大，S在样本空间中内点抽取概率相应减小。S样本数量过少对应内点提取概率高，但可能剔除了有效sm，造成聚类失败。相反，S样本数量过大，运算复杂度高。选取参数如下：
 | (13) |
进一步进行了基于点特征单目SLAM的J-Linkage三维平面发现和直线增强的J-Linkage三维平面发现对比实验，如图 5所示。
 |
| 图 5 J-Linkage三维平面发现 Fig. 5 J-Linkage three-dimensional planes discovery |
| 图选项 |
不同平面对应于不同的颜色表示，图 5(a)为基于点特征单目SLAM的J-Linkage三维平面发现结果，图 5(b)为对应的二维平面示意图，图 5(c)为直线增强的单目SLAM的J-Linkage三维平面发现结果，图 5(d)为对应的二维平面示意图。由于单目SLAM点云的稀疏特性，外点影响显著，直线增强的聚类效果优于单纯依赖点特征聚类方法。
为了进一步验证直线匹配增强的聚类算法在单目SLAM制图过程中的平面发现效果，在实验室内进行了不同场景下的多平面发现实验。图 6(a)为人工布置的实验台场景多平面发现, 图 6(b)为非实验台设备堆积场景多平面发现。
 |
| 图 6 直线增强的J-Linkage三维平面发现 Fig. 6 Line enhanced J-Linkage three-dimensional planes discovery |
| 图选项 |
图 6(a)左侧为直线增强的J-Linkage平面发现三维空间示意图，聚类的三维平面数量依次为1、2、3个。图 6(a)右侧为对应二维平面示意图。图 6(b)同图 6(a)。由不同场景实验结果可见，结合了点线面几何信息的单目SLAM三维空间地图绘制具有较好的客观描述能力，在三维空间信息表达中可以有效降低冗余信息，为单目SLAM的进一步研究与应用打下良好基础。
4 结论 本文提出一种适用于单目SLAM的直线匹配增强三维平面发现方法。
1) 基于单目SLAM系统使用快速直线搜索匹配算法，有效实现二维直线提取与匹配。通过将直线特征引入单目SLAM三维地图构建过程，提高了单目SLAM制图稳定性。
2) 提出直线增强J-Linkage算法，在单目SLAM制图过程中生成多平面特征，点线面结合的地图构建方法，提高了地图描述效率。
从实验结果可知，本文提出的适用于单目SLAM的直线匹配增强三维平面发现方法相对于传统仅基于点特征单目SLAM制图方法，提高了制图稳定性和地图描述效率，适用于室内环境单目SLAM地图构建，但缺陷在于：适用于直线与点特征较丰富的环境，在纹理信息较弱的场景中效果欠佳。后续研究工作可以把直线和平面特征引入到单目SLAM跟踪部分，实现完整的点线面特征相结合单目SLAM系统应用。
参考文献
| [1] | SMITH R, SELF M, CHEESEMAN P.Estimating uncertain spatial relationships in robotics[M]//COX I J, WILFONG G T.Autonomous robot vehicles.New York:Springer, 1990:167-193. |
| [2] | WOODFILL J, ZABIH R.A real-time vision system for robots in unstructured domains[C]//Sensor fusion IV:Control Paradigms and Data Structures.Bellingham, WA:Society of Photo-Optical Instrumentation Engineers, 1992, 1611:346-355. |
| [3] | CHONG T J, TANG X J, LENG C H, et al. Sensor technologies and simultaneous localization and mapping (SLAM)[J].Procedia Computer Science, 2015(76): 174–179. |
| [4] | DAVISON A J.Real-time simultaneous localisation and mapping with a single camera[C]//9th IEEE International Conference on Computer Vision, 2003.Piscataway, NJ:IEEE Press, 2003:1403-1410. |
| [5] | DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM:Real-time single camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052–1067.DOI:10.1109/TPAMI.2007.1049 |
| [6] | KLEIN G, MURRAY D.Parallel tracking and mapping for small AR workspaces[C]//6th IEEE and ACM International Symposium on Mixed and Augmented Reality, 2007.Piscataway, NJ:IEEE Press, 2007:225-234. |
| [7] | TRIGGS B, MCLAUCHLAN P F, HARTLEY R I, et al.Bundle adjustment:A modern synthesis[M]//TRIGGS B, ZISSERMAN A, SZELISKI R.Vision algorithms:Theory and practice.Berlin:Springer, 1999:298-372. |
| [8] | TAN W, LIU H, DONG Z, et al.Robust monocular SLAM in dynamic environments[C]//2013 IEEE International Symposium on Mixed and Augmented Reality (ISMAR).Piscataway, NJ:IEEE Press, 2013:209-218. |
| [9] | KLEIN G, MURRAY D.Improving the agility of key frame-based SLAM[M]//FORSYTH D, TORR P, ZISSERMAN A.Computer Vision-ECCV 2008.Berlin:Springer, 2008:802-815. |
| [10] | SMITH P, REID I D, DAVISON A J.Real-time monocular SLAM with straight lines[C]//Proceedings of the British Machine Vision Conference 2006.London:British Machine Vision Conference, 2006, 6:17-26. |
| [11] | ZHOU H, ZOU D, PEI L, et al. StructSLAM:Visual SLAM with building structure lines[J].IEEE Transactions on Vehicular Technology, 2015, 64(4): 1364–1375.DOI:10.1109/TVT.2015.2388780 |
| [12] | ZHAO L, HUANG S, YAN L, et al. A new feature parametrization for monocular SLAM using line features[J].Robotica, 2015, 33(3): 513–536.DOI:10.1017/S026357471400040X |
| [13] | JEONG W Y, LEE K M.Visual SLAM with line and corner features[C]//2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway, NJ:IEEE Press, 2006:2570-2575. |
| [14] | GEE A P, CHEKHLOV D, CALWAY A, et al. Discovering higher level structure in visual SLAM[J].IEEE Transactions on Robotics, 2008, 24(5): 980–990.DOI:10.1109/TRO.2008.2004641 |
| [15] | 缪君, 储珺, 张桂梅, 等. 基于稀疏点云的多平面场景稠密重建[J].自动化学报, 2015, 41(4): 813–822. MIAO J, CHU J, ZHANG G M, et al. Dense multi-planar scene reconstruction from sparse point cloud[J].Automatica Sinica, 2015, 41(4): 813–822.(in Chinese) |
| [16] | HARTLEY R, ZISSERMAN A. Multiple view geometry in computer vision[M].Cambridge: Cambridge University Press, 2003: 344-352. |
| [17] | TOLDO R, FUSIELLO A.Robust multiple structures estimation with j-linkage[M]//FORSYTH D, TORR P, ZISSERMAN A.Computer Vision-ECCV 2008.Berlin:Springer, 2008:537-547. |
| [18] | KANAZAWA Y, KAWAKAMI H.Detection of planar regions with uncalibrated stereo using distributions of feature points[C]//Proceedings of the British Machine Vision, Kingston, 2004:1-10. |
| [19] | ZULIANI M, KENNEY C S, MANJUNATH B S.The multiransac algorithm and its application to detect planar homographies[C]//IEEE International Conference on Image Processing, 2005.Piscataway, NJ:IEEE Press, 2005, 3:153-159. |
| [20] | ZHANG W, KǒSECKà J.Nonparametric estimation of multiple structures with outliers[M]//VIDAL R, HEYDEN A, MA Y.Dynamical vision.Berlin:Springer, 2007:60-74. |
