技术领域
[0001] 本
发明涉及视觉同时建图与定位技术领域，特别是一种基于特征的
双目视觉SLAM(同时定位与建图)技术领域。
背景技术
[0002] 针对视觉同时建模与定位技术，基于关键
帧的优化和图优
化成为视觉SLAM问题的主流
框架。图优化技术已经被证实在计算所消耗的资源和结果的一致性方面比传统的滤波框架具有更好的性能。点特征是在视觉同时建图与定位技术中最被广泛使用的特征，在室内和室外环境中都特别丰富，在连续的
图像序列中容易被
跟踪，而且在几何变换中方便计算。然而，点特征对于环境依赖较大，高
质量的点特征需要鲁棒性高但耗时的特征检测与描述。在图像中线特征比点特征的表示层次高，在结构化环境中提供了更鲁棒的信息，利用较少的线特征结合点特征建立环境地图与定位可以更加高效以及精确。
发明内容
[0003] 本发明所要解决的技术问题是提供一种基于点线综合特征的视觉SLAM方法，可以用于在室内室外环境的
机器人定位与
姿态估计，由于综合使用点线特征使得系统更加鲁棒，更加精确。本方法可用于室内室外的场景地图的构建，构建出的地图综合了特征点和特征直线，能够提供更加丰富的场景信息。为此，本发明提供以下技术方案：
[0004] 一种基于点线综合特征的视觉同时建图与定位方法，其特征在于，包括离线建立视觉词典和在线建立稀疏视觉特征地图两个部分：
[0005] 首先，利用聚类方法离线建立树状的视觉词典，即描述子空间的KD树，并确定树状视觉词典中每个
节点的逆文本
频率，所述每个节点即为描述子的聚类中心：
[0006] 将每帧图像包含的特征转化为视觉词汇，即特征描述子；对所述视觉词汇进行分层聚类，建立描述子空间的KD树，该KD树称为视觉词典；使用特征描述子离线建立树状的视觉词典，将特征描述子的训练图像集中提取出来；所述描述子是ORB(Oriented FAST and Rotated BRIEF(BRIEF为Binary Robust Independent Elementary Features二进制鲁棒独立基本特征描述子)，定向快速
角点检测和二进制鲁棒独立基本特征描述子点特征描述子和LBD直线特征描述子；所述ORB点特征描述子和LBD(Line Band Descriptor线带描述子)直线特征描述子均为二进制描述子，把两种二进制描述子分别进行拓展：为ORB点特征添加标志位0，为LBD线特征添加标志位1，所述标志位0和标志位1可区分直线特征与点特征；获取LBD直线特征描述子之前首先要用LSD(Line Segment Detector线段检测子)检测直线，再用LBD描述子对该直线进行描述；
[0007] 视觉词典中每个节点的权重由该节点包含的所有特征描述子的逆文本频率(即IDF，主要思想是：如果包含视觉词汇t的图片越少，则逆文本频率越大，则说明词汇t具有很好的类别区分能
力)确定；
[0008] 然后，在线建立稀疏的视觉特征地图，步骤如下：
[0009] 步骤一，从双目相机中获取校正后的图像，并对校正后的图像的特征提取及描述：
[0010] 提取校正后的图像中的点线特征及其描述子，在线提取ORB点特征描述子和LBD线特征描述子；
[0011] 步骤二，双目相机中校正后的图像进行特征匹配及
三维重建：
[0012] 匹配校正后的图像中的特征点和特征直线，建立匹
配对，利用双目视觉成像模型把特征点和特征直线进行三维重建，在重建中用普吕克坐标来表示特征直线，并维护直线的端点，用特征点和特征直线建立综合点线特征的稀疏特征地图，采用普吕克坐标用于直线的表示和计算
[0013] 步骤三，前后帧图像匹配、局部地图匹配及相机的
运动估计：
[0014] 在重建出三维空间中的特征点和特征直线后，对这些点和线进行跟踪匹配，匹配包括两部分：前后图像匹配以及局部地图匹配，所述前后帧图像匹配用于估计当前时刻相机的
位姿，
[0015] 求解位姿的过程如下，假设当前时刻左相机
坐标系Oc在世界坐标系Ow中旋转和平移分别为Rwc和twc，重构的特征点j在世界坐标系Ow的坐标为Pjw，则该特征点在当前时刻左相机坐标系Oc下的坐标Pjc为：
[0016] Pjc＝RcwPjw+tcw
[0017] 重构的特征直线i在世界坐标系Ow的坐标为Liw，Liw＝[nT,vT]T，则该特征直线在当前时刻左相机坐标系Oc下的坐标为：
[0018]
[0019] 其中Rcw＝RwcT，tcw＝-Rwctwc，分别为世界坐标系在左相机坐标系中的旋转和平移，[tcw]×是由向量tcw构成的3×3的反对称矩阵；将特征点Pjc通过针孔相机模型投影到当前左相机中，得到其投影的图像坐标 将特征直线Lic投影到当前左相机中，得到其投影直线方程为li；分别定义点和线特征的误差，点的误差为重投影误差，即特征点投影坐标 与观测坐标pj之间的距离epj；线的误差为观测线段的两个端点ep1i、ep2i到投影直线方程的几何距离eli；运动估计的目标是求解以下非线性最小二乘问题：
[0020]
[0021] 即求解当前相机的姿态使得特征点和特征直线的重投影误差最小；其中α、β为点特征重投影误差和线特征重投影误差的权重值，α、β是两个常数为了剔除错误图像特征匹配的影响，在优化过程中可采用Ransac(Random Sample Consensus，随机
采样一致性)方法得到运动估计的解；
[0022] 步骤四，利用步骤一中得到的视觉词典做回环检测：
[0023] 对视觉关键帧提取得到的点、线特征描述子，建立图像
数据库，所述图像数据库包含每一关键帧中的点、线特征描述子，根据建立的视觉词典，把图像的特征转换成词包向量，其中词包向量包含图像中每个视觉词汇的TF-IDF(TF表示词条在一帧图片中出现的频率，IDF为上文所述的逆文本频率，TF-IDF表示TF与IDF的乘积)分数，如果一个视觉词汇在同一帧图像中出现的频率越高TF-IDF分数就越高，但是在整个图像数据库中出现频率越高TF-IDF分数会越低；
[0024] 评价两幅图像的相似性时，根据提取到的特征把图像转换成词包向量，然后根据词包向量计算相似度得分，对当前相机采集的图像与图像数据库中的图像进行比较，得分较高为同一个
位置采集得到的图像，即为一个闭环，表示之前已经
访问过这个位置，再利用几何一致性，即两帧图像中有足够多的匹配对支持欧氏变换，时间一致性，即两帧图像前后的若干图像序列也应该很相似，进一步判断是否为闭环；
[0025] 步骤五，把步骤二到步骤四得到的点线特征、相机运动估计、闭环检测等放入基于关键帧的图优化框架中，在后端的优化中采用直线的
正交表示来最小化直线的参数个数。在图优化框架中优化相机的位姿和特征点和线的位姿，实现相机的定位与在线稀疏视觉特征地图的构建。
[0026] 在上述技术方案的
基础上，本发明还可以采用一下进一步的技术方案：
[0027] 提取图像中的点线特征时，特征点的检测选用FAST角点检测，用ORB描述子进行描述，直线特征的检测采用LSD
算法提取直线特征并用LBD描述子表示直线。
[0028] 对于直线特征的表示，采用普吕克坐标用于直线的计算，包括几何变换，三维重建，在后端的优化中采用直线的正交表示来最小化直线的参数个数。
[0029] 用聚类方法kmeans++(K均值++聚类方法)建立综合点线特征的离线视觉词典，用于在线过程中识别和查询相似的图像进行回环检测，在建立词典的过程中通过增加标志位的方法使得点线特征在视觉词典里和建立图像数据库时区别对待，评价两幅图像的相似性时，根据提取到的特征把图像转换成词包向量，其中包含图像中每个视觉词汇的TF-IDF分数，如果一个词汇在同一帧图像中出现的频率越高这个分数就越高，但是在整个数据集中出现频率越高这个分数会越低；
[0030] 词包向量中有点特征部分vip和线特征部分vil；两个词包向量v1，v2的相似性定义为：
[0031]
[0032] 其中a,b为点特征得分和线特征得分的权重值，是两个常数，且满足a+b＝1。
[0033] 由于采用本发明的技术方案，本发明的有益效果为：本发明视觉词典应该用多种多样的大量数据集进行训练，以达到较好的聚类效果，视觉词典建成后可以重复使用；本发明能够尽量使用较少的特征来估计当前时刻相机的位姿，而局部地图匹配涉及到的特征较多，能够得到更精确的解。
附图说明
[0034] 图1是本发明基于聚类方法建立的综合点线特征的视觉词典模型；
[0035] 图2是本发明特征直线的普吕克坐标表示示意图；
[0036] 图3是本发明空间中无线长直线的端点的选取；
[0037] 图4是本发明特征直线的重投影误差模型；
[0038] 图5是本发明利用前端得到的点线特征、相机运动估计、闭环检测等建立图模型。
具体实施方式
[0039] 为了更好地理解本发明的技术方案，以下结合附图作进一步描述。
[0040] 利用聚类方法离线建立视觉词典，确定节点的逆文本频率(IDF)：
[0041] 为了判断相机是否重复访问过同一区域，将每帧图像本身包含的特征转化为视觉词汇。这些视觉词汇对应离散化的描述子空间—称之为视觉词典。如图1所示，使用大量的特征描述子离线建立树状词典，特征描述子从大量的训练图像集中提取出来，建立树状词典的过程也是不断用Kmeans++算法聚类的过程。这里的描述子是ORB点特征描述子和LBD直线特征描述子。由于它们都是256位的二进制描述子，因此可以把它们放在同一个视觉词典里，可以简化建立视觉词典的过程以及进行回环检测时所进行的操作。通常图像中的点特征多和线特征少，因此点线特征在视觉词典里要区别对待。把两种256位的二进制描述子分别进行拓展：为ORB点特征添加标志位0，为LSD线添加标志位1。这样用标志位便可区分直线特征与点特征，在线建立图像数据库、比较图像相似性等时，点特征和线特征也加以区分。如图1为基于聚类方法建立的综合点线特征的视觉词典模型。视觉词典应该用多种多样的大量数据集进行训练，以达到较好的聚类效果，视觉词典建成后可以重复使用。视觉词典中每个节点的权重由该节点包含的所有特征描述子的逆文本频率(IDF)确定。
[0042] IDF＝log(N/ni)
[0043] 其中，N为数据集中所有图像的数量，ni为包含有该节点所代表的特征的图像数量。
[0044] 在线的综合点线特征的视觉SLAM主要步骤：
[0045] 步骤一，从双目相机获取校正后的图像，并进行图像的特征提取及描述[0046] 提取双目相机图像中的点、线特征及其描述子。其中特征点的检测选用FAST角点检测，用ORB描述子进行描述。它们的计算和匹配速度都非常的快，同时对视角具有旋转不变性。直线特征的检测采用LSD(line segment detection)算法提取直线特征并用LBD(line band descriptor)描述子表示直线。ORB描述子和LBD描述子都是256位的二进制描述子，存储结构相同，这为建立综合点线特征的离线词典和查询图像数据库等提供了便利。该步骤与离线建立视觉词典过程中提取特征及描述子的部分相同。
[0047] 步骤二，左右图像特征匹配及三维重建
[0048] 在做左右图像匹配的时候，把右图像中的特征点及特征直线的中点投影到左图像上。由于图像经过校正，只需要在左图中的一个矩形窗口内寻找与从右图特征中汉明距离最小的特征，该特征即为与右图特征匹配的特征。再通过对汉明距离大小进行排序，自适应地选取
阈值，剔除一些距离较大的匹配对，保证匹配的准确度。
[0049] 特征点的三维重建：
[0050] 对于已经校正的图像，假设匹配点在左右图像中的点分别为m＝[u1 v]T和m′＝[u2 v]T，由m和m′决定的三维点M在左相机坐标系下的坐标为[X Y Z]T,则有：
[0051]
[0052]
[0053]
[0054] 其中B、f、uc和vc为图像校正后双目
立体视觉系统的参数,B为双目相机的基线距离，f为相机焦距，[uc vc]T是光轴与像平面交点的
像素坐标,d＝u1-u2是匹配点的
视差，视差反应了三维点的深度。
[0055] 特征直线的三维重建：
[0056] 显然用两个三维的端点来表示直线是不合适的，因为视角的变化和一些障碍物把从图像中提取和追踪直线的端点变得十分的困难。因此，把空间中三维直线表示成无穷长的直线最为合适。如图2所示，采用普吕克坐标用于直线的计算，包括几何变换、三维重建等，用直线的正交表示用于后端的优化。
[0057] 在对直线进行三维重建时，为了高效得进行几何变换和计算，采用普吕克坐标L＝[nT,vT]T来表示直线，如图2，其中n为直线与相机原点Oc构成的平面π的法向量，v为直线L的方向向量。普吕克坐标有一约束条件n垂直于v，即n×v＝0。空间中的直线L在像平面中的投影为直线l,相应的直线端点A、B投影为点a、b。在相机坐标系Oc中，c＝KC,d＝KD,n＝C×D,l＝c×d，其中c,d,l为齐次坐标表示，×为外积，即叉乘。K为相机内参数矩阵，[0058]
[0059] 可推得像平面中的直线l，满足l＝det(K)K-Tn。假设左相机中心与空间直线L构成的平面为πl，右相机中心与空间直线L构成的平面为πr，则这两个平面的交线即为空间直线。平面πl的齐次坐标表示为：
[0060] πl＝PlTll∈R4
[0061] 其中ll为左相机像平面中空间直线所成的像。Pl为与左相机的投影矩阵，[0062] Pl＝Kl[I|0]
[0063] Kl为左相机的内参数矩阵，I为3×3的单位矩阵，0为3×1的零矩阵。同理，由相机的外参数可以得到右相机中心与空间直线L构成的平面为πr的齐次坐标表示πr。两平面的交线即为空间直线L，L的对偶普吕克矩阵表示为
[0064]
[0065] 对偶普吕克矩阵和普吕克坐标表示的关系为：
[0066]
[0067] 利用上式可以得到普吕克坐标。
[0068] 以上即为特征直线的三维重建，另外，由于要建立场景地图，空间直线L是无限长的，为了便于显示，我们需要裁剪该空间直线，即维护直线的两个端点C、D需要维护。空间直线L上端点C、D的选取，可根据一定的规则由左相机像平面中空间直线L的成像ll的端点进行几何变换得到，如图3，为直线的端点的选取示意图。图中e为左相机像平面中与l垂直的直线lc上的点，e-c间的距离可以设为任意值。平面π′为直线ec与相机中心Oc所确定的平面。用平面π′去截空间中的直线L，可以得到端点C。同理，可以得到端点D。在相机运动的过程中，左相机像平面上的同一条空间直线L的成像l的端点c、d并不是固定的，因此裁剪得到的C、D也是不一样的，仅选用距离最大的C、D点作为空间中维护的直线端点。
[0069] 步骤三，前后图像特征匹配及相机的运动估计 左右图像特征匹配及三维重建后，得到了特征点j和特征直线i在世界坐标系中的三维坐标Pjw和特征直线的普吕克坐标Liw，前后图像匹配后可以得到特征点j在当前时刻左图像中的投影特征直线在当前时刻左图像中的投影li。假设当前时刻左相机坐标系Oc在世界坐标系Ow中旋转和平移分别为Rwc和twc，则该特征点在当前时刻左相机坐标系Oc下的坐标为Pjc＝RcwPjw+tcw。特征直线i在当前时刻左相机坐标系Oc下的坐标为，其中Rcw＝RwcT，tcw＝-Rwctwc，分别为世界坐标系在左相机坐标系中的旋转和平移。[tcw]×是由向量tcw构成的3×3的反对称矩阵。将特征点Pjc通过针孔相机模型投影到当前左相机中，得到其投影的图像坐标将特征直线Lic投影到当前左相机图像中，得到其投影直线方程为li。分别定义点和线特征的误差，点的误差为重投影误差，即特征点投影坐标 与观测坐标pj之间的距离epj：
[0070]
[0071] 线的误差为观测线段的两个端点ep1i、ep2i到投影直线方程的几何距离eli：
[0072]
[0073] 其中ep1i＝[ep1i1 ep1i2 1]T为端点ep1i的齐次坐标表示,ep2i为端点ep2i的齐次坐标表示，lc＝[lc1 lc2 lc3]T为直线方程lc的系数构成的向量。
[0074] 运动估计的目标是求解以下非线性最小二乘问题：
[0075]
[0076] 即求解当前相机的姿态使得特征点和特征直线的重投影误差最小。其中α、β为点特征重投影误差和线特征重投影误差的权重值，是两个常数，可根据经验设置。为了剔除错误图像特征匹配的影响，在此步骤可采用Ransac方法得到运动估计的更好的解。
[0077] 步骤四，利用离线训练的视觉词典做回环检测
[0078] 对视觉关键帧提取得到的点线特征描述子，建立图像数据库。将关键帧提取到的所有描述子与视觉词典中的作为节点的聚类中心计算距离，选择词典树中的一层作为比较层(一般选取4-6层)，即把提取到的所有描述子划分到词典树种该层中距离它最近的节点。根据划分情况，可以把图像离散成词包向量，词包向量的维数为比较层节点的个数，词包向量包含图像中每个视觉词汇的TF-IDF分数，词包向量中有点特征部分vip和线特征部分vil。
如果一个词汇在同一帧图像中出现的频率越高这个分数就越高，但是在整个数据集中出现频率越高这个分数会越低。TF-IDF为：
[0079] TF-IDF＝IDF*(niIt/nIt)
[0080] niIt为在图像It中该视觉词汇的数量，nIt为图像It中所有视觉词汇的数量，IDF为该视觉词汇在建立的离线视觉词典中的逆文本频率。
[0081] 新生成的词包向量接下来会和图像数据库中的词包向量进行比较，进行相似性判断。两个词包向量v1，v2的相似性定义为：
[0082]
[0083] 其中a,b为点特征得分和线特征得分的权重值，是两个常数，且满足a+b＝1，可根据经验设置。单单根据相似性来进行回环检测会出现误检，需要辅助其他信息。数据库中的图像在时间上相近一般得到相似的分数。利用这一特性，将时序上相近的图像分组，并以组为单位比较分数，图像组的分数就是组中每帧图像的分数和。每帧图像的分数必须高于某个阈值才能累加到图像组的分数上。一旦搜索整个图像数据库，分组分数最高的那个组就被选中，并且其中单帧图像得分最高的那个图像就被认为是待定闭环图像。最后再利用几何验证(比较图像中的所有特征点)，时间一致性(闭环图像对的前后时间段内的图像也会有相似性)等策略得到闭环的图像对。
[0084] 步骤五，把前端得到的点线特征、相机运动估计、闭环检测等放入基于关键帧的图优化框架中
[0085] 对需要优化的目标函数，即点特征和线特征的误差模型、闭环检测误差模型进行建模。这是非线性优化问题，可以建立图模型，然后利用解该问题的稀疏性调用开源工具g2o(General Graph Optimization)、GTSAM(Georgia Tech Smoothing and Mapping)、Ceressolver等图优化工具进行
迭代优化。最终得到优化后的相机位置姿态与空间中的点和直线。
[0086] 点特征的误差模型：
[0087] 假设当前时刻i左相机坐标系Oc在世界坐标系Ow中旋转和平移分别为Rwc和twc，设重构的特征点j在世界坐标系Ow的坐标为Pwj，则该特征点在当前时刻左相机坐标系Oc下的坐标为：
[0088] Pij＝RcwPwj+tcw
[0089] Pij＝[xij yij zij]T
[0090] pij通过相机投影模型的投影到左相机图像中，图像坐标为 其中π为投影方程：
[0091]
[0092] 其中，fx，fy为相机的横纵方向的焦距，(uc,vc)为相机的成像原点，均为相机内参。
[0093] 点的重投影误差定义为特征点投影坐标 与观测坐标pij之间的距离eij：
[0094]
[0095] 线特征的误差模型：
[0096] 重构的特征直线k在世界坐标系Ow的坐标为Lwk，Lwk＝[nT,vT]T，则该特征直线在当前时刻左相机坐标系Oc下的坐标为：
[0097]
[0098] 如图4所示，将特征直线Lik投影到当前左相机图像中，得到其投影直线方程为[0099]
[0100] 直线L在左相机图像平面的投影直线为 而观测线段为lik。令观测线段lik的端点a,b到投影直线 的距离dl1,dl2设置为误差函数：
[0101]
[0102] 其中a＝[a1 a2 1]T为端点a的齐次坐标,b＝[b1 b2 1]T为端点b的齐次坐标，为直线方程 的系数构成的向量。
[0103] 在后端优化过程中，为了使直线的参数个数最小化防止过参数化，采用正交表示方法(U,W)∈SO3×SO2来参数化直线，其中SO3为三维正交旋转矩阵，SO2为二维正交旋转矩阵，其
自由度分别为3和1。
[0104] 令
[0105] 这里用最少的四个参数 其中θ是3×I的矢量，是一个标量。可以通过U*←R(θ)U, 来更新U,W∈SO3×SO2。
[0106] 闭环约束的误差模型：
[0107] 假设由某一时刻相机的位置姿态xi，利用闭环检测方法检测到该位置i与已经走过的位置i′为同一位置，即找到了一对闭环xi与xi′，产生了闭环约束Cl。则闭环约束的误差为ec＝xi-g(xi′,Cl)。其中函数g为根据闭环匹配对中某一时刻的位置姿态和闭环约束计算闭环匹配对中另一时刻的位置姿态的函数。
[0108] 把特征点和特征直线作为路标l，相机的位置姿态x和路标l作为图模型中的节点,作为为把回环检测Cl、以及双目相机的观测Z作为边建立了图模型，如图5所示。图优化要解决的问题就是在u、z、c已知的情况下，不断地优化变量l、x，因此把已知的u、z、c作为观测Z，把变量l、x作为状态X。图优化模型要解决的问题就是最大化联合概率，求得l*、x*[0109]
[0110]
[0111] 由于假设观测Z在状态Xi,Xj之间的观测误差为e0(Xi,Xj),即上面提到的四种误差。假设所有误差服从协方差为 的高斯分布,则
[0112]
[0113] 取上式的负对数，图优化模型的目标函数将变为:
[0114]
[0115] 该问题为非线性优化问题，在图优化框架中可以通过高斯
牛顿、LM LM(Levenberg-Marquardt Optimization)、Dogleg(Powell提出的方法)等方法求解。
