本发明涉及配准二维(2D)和三维(3D)数据，更具体地涉及针对传感器姿态估计和计算机视觉应用(诸如3D场景重建、机器人学以及增强现实)而配准从RGB-D传感器获得的2D和3D数据。
背景技术：
同时定位与地图构建(SLAM)在计算机视觉、机器人学以及增强现实中的许多应用中使用。SLAM系统的重要部分是传感器定位。其涉及在传感器坐标系中的图元与SLAM地图坐标系的配准。使用单目传感器的SLAM是已知的。随着低成本3D传感器的普及，许多SLAM系统使用红、绿、蓝以及深度(RGB-D)传感器，诸如微软传感器，来利用使用颜色数据和深度数据这两者。
虽然RGB-D传感器可以提高配准准确性，但这种传感器通常由于硬件限制和噪声而仅在有限范围(例如对于为0.5m至4m)内提供准确深度。大多数RGB-D SLAM系统仅使用具有有效深度的像素，而忽略与离传感器太近或太远的场景点关联的像素。这是对传感器提供的信息的低效使用，并且会引入配准不准确性，特别是对于具有大深度变化的场景。
大多数传统SLAM系统使用单个类型的图元作为测量结果，例如2D点或3D点。例如，基于特征的单目SLAM系统提取2D点特征，使用2D对2D点对应性通过三角测量来初始化3D点陆标，然后使用2D点测量结果与3D点地标之间的2D对3D对应性来估计连续图像中的传感器姿态，参见Davison等人，“MonoSLAM:Real-time single camera SLAM,”IEEE Trans.Pattern Anal.Mach.Intell.，第29卷，第6期，第1052-1067页，2007年6月以及Klein和Murray,“Parallel tracking and mapping for small AR workspaces”Proc.IEEE Int’l Symp.Mixed and Augmented Reality(ISMAR)，第1-10页，2007年11月。
基于特征的RGB-D SLAM系统提取3D点特征并使用3D对3D点对应性来估计传感器姿态。在一些SLAM系统中，平面特征也可以被用作测量结果。近来的密集SLAM系统，单目和RGB-D这两者，不依赖特征提取，相反利用图像中的所有2D或3D点并使用于直接配准的测光误差或迭代最近点(ICP)成本最小化。然而，这些系统仍然仅使用单个类型的图元，例如，2D点或3D点。
一些SLAM系统使用3D测量结果的混合。一种系统使用平面对平面和线对平面对应性这两者，参见Trevor等人，“Planar surface SLAM with 3D and 2D sensors”Proc.IEEE Int’l Conf.Robotics and Automation(ICRA)，第3041-3048页，2012年5月。另一种系统使用点对点和平面对平面对应性这两者，参见Taguchi等人，“Point-plane SLAM for hand-held 3D sensors”Proc.IEEE Int’l Conf.Robotics and Automation(ICRA)，第5182-5189页，2013年5月。然而，它们的系统中使用的所有测量结果是3D图元。
一些系统解决RGB-D图像的一些部分中的深度测量结果的缺乏问题，并且使用2D和3D测量结果这两者，参见Hu等人，“Arobust RGB-D SLAM algorithm”Proc.IEEE/RSJ Int’l Conf.Intelligent Robots and Systems(IROS),2012年10月以及Zhang等人，“Real-time depth enhanced monocular odometry,”Proc.IEEE/RSJ Int’l Conf.Intelligent Robots and Systems(IROS)，2014年9月。Hu等人根据可用深度在2D对2D对应性与3D对3D对应性之间启发式地切换，由此，他们在单个配准中不使用这两个对应性。Zhang等人使用2D和3D测量这两者来配准用于视觉测程法的两个图像，但3D测量结果被假定为在两个图像中的仅仅一个中，由此，使用2D对3D对应性。
技术实现要素：
本发明的实施方式提供用于同时配准3D点和3D线的系统和方法。方法可以在同时定位与地图构建(SLAM)系统中使用。
配准是SLAM系统的最重要部分，其关于全局SLAM地图中的陆标来配准从帧提取的测量结果。具有RGB-D传感器的传统SLAM系统由于RGB-D传感器的硬件限制和噪声而仅将深度用于有限范围。不使用在离传感器太远或太近的场景中的深度。这种系统引入配准误差，特别是在具有大深度变化的场景中。
本发明的实施方式提供了使用具有和不具有深度的点测量结果的新颖的RGB-D SLAM系统和方法。系统从RGB帧(图像)提取关键点。具有有效深度的关键点变成3D点特征，而没有有效深度的关键点被认为是2D点特征。基于传感器姿态和传感器的固有参数用穿过传感器的光学中心和该2D点的2D图像坐标的3D线来表示帧的2D点。
然后，确定点对点、线对点以及线对线对应性。对于线对点对应性，我们使用由2D点特征关联的3D线来“精确定位”对应的3D点特征，并且生成比仅使用点对点对应性大的范围约束。对于线对线对应性，三角测量3D线来生成在随后处理中用作3D点特征的3D点。这里，因为3D线和2D点都提及没有有效深度的关键点，所以它们可互换地使用。系统还可以提取平面来生成3D平面特征，并且除了使用点对点对应性、线对点对应性以及线对线对应性，还可以使用平面对平面对应性来进一步提高配准准确性。
混合对应性用于在线SLAM和离线后处理这两者中。在线SLAM针对实时操作通过确定连续图像间的对应性而较关注于处理速度，而离线后处理为了较高的准确性而在所有图像之间生成较多的对应性。
附图说明
[图1A]图1A示意性示出了根据本发明的实施方式的2D和3D点测量结果。
[图1B]图1B示意性示出了传统3D测量结果。
[图2]图2是根据本发明的实施方式的用于由传感器获取的2D和3D数据的混合同时定位与地图构建(SLAM)的系统和方法的示意图。
[图3]图3是图2的系统和方法的另外细节的示意图。
具体实施方式
本发明的实施方式提供了用于由红、绿、蓝以及深度(RGB-D)传感器获取的2D和3D数据的混合同时定位与地图构建的系统和方法。
图1A示意性示出了从由我们发明的实施方式使用的2D点“精确定位”3D点的关键构思。三角形100表示处于不同姿态(视点)即不同3D位置和3D方位的传感器。另选地，可以使用多个传感器。
虚线101表示与2D测量结果即具有无效深度的2D关键点关联的线，并且圆102和实线103表示3D测量结果。三角测量的3D点用菱形104来示出。三角测量使用线101的交点。除了在传统SLAM系统中使用的点对点对应性之外，我们还使用线对点和线对线对应性来生成大范围约束。由2D点(具有无效深度的关键点)限定的线“精确定位”3D点，即具有有效深度的关键点，或通过使线交叉而三角测量的3D点。
图1B示出了传统SLAM系统，其仅使用点对点对应性，这由于有限深度范围而得到图像之间的小范围约束。
与传统SLAM系统对比，我们的系统从图像提取2D和3D测量结果这两者，其中，具有有效深度的关键点变成3D测量结果，并且没有有效深度的关键点被当作2D测量结果。然后，系统建立点对点、线对点以及线对线对应性。包括2D测量结果的另外对应性提供图像之间的大范围约束。即，我们用由2D点限定的线来“精确定位”3D点。
我们的系统另外可以提取平面来生成3D平面特征，并且除了使用点对点对应性、线对点对应性以及线对线对应性，还可以使用平面对平面对应性来进一步提高配准准确性。
我们在在线SLAM和离线后处理这两者中使用混合对应性。对于在线SLAM，我们针对实时处理限制相邻图像间的对应性搜索。与仅使用点对点对应性的传统方法相比，该方案生成较大范围的约束，但不利用可能存在于远处图像之间的对应性。
我们的后处理通过迭代地确定各图像与使用来自所有图像的测量结果和对应性生成的SLAM地图之间的混合对应性来建立这种对应性。在各迭代中，首先基于图像的当前姿态更新对应性，这些对应性然后用于使用光束平差(bundle adjustment)过程来更新姿态。
精确定位SLAM和后处理
该章节描述我们的使用3D点和3D线的配准方法。我们的方法可以并入到任何基于点特征的RGB-D SLAM系统中。这里我们提供基于关键帧的SLAM系统的示例，其使用在SLAM地图中存储代表关键帧。图2中可以看到我们的配准方法的概述，并且图3中可以看到使用配准方法的我们的SLAM系统。
我们将方法用于在线SLAM和离线后处理这两者中。在在线SLAM中，我们将各输入帧处理一次，并且将帧配准到包括关键帧的之前测量结果和对应性的SLAM地图380。在离线后处理中，我们处理SLAM地图中的各关键帧，从而将该关键帧与SLAM地图中的所有关键帧配准，并且只要关键帧的姿态被更新就迭代处理。
符号
我们使用测量结果和陆标的传统术语。系统从各RGB-D帧提取测量结果并在全局SLAM地图中生成陆标。
3D点测量结果是(pm,Dm)，其中，表示其3D位置，并且Dm表示其关联的描述符。2D点测量是其中，表示2D像素坐标，是其关联的描述符，并且vm＝(cm，um)表示3D线，该3D线穿过传感器的光学中心和2D点测量结果使得基于传感器的固有参数即焦距(fx,fy)和主点(cx,cy))成立
cm＝[0，0，0]T以及
um＝[(qx-cx)/fx，(qy-cy)/fy，1]T
并且T是转置运算符。
3D平面测量结果由(πm,Im)来表示，其表示平面参数和与平面关联的一组3D内围层点。
陆标是测量结果的集合。3D点陆标由(pl,Dl)表示，其中，表示其3D位置，并且Dl表示与该陆标关联的一组描述符。3D平面陆标是(πl,Il)，其具有平面参数πl和来自所有关联帧的3D内围层点的集合Il。
配准方法
我们利用红、绿、蓝以及深度(RGB-D)传感器201获取2103D场景202的数据203。该数据可以被获取作为针对传感器的不同姿态(视点)的帧序列，或者另选地使用多个传感器。从各帧中的数据，我们提取3D点211和3D线212以及关联的描述符。
我们选择220包括在第一坐标系中表示的至少三个3D点的第一组图元。我们还选择230在第二坐标系中表示的第二组图元，其中，第二组图元包括3D点和3D线的任意组合从而获得总共至少3个图元。
然后，我们联合地通过使用第一组图元和第二组图元将3D点彼此配准240，并且将3D线与3D点配准240从而获得配准后的图元250。配准可以使用随机抽样一致(RANSAC)。配准后的图元可以在任意类型的SLAM系统260中使用。步骤在处理器200中执行。
特征提取
我们的系统从各RGB图像提取311 2D关键点。各关键点具有位置(x,y)和关联的描述符。2D关键点可以通过使用例如尺度不变特征变换(SIFT)关键点检测器和描述符来获得，但任意种类的关键点检测器和描述符可以用于该步骤。如果对应的深度在预定义范围内，那么深度被认为是有效的。具有有效深度的关键点被后向投影并用作3D点测量结果322。具有无效深度的关键点被认为是2D点测量结果321，并且被表示为穿过传感器的光学中心和2D关键点的3D线。
我们还可以使用例如凝聚层次聚类agglomerative hierarchical clustering来提取312 3D平面测量323，参见Feng等人，“Fast plane extraction in organized point clouds using agglomerative hierarchical clustering,”Proc.IEEE Int’l Conf.Robotics and Automation(ICRA)，2014年5月。
因为后处理应用于已经提取了特征的已有SLAM地图，所以在后处理期间省略了特征提取步骤。
注意，关键点可以从强度图像提取而不从RGB图像提取。由此，我们的系统可以将强度和深度帧而不是RGB-D帧用作输入。这里，像素图像用于指代强度图像或RGB图像。
对应性搜索
在特征提取之后，我们确定从当前帧提取的关键点与SLAM地图中的关键点之间的对应性330。因为平面的数量远远少于点的数量，所以不搜索平面对应性。反而，我们的RANSAC配准考虑所有可能的平面对应性。
在线SLAM
我们在当前帧的关键点与SLAM地图的最后k个关键帧的关键点之间执行全部对全部描述符匹配。因为假定连续传感器动作的情况下最后关键帧较可能观察与当前帧场景中的相同区域，所以考虑k个关键帧而不是考虑地图的所有关键点，减少了搜索时间。匹配返回以下的对应性、点对点、线对点以及线对线对应性，在RANSAC配准阶段中特别考虑对应性中的每个。线对点对应性考虑两种情况：从3D点陆标到2D点测量结果或从2D点陆标到3D点测量结果。
离线后处理
我们通过使用关键帧的姿态作为预测来避免不必要的描述符。我们基于所预测的姿态将地图的所有3D点投影到帧。如果帧中的测量结果的投影落在r个像素的邻域内，则其与3D点陆标匹配。这生成点对点对应性或从3D陆标到2D测量结果的对应性(即，点对线)。对于2D陆标与3D测量结果之间的点对线对应性，随着投影方向的变化以类似方式进行搜索，即，帧的3D点测量结果被投影到初始化2D点陆标的关键帧。对于线对线对应性而言，为了将2D点测量与2D点陆标匹配，我们测试到核线的距离小于r个像素。
RANSAC配准
RANSAC配准340按(i)三个平面、(ii)两个平面+一个3D点、(iii)一个平面+两个3D点以及(iv)三个3D点的顺序尝试不同类型的假设。因为我们使用2D点测量结果，所以我们还将最后一个假设添加到该列表，其考虑了三个线对点对应性。
我们应用透视三点(P3P)过程来使用线对点对应性确定配准参数，参见Haralick等人，“Review and analysis of solutions of the three point perspective pose estimation problem”Int’l J.Computer Vision，第13卷，第3期，第331-356页，1994年12月。注意，除了线对点对应性，通过忽视3D点中的一个的深度，我们还可以把点对点对应性当作线对点对应性。
基于对应性的类型进行RANSAC内围层检查。两个3D点在点之间的距离小于阈值时被认为是内围层。对于线对点对应性，我们考虑3D点和对应于2D点的线之间的距离。3D点陆标pl与2D点测量qm之间的距离是δ(pl，qm)，
δ(pl,qm)＝||pl-(cm+t*um|| (2)
其中，T是包含2D点测量的关键帧的姿态，并且<·,·>表示点积。如果距离小于阈值，那么对应性被计为内围层。对于线对线对应性，我们检查像素到其他点的对应核线的距离小于阈值。
SLAM地图更新
如果RANSAC配准成功350，则使用所估计的姿态和内围层对应性来更新370SLAM地图380。
在线SLAM
当帧的所估计姿态与地图中已经存在的关键帧的姿态不同时，将该帧作为新的关键帧360添加到地图。如果2D测量结果与3D点陆标匹配，那么通过添加2D测量结果的描述符来扩大描述符的集合。对于线对线匹配的情况，我们通过确定两个线上的最近点的中部来执行三角测量，并且通过收集2D测量结果的描述符将该中部作为3D点陆标添加到地图。我们忽略用小照相机基线进行的线对线匹配的三角测量，因为其引入噪声。如果3D点测量结果与2D点陆标匹配，则经由被转移到地图坐标系的3D点测量的3D坐标来更新陆标作为3D点陆标。所有未被匹配的点测量作为点陆标被添加到地图。
离线后处理
因为后处理应用于现有地图，所以所有测量已经关联到陆标。因此，该阶段仅更新在关键帧与地图之间的对应性。使用RANSAC内围层来细化关键帧的对应性。如果测量结果与其当前关联到的陆标不同的陆标匹配，则该测量结果被从当前路标去除并且与新内围层陆标关联。类似地，如果测量结果与其当前陆标不匹配，则该测量结果与该陆标分离，并且生成新陆标。
光束平差(bundle adjustment)
三元组(k,l,m)表示关联，其中3D点陆标pl(l＝1，...，L)关联到具有姿态Tk(k＝1，...，K)的关键帧k中的3D点测量结果或2D点测量结果类似地，三元组(k，l′，m′)表示使得3D平面陆标πl'(l'＝1,…,L')关联到关键帧k中的3D平面测量结果的关联。假设A1、A2以及A3分别包含表示地图中的点对点关联、线对点关联以及平面对平面关联的所有三元组。然后，光束平差390使与3D点陆标坐标、3D平面陆标参数以及关键帧姿态有关的以下误差函数最小化：
这里，是从关键帧k中的平面测量结果的一组内围层点(πm′，Im′)采样的3D点到平面陆标πl的距离。
在线SLAM
异步地执行光束平差。我们还执行合环。合环首先使用基于关键点确定的局部聚合描述符向量(VLAD)描述符检查地图中的关键帧的所有对的外表相似性，参见Jégou等人，“Aggregating local image descriptors into compact codes”IEEE Trans.Pattern Anal.Mach.Intell.，第34卷，第9期，第1704-1716页，2012年9月。在线SLAM还检查关键帧的对之间的相似性。当关键帧的当前姿态过于不同时我们不尝试合环。对于通过相似性测试的关键帧对，我们的系统然后使用混合对应性来执行RANSAC配准，并且如果RANSAC成功，则使用内围层对应性来更新地图中的关联。
离线后处理
后处理迭代地执行，以更新关联并细化地图中的陆标参数和关键帧姿态。在后处理的迭代中，每个关键帧被与包括剩余关键帧在内的地图重新配准，并且更新其对应性，即，在需要时执行陆标的分离和融合。在更新所有对应性之后，我们再次执行光束平差，以细化陆标参数和关键帧姿态。如果关键帧姿态中的平均变化大于阈值，则我们重复迭代。由此，光束平差在离线后处理中依次运行。
SLAM系统的总结
我们用例如红、绿、蓝以及深度(RGB-D)传感器201获取310 3D场景202的帧203。如下依次处理帧中的数据。
对于各帧，我们提取311关键点。关键点包括没有有效深度的2D点321和具有有效深度的3D点322。我们还可以提取312 3D平面323。然后，我们执行当前帧与包括先前已处理的帧的SLAM地图380之间的对应性的搜索330从而获得点对应性，并且考虑输入帧与SLAM地图之间的所有可能的平面对应性。接着，我们使用点对点、线对点、线对线以及平面对平面对应性执行RANSAC配准340。
如果RANSAC成功350，那么确定360帧是否是新的关键帧，如果是，则因此更新370SLAM地图380，否则，我们获取下一帧。我们异步地执行光束平差和合环过程390来优化SLAM地图380中的3D点陆标、3D平面陆标以及关键帧姿态。光束平差同时细化描述场景202的几何结构的3D坐标。
本发明的效果
本发明的实施方式提供用于SLAM系统的配准后的图元。本发明的一个目的是有效地使用由RGB-D传感器获取的所有数据。系统可以被认为是2D和3D SLAM的混合。2D测量结果被表示为穿过传感器的光学中心和2D点的3D线。
将3D点匹配到关联的3D线改善了帧之间的配准。两个匹配的2D测量结果被三角测量并添加到地图。
系统支持帧之间的较好的对应性，这产生配准准确性的提高。此外，同一方案用于可以细化重建的离线后处理过程。
结果表明，对于具有高的深度变化的场景，与传统系统相比，精确位置SLAM和后处理提高了准确性。