CN108776976A - 一种同时定位与建图的方法、***及存储介质 - Google Patents
一种同时定位与建图的方法、***及存储介质 Download PDFInfo
- Publication number
- CN108776976A CN108776976A CN201810578095.3A CN201810578095A CN108776976A CN 108776976 A CN108776976 A CN 108776976A CN 201810578095 A CN201810578095 A CN 201810578095A CN 108776976 A CN108776976 A CN 108776976A
- Authority
- CN
- China
- Prior art keywords
- visual field
- big visual
- field frame
- map
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 230000000007 visual effect Effects 0.000 claims abstract description 587
- 230000033001 locomotion Effects 0.000 claims abstract description 34
- 238000003384 imaging method Methods 0.000 claims abstract description 26
- 238000005457 optimization Methods 0.000 claims description 35
- 239000013598 vector Substances 0.000 claims description 31
- 230000003716 rejuvenation Effects 0.000 claims description 19
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 description 15
- 230000008569 process Effects 0.000 description 13
- 238000001514 detection method Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 8
- 230000009466 transformation Effects 0.000 description 8
- 230000006870 function Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000001427 coherent effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000002045 lasting effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 239000011800 void material Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/003—Navigation within 3D models or images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/80—Geometric correction
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Software Systems (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Computer Graphics (AREA)
- Computer Hardware Design (AREA)
- General Engineering & Computer Science (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明的实施例提供了一种同时定位与建图的方法、***和存储介质。该方法包括:通过大视场相机获取大视场图像;根据大视场相机的内参将大视场图像基于多虚拟针孔相机模型投影到至少两个不同朝向的虚拟针孔相机的成像平面上,以得到基于每个虚拟针孔相机的投影图,并且展开所述至少两个不同朝向的所述投影图以得到与大视场图像对应的大视场去畸变图像,其中,多虚拟针孔相机模型包括所述至少两个不同朝向的虚拟针孔相机并且虚拟针孔相机的相机中心与大视场相机的相机中心重合;基于大视场去畸变图像跟踪所述大视场相机运动的位姿并构建地图。上述方案可以保留大视场相机的所有视野,同时能够不受图像畸变的影响。
Description
技术领域
本发明涉及人工智能领域,更具体地涉及一种同时定位与建图的方法、***及存储介质。
背景技术
同时定位与建图(Simultaneous Localization And Mapping,简称SLAM)是一种通过实时跟踪机器人运动并在此过程中同时建立周围环境地图以达到定位导航等目标的技术。
传统的单目SLAM使用的相机为透视相机(perspective camera)或者称为针孔相机(pinhole camera)。由于相机的视角(Field-of-View)有限,所获取的图像间存在的共有特征不足,可能导致SLAM算法跟踪丢失。相对于传统的透视相机,大视场相机如鱼眼相机(fisheye camera)、反射折射相机或全景成像相机等,拥有更为宽广的视角,可以看到更广视野范围内的信息。结合大视场相机的SLAM技术得到了广泛的研究和关注。
现有的基于大视场图像的SLAM技术方案主要有两种。
一种是先对大视场相机获得的大视场图像使用传统的去畸变方法进行去畸变处理,再把去畸变后的图像作为普通图像使用传统单目SLAM技术实现同时定位与建图。这种技术方案简单易行,但是传统的去畸变方法将导致损失很多的视角,不能充分利用大视场相机的广视角。
另一种是基于大视场相机成像模型直接针对未做畸变校正的大视场图像进行SLAM处理。即直接在未做畸变校正的大视场图像上提取特征进行处理。这种技术方案提取的特征可能会受到图像畸变的影响,同时复杂的大视场相机成像模型将导致优化变得异常复杂,从而影响***的性能。
因此,迫切需要一种新的SLAM技术,能够保留大视场相机的所有视野同时避免图像畸变的影响。
发明内容
考虑到上述问题而提出了本发明。本发明提供了一种同时定位与建图的方法,包括:
通过大视场相机获取大视场图像;
根据所述大视场相机的内参将所述大视场图像基于多虚拟针孔相机模型投影到至少两个不同朝向的虚拟针孔相机的成像平面上,以得到基于每个虚拟针孔相机的投影图,并且展开所述至少两个不同朝向的所述投影图以得到与所述大视场图像对应的大视场去畸变图像,其中,所述多虚拟针孔相机模型包括所述至少两个不同朝向的虚拟针孔相机并且所述虚拟针孔相机的相机中心与所述大视场相机的相机中心重合;
基于所述大视场去畸变图像跟踪所述大视场相机运动的位姿并构建地图。
示例性地,所述基于所述大视场去畸变图像跟踪所述大视场相机运动的位姿并构建地图包括:
针对每个大视场去畸变图像提取特征点,并基于所提取的特征点构建对应的大视场帧;
基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图。
示例性地,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图包括初始化步骤,所述初始化步骤包括:
选取两个大视场帧;
把所述两个大视场帧中的每个大视场帧基于所述多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,并通过对所述两个子视场帧进行帧间匹配来构建地图点;
确定所述两个大视场帧为关键大视场帧,并根据所述关键大视场帧和所述地图点构建所述地图。
示例性地,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括全局捆集优化步骤,所述全局捆集优化步骤包括:
针对所述地图中的每个关键大视场帧:
针对与该关键大视场帧关联的每个地图点,基于所述多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点,并根据该地图点关联的特征点与该地图点的重投影点确定该地图点的重投影误差;
根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。
示例性地,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括跟踪步骤,所述跟踪步骤包括:
确定当前大视场帧的参考大视场帧;
根据当前大视场帧和参考大视场帧基于多虚拟针孔相机模型确定所述当前大视场帧的位姿;以及
根据所述当前大视场帧和所述参考大视场帧之间匹配的特征点对,针对所述参考大视场帧上的每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到所述当前大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述当前大视场帧中的重投影点,根据该重投影点与所述当前大视场帧中匹配的特征点确定该地图点的重投影误差,并根据所有所述匹配的特征点关联的地图点的重投影误差更新所述当前大视场帧的位姿。
示例性地,所述确定当前大视场帧的参考大视场帧包括:
确定当前大视场帧的前一大视场帧为参考大视场帧。
示例性地,所述确定当前大视场帧的参考大视场帧包括:
选取局部地图中与当前大视场帧或与该当前大视场帧的前一大视场帧共视程度最高的关键大视场帧作为参考大视场帧。
对于所述地图中的关键大视场帧的数量小于N的情况,所述局部地图包括所述地图中所有的关键大视场帧以及所有的地图点;对于所述地图中的关键大视场帧的数量不小于N的情况,所述局部地图包括所述地图中与所述当前大视场帧共视程度最高的至少N个关键大视场帧以及与所述至少N个关键大视场帧关联的地图点,其中N为大于2的整数。
示例性地,所述初始化步骤还包括:
根据所述关键大视场帧构建基于词袋模型的向量并将所述基于词袋模型的向量加入到地图数据库;
所述确定当前大视场帧的参考大视场帧包括:
根据当前大视场帧构建基于词袋模型的向量,并根据所述基于词袋模型的向量查询所述地图数据库以获取与所述当前大视场帧匹配的关键大视场帧,以作为参考大视场帧。
示例性地,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括地图更新步骤,所述地图更新步骤包括:
确定当前大视场帧是否是关键大视场帧;以及
对于所述当前大视场帧是关键大视场帧的情况:
针对局部地图中的每个关键大视场帧,把该关键大视场帧和所述当前大视场帧基于所述多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,并通过对该两个子视场帧进行帧间匹配来构建新的地图点;
基于所述当前大视场帧的位姿把所述新的地图点变换为世界坐标系中的地图点并***到所述地图;
把所述当前大视场帧***到所述地图。
对于当前大视场帧不是关键大视场帧的情况:
针对局部地图中的每个地图点,根据所述当前大视场帧的位姿把该地图点基于多虚拟针孔相机模型变换到所述当前大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述当前大视场帧中的重投影点,并根据所述重投影点把该地图点与所述当前大视场帧中的特征点进行关联。
示例性地,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括局部捆集优化步骤,所述局部捆集优化步骤包括:
针对所述局部地图中的每个关键大视场帧:
针对与该关键大视场帧关联的每个地图点,基于所述多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点,并根据该地图点关联的特征点与该地图点的重投影点确定该地图点的重投影误差;
根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。
示例性地,所述地图更新步骤还包括:
对于当前大视场帧是关键大视场帧的情况,根据所述当前大视场帧构建新的基于词袋模型的向量并将所述新的基于词袋模型的向量加入到所述地图数据库;
所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括闭环检测处理步骤,所述闭环检测处理步骤包括:
对于当前大视场帧是关键大视场帧的情况,检测所述地图数据库中的、与所述当前大视场帧相似的闭环大视场帧;
确定所述当前大视场帧与所述闭环大视场帧之间匹配的特征点对;
针对所述当前大视场帧中每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到所述闭环大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述闭环大视场帧中的重投影点,根据该重投影点与所述闭环大视场帧中匹配的特征点确定第一重投影误差;
根据所述当前大视场帧中所有匹配的特征点的第一重投影误差确定第一累计重投影误差;
针对所述闭环大视场帧中每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到所述当前大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述当前大视场帧中的重投影点,根据该重投影点与所述当前大视场帧中匹配的特征点确定第二重投影误差;
根据所述闭环大视场帧中所有匹配的特征点的第二重投影误差确定第二累计重投影误差;
利用所述第一累计重投影误差和所述第二累计重投影误差,对所述地图中与所述当前大视场帧具有共视关系的关键大视场帧以及与其关联的地图点进行校正。
示例性地,所述大视场相机是鱼眼相机、反射折射相机或全景成像相机
示例性地,所述至少两个不同朝向包括:
基于立方体的前向、上朝向、下朝向、左朝向以及右朝向;或
夹角为90度的两个朝向。
根据本发明又一方面,还提供了一种用于同时定位与建图的***,包括大视场相机、处理器和存储器,其中:
所述大视场相机用于获取大视场图像;
所述存储器中存储有计算机程序指令,所述计算机程序指令被所述处理器运行时用于执行上述的同时定位与建图的方法。
根据本发明再一方面,还提供了一种用于同时定位与建图的存储介质,所述存储介质上存储了程序指令,所述程序指令在运行时用于执行上述的同时定位与建图的方法。
根据本发明实施例的同时定位与建图的方法、***以存储介质基于多虚拟针孔相机模型对大视场相机获得的大视场图像进行去畸变处理,针对去畸变后的大视场图像基于同样的多虚拟针孔相机模型进行位姿跟踪优化和建图。由此所构建的地图既可以保留大视场相机所有视野,同时也能够不受图像畸变的影响。
上述说明仅是本发明技术方案的概述,为了能够更清楚了解本发明的技术手段,而可依照说明书的内容予以实施,并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂,以下特举本发明的具体实施方式。
附图说明
通过结合附图对本发明实施例进行更详细的描述,本发明的上述以及其它目的、特征和优势将变得更加明显。附图用来提供对本发明实施例的进一步理解,并且构成说明书的一部分,与本发明实施例一起用于解释本发明,并不构成对本发明的限制。在附图中,相同的参考标号通常代表相同部件或步骤。
图1示出了根据本发明一个实施例的同时定位与建图的方法的示意性流程图;
图2示出了通过大视场相机获取的一个大视场图像;
图3示出了通过传统去畸变方法获得的普通去畸变图像;
图4示出了根据本发明一个实施例的2个朝向的多虚拟针孔相机模型;
图5示出了根据本发明一个实施例的5个朝向的多虚拟针孔相机模型;
图6示出了根据本发明一个实施例的基于5个朝向的多虚拟针孔相机模型去畸变的立方体投影图;
图7示出了根据本发明一个实施例的基于5个朝向的多虚拟针孔相机模型去畸变展开的大视场去畸变图像;
图8示出了根据本发明一个实施例的基于大视场去畸变图像跟踪大视场相机运动的位姿并构建地图的示意性流程图;
图9示出了根据本发明一个实施例的基于大视场帧跟踪大视场相机运动的位姿并构建地图的示意性流程图;以及
图10示出了根据本发明一个实施例的三角测量地图点的示意图。
具体实施方式
为了使得本发明的目的、技术方案和优点更为明显,下面将参照附图详细描述根据本发明的示例实施例。显然,所描述的实施例仅仅是本发明的一部分实施例,而不是本发明的全部实施例,应理解,本发明不受这里描述的示例实施例的限制。基于本发明中描述的本发明实施例,本领域技术人员在没有付出创造性劳动的情况下所得到的所有其它实施例都应落入本发明的保护范围之内。
大视场相机拥有宽广的视角,可以看到更广视野范围内的信息。如鱼眼相机的视场角可以达到220°或230°。结合大视场相机的SLAM技术,可以利用大视场相机获得图像间更为丰富的共有特征来跟踪相机运动的位姿并构建地图。
在本发明的实施例中,通过利用多虚拟针孔相机模型模拟大视场相机,提供了一种同时定位与建图的方法。下面,将参考图1描述根据本发明一个实施例的同时定位与建图的方法。图1示出根据本发明一个实施例的同时定位与建图的方法1000的示意性流程图。如图1所示,该方法1000包括以下步骤。
步骤S1100,通过大视场相机获取大视场图像。
大视场相机如鱼眼相机、反射折射相机或全景成像相机等,拥有宽广的视角,可以看到更广视野范围内的信息,从而可以获得宽广视角的大视场图像。图2示出了通过鱼眼相机获取的一个大视场图像。可以看出,该大视场图像比普通相机获得的图像有更为宽广的视场,但是整个图像存在空间畸变,离图像中心越远畸变越大。
步骤S1200,对大视场图像执行去畸变处理。
图3示出了通过传统的去畸变方法对该大视场图像进行去畸变处理后得到去畸变图像。普通相机获得的图像视角一般约为80度,该去畸变图像的视角为100度。虽然相比普通相机获得的图像视角有了提升,但相对于未去畸变处理前的图像还是损失了很多视角。由此,无法构建包括大视场图像的所有视角的地图。
根据本发明的实施例,在步骤S1200中,首先,根据大视场相机的内参将通过步骤S1100获得的大视场图像基于多虚拟针孔相机模型投影到至少两个不同朝向的虚拟针孔相机的成像平面上,以得到基于每个虚拟针孔相机的投影图。然后,展开所述至少两个不同朝向的投影图以得到与该大视场图像对应的大视场去畸变图像。
所述多虚拟针孔相机模型包括所述至少两个不同朝向的虚拟针孔相机,并且所有虚拟针孔相机的相机中心与所述大视场相机的相机中心重合。换言之,多虚拟针孔相机模型由两个以上相机中心与大视场相机的相机中心重合的虚拟针孔相机组成。如图4示出了根据本发明一个实施例的2个朝向的多虚拟针孔相机模型。两个虚拟针孔相机的朝向为90度夹角,且相机中心与大视场相机的相机中心重合于C点。多虚拟针孔相机模型还可以由三个或更多不同朝向的虚拟针孔相机组成,如由包括左向、前向和右向三个不同朝向的虚拟针孔相机组成。图5示出了根据本发明一个实施例的5个朝向的多虚拟针孔相机模型。如图5所示,该多虚拟针孔相机模型包括立方体的前向、上朝向、下朝向、左朝向以及右朝向共5个朝向的虚拟针孔相机。5个虚拟针孔相机的相机中心与大视场相机的相机中心重合于C点。
以图2通过鱼眼相机获得的大视场图像为例,根据大视场相机的内参,基于图5所示的多虚拟针孔相机模型,把该大视场图像投影到5个朝向的虚拟针孔相机的成像平面上,以得到基于每个虚拟针孔相机的投影图,如图6所示。图6示出了根据本发明一个实施例的基于5个朝向的多虚拟针孔相机模型去畸变的立方体投影图。展开该5个朝向的投影图得到与该大视场图像对应的大视场去畸变图像,如图7所示。图7示出了根据本发明一个实施例的基于5个朝向的多虚拟针孔相机模型去畸变展开的大视场去畸变图像。
如图7所示,大视场去畸变图像保留了大视场图像的所有视角。基于该大视场去畸变图像进行SLAM能够构建包括原有所有视角内容的地图。
步骤S1300,基于步骤S1200获得的大视场去畸变图像跟踪该大视场相机运动的位姿并构建地图。
可选地,通过提取大视场去畸变图像的特征点,也即大视场去畸变图像的关键点和描述子,基于大视场去畸变图像的特征点跟踪相机运动的位姿并构建地图。可选地,直接根据大视场去畸变图像中的像素亮度信息估计相机的运动的位姿并构建地图,不用计算关键点和描述子。
通过上述基于多虚拟针孔相机模型去畸变的方法得到的大视场去畸变图像保留了原始大视场图像的所有视角。由此可以基于大视场图像间丰富的共有特征进行同时定位和建图,获得更高效的定位和更准确的地图。与此同时,上述方法还避免了大视场相机复杂的投影模型给***带来的额外计算成本。
图8示出了根据本发明一个实施例的基于大视场去畸变图像跟踪大视场相机运动的位姿并构建地图的示意性流程图。如图8所示,方法S1300可以包括如下步骤。
步骤S1310,针对每个大视场去畸变图像提取特征点,并基于所提取的特征点构建对应的大视场帧。
可选地,对步骤S1200获得的大视场去畸变图像进行缩放,获取该大视场去畸变图像对应的图像金字塔(Image Pyramid)。在该图像金字塔的各尺度图像中提取角点并计算描述子。由所述角点和描述子构成图像的特征点。所述角点是图像中辨识度高且具有代表性的区域,用于表示特征点在图像中的位置信息。描述子可以用向量表示,用于描述角点周围像素的信息。描述子可以按照外观相似的特征点应该有相似的描述子设计。
针对每个大视场去畸变图像提取其特征点,并基于所提取的特征点构建对应的大视场帧。该大视场帧包括对应的大视场去畸变图像中所有的特征点。大视场帧构建完成后,可将该大视场帧对应的大视场去畸变图像的像素数据丢弃,从而节约存储空间,降低***功耗。
步骤S1320,基于步骤S1310获得的大视场帧,跟踪所述大视场相机运动的位姿并构建地图。因为大视场帧中包括大视场去畸变图像中的全部特征点的信息,所以能够依此跟踪大视场相机运动的位姿并构建地图。
由此,可以充分利用大视场去畸变图像的所有视野。同时基于特征点的处理可以进一步减小计算量,从而降低***功耗。
下面,将参照图9描述根据本发明一个实施例的步骤S1320的详细步骤。图9示出了根据本发明一个实施例的基于大视场帧跟踪大视场相机运动的位姿并构建地图的示意性流程图。
步骤S1320可以包括初始化步骤S1321。初始化是根据初始的两个大视场帧,进行SLAM的初始化。初始化步骤S1321可以包括以下子步骤:
初始化子步骤1:选取两个大视场帧。
选取的两个大视场帧可以分别作为初始化的当前大视场帧和参考大视场帧。当前大视场帧和参考大视场帧可以是连续帧,也可以在二者中间具有一个或多个间隔帧。当前大视场帧和参考大视场帧之间需要存在一定的视差,以保证初始化的顺利进行。
初始化子步骤2:把初始化子步骤1选取的当前大视场帧基于多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,对参考大视场帧也执行同样操作。从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,这两个子视场帧分别来自当前大视场帧和参考大视场帧。通过对这两个子视场帧进行帧间匹配来构建地图点。
使用上述实施例中如图5所示的5个朝向的多虚拟针孔相机模型。把参考大视场帧F1基于该多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧F11、F12、F13、F14和F15。把当前大视场帧F2也基于该多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧F21、F22、F23、F24和F25。其中,子视场帧F11和F21对应于前向的虚拟针孔相机,子视场帧F12和F22对应于上朝向的虚拟针孔相机,子视场帧F13和F23对应于下朝向的虚拟针孔相机,子视场帧F14和F24对应于左朝向的虚拟针孔相机,子视场帧F15和F25对应于右朝向的虚拟针孔相机。通过对子视场帧F11和F21、F12和F22、F13和F23、F14和F24以及F15和F25进行帧间匹配来构建地图点。
下面以子视场帧F11和F21为例对帧间匹配进行说明。
首先,对子视场帧F11和F21的特征点进行匹配,检测匹配的特征点对的个数是否大于或等于初始化阈值,若小于初始化阈值,则初始化失败。如果匹配的特征点对的个数超过初始化阈值,运用例如随机抽样一致算法(Random Sample Consensus,简称RANSAC)基于匹配的特征点对的方向向量计算两帧之间的本质(Essential)矩阵。其中,初始化阈值表示初始化构建地图所需的最少的特征点对的数量,可以直接使用默认设置值,如100,也可由用户预先设置。
然后通过分解本质矩阵得到当前大视场帧与参考大视场帧之间的相对位姿,所述相对位姿可以通过位姿矩阵表示。根据当前大视场帧与参考大视场帧之间的相对位姿对匹配的特征点对进行三角测量得到该特征点对对应的地图点的三维坐标,也即地图点的位置。
如图10所示,O1点是子视场帧F11对应的虚拟针孔相机的相机中心,O2点是子视场帧F12对应的虚拟针孔相机的相机中心,p1和p2是匹配的特征点。通过向量O1p1的方向和向量O2p2的方向,就可以确定地图点的三维坐标也即P点的位置。在SLAM中由于噪声的影响,向量O1p1和向量O2p2有可能没有交点,这个时候可以使用例如最小二乘法求取使误差最小的P点的坐标。O1和O2之间的距离对三角测量的误差影响很大。距离太短,也就是说相机的平移太小时,对P点观测的角度误差会导致较大的深度误差。而距离太远,场景的重叠部分会少很多,使特征匹配变得困难。因此,当前大视场帧和参考大视场帧之间需要存在一定的视差。如果选取的两个大视场帧不满足要求,初始化失败,放弃这两个大视场帧并重新初始化。
最后基于上述三角测量得到地图点的三维坐标构建初始的地图点。其中,以该三维坐标作为地图点的坐标,以该三维坐标对应的特征点的描述子作为地图点的描述子。
初始化子步骤3:确定上述两个大视场帧为关键大视场帧,并根据所述关键大视场帧和上述初始的地图点构建地图。
对于新构建的地图点,还需要进行关联处理。一个地图点可被多个关键大视场帧观测到,将观测到这个地图点的关键大视场帧与这个地图点进行关联,同时记录关键大视场帧上哪一个特征点与这个地图点有关联,也即哪一个特征点可以用于测量以得到这个地图点。对于上述初始化得到的地图点,需要关联初始化创建的两个关键大视场帧,同时记录这两个关键大视场帧上哪一个特征点与该地图点有关联。
所构建的初始的地图,包括上述两个关键大视场帧和上述初始的地图点,以及它们之间的关联关系的信息。
可选地,该SLAM方法的初始化还包括如下步骤:对于匹配的特征点对的个数超过初始化阈值的情况,根据上述两个关键大视场帧构建基于词袋(Bag of Word)模型的向量,并将所述基于词袋模型的向量加入到地图数据库。在词袋模型中,根据各种图像特征进行聚类。比如说,眼睛、鼻子、耳朵、嘴、各种特征的边缘和角等等为不同的特征类。假设有10000个类,对于每一个大视场帧,可以分析它含有哪几个类,以1表示有,以0表示没有。那么,这个大视场帧就可用一个10000维的向量来表达。对于不同的大视场帧,可以通过比较它们基于词袋模型的向量来判断它们的相似程度。所述地图数据库用于存储根据关键大视场帧构建的基于词袋模型的向量。
示例性地,步骤S1320还可以包括全局捆集优化步骤S1322。全局捆集优化对SLAM当前建立的地图(以下简称当前地图)中的所有关键大视场帧和所有地图点进行优化。例如,对通过上述初始化步骤S1321构建的地图进行全局捆集优化,即对上述只有两个关键大视频帧和地图点的地图进行全局捆集优化。可以理解,除了对步骤S1321的初始地图,还可以在构建地图过程中的任意时刻,对该时刻当前的地图执行该全局捆集优化步骤S1322。捆集优化的目的在于通过微调地图中关键大视场帧的位姿以及地图点的位置,最小化SLAM所构建地图中的地图点在关键大视场帧上的重投影误差,由此优化所建立的地图。
针对上述地图中的每个关键大视场帧的捆集优化处理如下。
针对与该关键大视场帧关联的每个地图点,基于多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点。根据该地图点在该关键大视场帧上关联的特征点与该地图点的重投影点确定该地图点的重投影误差。
仍以上述图5所示的5个朝向的多虚拟针孔相机模型为例,基于该多虚拟针孔相机模型将地图点变换到对应的虚拟针孔相机的坐标系中,例如与该地图点对应的虚拟针孔相机为前向的虚拟针孔相机,再投影到该前向的虚拟针孔相机的成像平面上以得到该地图点的重投影点。由于这里使用的多虚拟针孔相机模型与步骤S1200中大视场图像去畸变处理所使用的多虚拟针孔相机模型是同一个模型,因此该成像平面对应于该关键大视场帧分解到前向的虚拟针孔相机的子视场帧。该重投影点可以理解为基于该子视场帧的位姿对该地图点的观测值。根据该地图点在该关键大视场帧上关联的特征点(即三角测量得到该地图点所依据的特征点)以及该地图点的重投影点确定该地图点的重投影误差。在SLAM所建立地图不存在误差的理想状况下,重投影误差为0。但是因为现实条件下不可避免地会引入测量误差等误差,导致重投影误差不能被完全消除,SLAM通过最小化该重投影误差来最优化所建立的地图。
根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。可以理解,关键大视场帧的位姿是指大视场相机获取该关键大视场帧的时刻大视场相机运动的位姿,为了简洁,称其为关键大视场帧的位姿。在本文的上下文中,这种某某帧的位姿都具有类似含义。
可选地,根据与该关键大视场帧关联的所有地图点的重投影误差确定损失函数。在得到损失函数之后,可选地,通过高斯牛顿法(Gauss-Newton)、列文伯格-马夸尔特法(Levenberg-Marquardt)等梯度下降方法迭代求解该关键大视场帧的位姿以及与其关联的地图点的位置各自对应的梯度,通过各自对应的梯度更新关键大视场帧的位姿以及与其关联的地图点的位置,最终使得当前的地图达到重投影误差最小的最优状态。
上述捆集优化基于与大视场图像去畸变处理一样的多虚拟针孔相机模型,把复杂的大视场相机的投影模型转换为多个虚拟针孔相机投影模型。由此避免了大视场相机复杂的投影模型带来的复杂优化处理,从而提升了***处理性能。
示例性地,步骤S1320还可以包括跟踪步骤S1323。在跟踪步骤S1323中通过最小化地图点在当前大视场帧上的重投影误差来优化当前大视场相机的位姿。在跟踪步骤S1323中,仅优化当前大视场相机的位姿,而其他时刻的大视场相机的位姿以及地图点的位置保持不变。步骤S1323可以在地图构建的任意时刻进行,例如基于上述初始化步骤1321构建的地图或基于上述全局捆集优化步骤S1322优化后的地图,SLAM持续根据新的大视场帧跟踪大视场相机运动的位姿。跟踪步骤S1323包括以下子步骤:
跟踪子步骤1:确定当前大视场帧的参考大视场帧。
可选地,确定当前大视场帧的前一大视场帧为参考大视场帧。
可选地,选取局部地图中与当前大视场帧共视程度最高的关键大视场帧作为参考大视场帧。其中,对于当前地图中的关键大视场帧的数量小于N的情况,局部地图包括当前地图中所有的关键大视场帧以及所有的地图点,其中N为大于2的整数。N可以直接使用默认设置值,如10,也可由用户预先设置。如当前地图是初始化得到的地图,局部地图即为当前地图,包括初始的两个关键大视场帧以及与其关联的地图点。对于当前地图中的关键大视场帧的数量不小于N的情况,局部地图包括当前地图中与当前大视场帧共视程度最高的至少N个关键大视场帧以及与所述至少N个关键大视场帧关联的地图点。
又可选地,选取局部地图中与当前大视场帧的前一大视场帧共视程度最高的关键大视场帧作为参考大视场帧。当前大视场帧和其前一大视场帧之间通常具有较高的共视程度,所以可以根据后者选取当前大视场帧的参考大视场帧。和与刚刚创建的当前大视场帧共视程度最高的关键大视场帧相比,与其前一大视场帧共视程度最高的关键大视场帧更易于选取。由此,利于SLAM方法的顺利实现。
可选地,通过全局匹配确定参考大视场帧。首先,根据当前大视场帧构建基于词袋模型的向量。然后,根据该基于词袋模型的向量查询初始化步骤S1321建立的地图数据库,以获取与当前大视场帧匹配的关键大视场帧,以作为参考大视场帧。
在一个示例中,对当前大视场帧和其前一大视场帧进行匹配,得到匹配的特征点对。如果匹配的特征点对的数量大于跟踪阈值,确定当前大视场帧的前一大视场帧为参考大视场帧。其中,跟踪阈值表示跟踪大视场相机的位姿所需的最少的特征点对的数量,可以直接使用默认设置值,如20,也可由用户预先设置。
如果当前大视场帧和其前一大视场帧匹配的特征点对的数量不大于跟踪阈值,选取局部地图中与当前大视场帧或与其前一大视场帧共视程度最高的关键大视场帧,对当前大视场帧和该关键大视场帧进行匹配,得到匹配的特征点对。如果匹配的特征点对的数量大于跟踪阈值,确定该关键大视场帧为参考大视场帧。
如果当前大视场帧和该关键大视场帧匹配的特征点对的数量不大于跟踪阈值,通过全局匹配确定参考大视场帧。具体确定过程如前所述,为了简洁,这里不再赘述。
由此,可以获得当前大视场帧的更具参考性的参考大视场帧,使得SLAM跟踪更为准确,建图更为高效。
跟踪子步骤2:根据当前大视场帧和上述确定的参考大视场帧,基于多虚拟针孔相机模型确定当前大视场帧的位姿。在一个示例中,通过确定当前大视场帧与参考大视场帧之间的相对位姿,来确定当前大视场帧的位姿。
把当前大视场帧基于多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,对参考大视场帧也执行同样操作。从而对于每个虚拟针孔相机得到与之对应的两个子视场帧。从分别对应于不同虚拟针孔相机的子视场帧对中,选取匹配的特征点对的数量最多的子视场帧对。通过对该子视场帧对中的两个子视场帧进行帧间匹配获得它们之间的相对位姿。具体的子视场帧的帧间匹配过程与初始化步骤S1321中的帧间匹配处理一致,为了简洁,这里不再赘述。
由于每个虚拟针孔相机的相机中心与大视场相机的相机中心是重合的,因此多虚拟针孔相机模型中的每个虚拟针孔相机与大视场相机之间存在固定的旋转角度。每个虚拟针孔相机的旋转角度对应一个确定的旋转矩阵。由此,大视场帧的位姿矩阵可以通过对应的旋转矩阵变换为子视场帧的位姿矩阵。反之,子视场帧的位姿矩阵也可以通过对应的旋转矩阵变换为大视场帧的位姿矩阵。
上述方案通过多虚拟针孔相机模型,把基于复杂的大视场相机投影模型的位姿求取转换为基于简单的虚拟针孔相机投影模型的位姿求取,使得大视场SLAM的算法极大简化,性能显著提升。
跟踪子步骤3:更新上述跟踪子步骤2获得的当前大视场帧的位姿。
根据当前大视场帧和参考大视场帧之间匹配的特征点对,针对参考大视场帧中每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到当前大视场帧的、对应的虚拟针孔相机的坐标系中。然后,再将该地图点投影到该虚拟针孔相机的成像平面上,以得到该地图点在当前大视场帧中的重投影点。
在一个示例中,当前大视场帧和参考大视场帧之间存在较大视差。基于如图5所示5个朝向的多虚拟针孔相机模型进行处理。参考大视场帧中某个匹配的特征点在左朝向的虚拟针孔相机的成像平面上。该特征点关联的地图点基于多虚拟针孔相机模型进行变换后,对应到当前大视场帧的前向的虚拟针孔相机的坐标系中。在当前大视场帧的前向的虚拟针孔相机的成像平面上得到该地图点的重投影点。可以理解,在参考大视场帧的位姿通过多虚拟针孔相机模型中的左朝向的虚拟针孔相机可以观察到该地图点,而在当前大视场帧的位姿通过多虚拟针孔相机模型中的前向的虚拟针孔相机可以观察到该地图点。
根据该重投影点与当前大视场帧中匹配的特征点确定该地图点的重投影误差。根据参考大视场帧中所有匹配的特征点关联的地图点的重投影误差来更新当前大视场帧的位姿。本步骤中的重投影误差计算以及根据重投影误差更新当前大视场帧的位姿的处理与步骤S1322全局捆集优化中的处理方法一致,为了简洁,这里不再赘述。
通过进一步优化更新当前大视场帧的位姿,提升当前大视场帧的位姿的可信度,减小跟踪误差。使得SLAM跟踪更为准确,建图更为高效。
示例性地,步骤S1320还可以包括地图更新步骤S1324。地图更新步骤S1324可以在当前的地图的基础上,随着大视场相机的运动,扩张地图。换言之,地图更新步骤S1324将***新的地图点到当前的地图中。可选地,地图更新步骤S1324可以在跟踪步骤S1323之后进行,对于当前大视场帧,通过跟踪步骤S1323确定其位姿,也即确定了当前时刻大视场相机运动的位姿。地图更新步骤S1324包括以下子步骤:
地图更新子步骤1:确定当前大视场帧是否是关键大视场帧。
由于大视场相机在持续的运动采集数据,对获得的每个大视场帧进行地图更新操作会带来巨大的计算量。因此,可以选择某些认为重要的大视场帧作为关键大视场帧,再根据关键大视场帧进行地图更新操作。如何确定关键大视场帧可以采用任何常规的或未来开发的技术。例如基于初始的关键大视场帧,每间隔10个大视场帧选1个为关键大视场帧。即选择第11个、第21个、第31个……为关键大视场帧。又例如,选取与上一个关键大视场帧有合适的视差的大视场帧为关键大视场帧。
对于当前大视场帧是关键大视场帧的情况,继续地图更新子步骤2,根据当前大视场帧进行地图更新处理。对于当前大视场帧不是关键大视场帧的情况,继续地图更新子步骤3,对当前大视场帧进行地图点关联处理。
地图更新子步骤2:对于当前大视场帧是关键大视场帧的情况,根据当前大视场帧进行地图更新处理。
针对局部地图中的每个关键大视场帧,把该关键大视场帧基于多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,对当前大视场帧也执行同样操作。从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,并通过对该两个子视场帧进行帧间匹配来构建新的地图点。
可选地,在对该两个子视场帧进行帧间匹配的过程中可以采用基于词袋模型的向量加速特征点之间的匹配。对于通过词袋模型匹配的特征点对,进一步检测其是否符合对极约束。对符合对极约束的特征点对,通过基于该特征点对的三角测量得到新的地图点的三维坐标点。
这里子视场帧的帧间匹配处理以及通过基于特征点对的三角测量得到新的地图点的三维坐标点的过程,与初始化步骤S1321中的对应处理一致,为了简洁,这里不再赘述。
构建新的地图点后,基于当前大视场帧的位姿把该新的地图点变换为世界坐标系中的地图点并***到当前的地图,并把当前大视场帧***到当前的地图。一般把初始化中用于构建地图的第一个关键大视场帧的坐标系作为世界坐标系。之后对于新的地图点,需要进行相机坐标系与世界坐标系的变换。
本领域的普通技术人员可以理解,当前地图通过不断***新的地图点和新的关键大视场帧在逐步“成长”完善。
可选地,根据当前大视场帧构建新的基于词袋模型的向量并将所述新的基于词袋模型的向量加入到上述地图数据库。根据地图数据库可以进行基于词袋模型的向量加速特征点之间的匹配,从而提高SLAM跟踪和建图的效率。
地图更新子步骤3:对于当前大视场帧不是关键大视场帧的情况,对当前大视场帧进行地图点关联处理。
针对局部地图中的每个地图点,根据当前大视场帧的位姿把该地图点基于多虚拟针孔相机模型变换到当前大视场帧的、对应的虚拟针孔相机的坐标系中。再把该地图点投影到该虚拟针孔相机的成像平面上,以得到该地图点在当前大视场帧中的重投影点。如果投影失败,表示从当前大视场帧的位姿观测不到该地图点。如果投影成功,表示从当前大视场帧的位姿可以观测到该地图点,获得该地图点的重投影点。在当前大视场帧的所有特征点中,选择该重投影点附近的特征点中、与该地图点最匹配的特征点与该地图点进行关联。可以理解,通过这一步骤,关联了当前大视场帧以及从该当前大视场帧的位姿可以观测到的地图点。如此,在对下一大视场帧处理时,该当前大视场帧即可作为下一大视场帧的前一大视场帧用于跟踪处理。使得SLAM跟踪更为连贯,定位更为精准,构建的地图更为准确。
示例性地,步骤S1320还可以包括局部捆集优化步骤S1325。
如果当前大视场帧是关键大视场帧,对通过上述步骤S1324更新的地图进行局部捆集优化。局部捆集优化的目的在于通过微调局部地图中关键大视场帧的位姿以及地图点的位置,最小化局部地图中的地图点在关键大视场帧上的重投影误差,由此优化所建立的地图。
针对局部地图中的每个关键大视场帧的捆集优化处理如下。
针对与该关键大视场帧关联的每个地图点,基于所述多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点。并根据该地图点关联的特征点与该地图点的重投影点确定该地图点的重投影误差。根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。本步骤中的捆集优化处理过程和上述全局捆集优化步骤S1322中的处理过程一致,为了简洁,这里不再赘述。
示例性地,步骤S1320还可以包括闭环检测处理步骤S1326。在地图持续构建的过程中,对于每一个新的关键大视场帧执行闭环检测处理步骤S1326进行处理。
对于当前大视场帧是关键大视场帧的情况,基于词袋模型的向量检测当前的地图数据库中的、与当前大视场帧相似的闭环大视场帧。
确定该闭环大视场帧与当前大视场帧之间匹配的特征点对。可选地,可以采用基于词袋模型的向量加速特征点之间的匹配。
根据该闭环大视场帧与当前大视场帧之间匹配的特征点对通过相似变换算子(Sim3Solver)以及RANSAC算法计算该闭环大视场帧与当前大视场帧之间的相似变换矩阵。
针对当前大视场帧中每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到该闭环大视场帧的、对应的虚拟针孔相机的坐标系中。再将该地图点投影到该虚拟针孔相机的成像平面上,以得到该地图点在该闭环大视场帧中的重投影点。根据该重投影点与该闭环大视场帧中匹配的特征点确定第一重投影误差。根据当前大视场帧中所有匹配的特征点的第一重投影误差确定第一累计重投影误差。
针对该闭环大视场帧中每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到当前大视场帧的、对应的虚拟针孔相机的坐标系中。再投影到该虚拟针孔相机的成像平面上,以得到该地图点在当前大视场帧中的重投影点。根据该重投影点与当前大视场帧中匹配的特征点确定第二重投影误差。根据该闭环大视场帧中所有匹配的特征点的第二重投影误差确定第二累计重投影误差。
根据所述第一累计重投影误差和所述第二累计重投影误差确定损失函数。通过最小化损失优化上述相似变换矩阵。
为了消除闭环过程中累计的误差,需要对当前地图中与当前大视场帧具有共视关系的关键大视场帧以及与其关联的地图点进行校正。首先获取与当前大视场帧具有共视关系的关键大视场帧。两个大视场帧观察到的公共的地图点的数量大于共视关系阈值表示这两个大视场帧具有共视关系。其中,共视关系阈值表示判断两个关键大视场帧具有共视关系所需的最少的公共的地图点的数量,可以直接使用默认设置值,如20,也可由用户预先设置。然后通过上述相似变换矩阵校正这些关键大视场帧的位姿以及与这些关键大视场帧所关联的地图点的位置。到此闭环检测处理完成。
随着大视场相机的运动,跟踪计算的大视场相机的位姿以及三角测量得到的地图点的位置,都是有误差的。即使使用局部捆集优化或全局捆集优化去优化,仍然会存在累积误差。通过上述闭环检测处理,可以有效消除累计误差,从而使得SLAM构建的地图更加准确。
可选地,闭环检测处理还包括通过姿态图优化(pose-graph optimization)对当前地图中所有关键大视场帧的位姿以及所有地图点的位置进行进一步优化。可选地,闭环检测处理还包括寻找并消除冗余的关键帧和地图点,以节约***存储空间同时避免冗余的运算操作。
上述实施例中的步骤S1321至S1326给出了一种基于多虚拟针孔相机模型的大视场SLAM的步骤S1320的实施方式。可以理解,基于步骤S1310获取的大视场帧,可以采用任何常规或未来开发的大视场SLAM方法。例如,上述基于多虚拟针孔相机模型进行重投影误差计算进行的优化更新处理,可以替换为基于单位方向向量误差计算进行优化更新处理。所述基于单位方向向量误差计算通过最小化地图点所对应的单位方向向量与该地图点关联的特征点对应的单位方向向量之间的差异达到最终的优化目标。所优化的目标损失可以是单位方向向量之间的距离,或单位向量之间的夹角,或可以是描述向量误差的其他指标。
根据本发明另一方面,还提供了一种用于同时定位与建图的***,包括大视场相机、处理器和存储器。所述大视场相机用于获取大视场图像。所述存储器存储用于实现根据本发明实施例的同时定位与建图的方法中的各个步骤的计算机程序指令。所述处理器用于运行所述存储器中存储的计算机程序指令,以执行根据本发明实施例的同时定位与建图的方法的相应步骤。
此外,根据本发明又一方面,还提供了一种存储介质,在所述存储介质上存储了程序指令,在所述程序指令被计算机或处理器运行时使得所述计算机或处理器执行本发明实施例的同时定位与建图的方法的相应步骤。所述存储介质例如可以包括平板电脑的存储部件、个人计算机的硬盘、只读存储器(ROM)、可擦除可编程只读存储器(EPROM)、便携式紧致盘只读存储器(CD-ROM)、USB存储器、或者上述存储介质的任意组合。所述计算机可读存储介质可以是一个或多个计算机可读存储介质的任意组合。
本领域普通技术人员可以意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
在本申请所提供的几个实施例中,应该理解到,所揭露的设备和方法,可以通过其它的方式实现。例如,以上所描述的设备实施例仅仅是示意性的,例如,所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个设备,或一些特征可以忽略,或不执行。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
类似地,应当理解,为了精简本发明并帮助理解各个发明方面中的一个或多个,在对本发明的示例性实施例的描述中,本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该本发明的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如相应的权利要求书所反映的那样,其发明点在于可以用少于某个公开的单个实施例的所有特征的特征来解决相应的技术问题。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本发明的单独实施例。
本领域的技术人员可以理解,除了特征之间相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外,本领域的技术人员能够理解,尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本发明的各个部件实施例可以以硬件实现,或者以在一个或者多个处理器上运行的软件模块实现,或者以它们的组合实现。本领域的技术人员应当理解,可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本发明实施例的同时定位与建图的装置中的一些模块的一些或者全部功能。本发明还可以实现为用于执行这里所描述的方法的一部分或者全部的装置程序(例如,计算机程序和计算机程序产品)。这样的实现本发明的程序可以存储在计算机可读介质上,或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到,或者在载体信号上提供,或者以任何其他形式提供。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制,并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
以上所述,仅为本发明的具体实施方式或对具体实施方式的说明,本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的保护范围之内。本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
1.一种同时定位与建图的方法,包括:
通过大视场相机获取大视场图像;
根据所述大视场相机的内参将所述大视场图像基于多虚拟针孔相机模型投影到至少两个不同朝向的虚拟针孔相机的成像平面上,以得到基于每个虚拟针孔相机的投影图,并且展开所述至少两个不同朝向的所述投影图以得到与所述大视场图像对应的大视场去畸变图像,其中,所述多虚拟针孔相机模型包括所述至少两个不同朝向的虚拟针孔相机并且所述虚拟针孔相机的相机中心与所述大视场相机的相机中心重合;
基于所述大视场去畸变图像跟踪所述大视场相机运动的位姿并构建地图。
2.如权利要求1所述的方法,其中,所述基于所述大视场去畸变图像跟踪所述大视场相机运动的位姿并构建地图包括:
针对每个大视场去畸变图像提取特征点,并基于所提取的特征点构建对应的大视场帧;
基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图。
3.如权利要求2所述的方法,其中,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图包括初始化步骤,所述初始化步骤包括:
选取两个大视场帧;
把所述两个大视场帧中的每个大视场帧基于所述多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,并通过对所述两个子视场帧进行帧间匹配来构建地图点;
确定所述两个大视场帧为关键大视场帧,并根据所述关键大视场帧和所述地图点构建所述地图。
4.如权利要求3所述的方法,其中,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括全局捆集优化步骤,所述全局捆集优化步骤包括:
针对所述地图中的每个关键大视场帧:
针对与该关键大视场帧关联的每个地图点,基于所述多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点,并根据该地图点关联的特征点与该地图点的重投影点确定该地图点的重投影误差;
根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。
5.如权利要求3或4所述的方法,其中,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括跟踪步骤,所述跟踪步骤包括:
确定当前大视场帧的参考大视场帧;
根据当前大视场帧和参考大视场帧基于多虚拟针孔相机模型确定所述当前大视场帧的位姿;以及
根据所述当前大视场帧和所述参考大视场帧之间匹配的特征点对,针对所述参考大视场帧上的每个匹配的特征点,将该特征点关联的地图点基于多虚拟针孔相机模型变换到所述当前大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述当前大视场帧中的重投影点,根据该重投影点与所述当前大视场帧中匹配的特征点确定该地图点的重投影误差,并根据所有所述匹配的特征点关联的地图点的重投影误差更新所述当前大视场帧的位姿。
6.如权利要求5所述的方法,其中,所述确定当前大视场帧的参考大视场帧包括:
确定当前大视场帧的前一大视场帧为参考大视场帧。
7.如权利要求5所述的方法,其中,所述确定当前大视场帧的参考大视场帧包括:
选取局部地图中与当前大视场帧或与该当前大视场帧的前一大视场帧共视程度最高的关键大视场帧作为参考大视场帧;
其中,
对于所述地图中的关键大视场帧的数量小于N的情况,所述局部地图包括所述地图中所有的关键大视场帧以及所有的地图点;
对于所述地图中的关键大视场帧的数量不小于N的情况,所述局部地图包括所述地图中与所述当前大视场帧共视程度最高的至少N个关键大视场帧以及与所述至少N个关键大视场帧关联的地图点,其中N为大于2的整数。
8.如权利要求5所述的方法,其中,
所述初始化步骤还包括:
根据所述关键大视场帧构建基于词袋模型的向量并将所述基于词袋模型的向量加入到地图数据库;
所述确定当前大视场帧的参考大视场帧包括:
根据当前大视场帧构建基于词袋模型的向量,并根据所述基于词袋模型的向量查询所述地图数据库以获取与所述当前大视场帧匹配的关键大视场帧,以作为参考大视场帧。
9.如权利要求5所述的方法,其中,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括地图更新步骤,所述地图更新步骤包括:
确定当前大视场帧是否是关键大视场帧;以及
对于所述当前大视场帧是关键大视场帧的情况:
针对局部地图中的每个关键大视场帧,把该关键大视场帧和所述当前大视场帧基于所述多虚拟针孔相机模型分解为分别对应每个虚拟针孔相机的子视场帧,从而对于每个虚拟针孔相机得到与之对应的两个子视场帧,并通过对该两个子视场帧进行帧间匹配来构建新的地图点;
基于所述当前大视场帧的位姿把所述新的地图点变换为世界坐标系中的地图点并***到所述地图;
把所述当前大视场帧***到所述地图;
对于当前大视场帧不是关键大视场帧的情况:
针对局部地图中的每个地图点,根据所述当前大视场帧的位姿把该地图点基于多虚拟针孔相机模型变换到所述当前大视场帧的、对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点在所述当前大视场帧中的重投影点,并根据所述重投影点把该地图点与所述当前大视场帧中的特征点进行关联;
其中,
对于所述地图中的关键大视场帧的数量小于N的情况,所述局部地图包括所述地图中所有的关键大视场帧以及所有的地图点;
对于所述地图中的关键大视场帧的数量不小于N的情况,所述局部地图包括所述地图中与所述当前大视场帧共视程度最高的至少N个关键大视场帧以及与所述至少N个关键大视场帧关联的地图点,其中N为大于2的整数。
10.如权利要求9所述的方法,其中,所述基于所述大视场帧跟踪所述大视场相机运动的位姿并构建地图还包括局部捆集优化步骤,所述局部捆集优化步骤包括:
针对所述局部地图中的每个关键大视场帧:
针对与该关键大视场帧关联的每个地图点,基于所述多虚拟针孔相机模型将该地图点变换到对应的虚拟针孔相机的坐标系中,再投影到该虚拟针孔相机的成像平面上,以得到该地图点的重投影点,并根据该地图点关联的特征点与该地图点的重投影点确定该地图点的重投影误差;
根据与该关键大视场帧关联的所有地图点的重投影误差,更新该关键大视场帧的位姿以及与该关键大视场帧关联的所有地图点的位置。
Priority Applications (6)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810578095.3A CN108776976B (zh) | 2018-06-07 | 2018-06-07 | 一种同时定位与建图的方法、***及存储介质 |
EP18921621.1A EP3806036A4 (en) | 2018-06-07 | 2018-12-28 | METHOD AND DEVICE FOR SIMULTANEOUS LOCALIZATION AND MAPPING |
KR1020197039024A KR102367361B1 (ko) | 2018-06-07 | 2018-12-28 | 위치 측정 및 동시 지도화 방법 및 장치 |
PCT/CN2018/124786 WO2019233090A1 (zh) | 2018-06-07 | 2018-12-28 | 一种同时定位与建图的方法及装置 |
JP2019572827A JP7096274B2 (ja) | 2018-06-07 | 2018-12-28 | 自己位置推定と環境マップ作成を同時に行う方法及び装置 |
US16/627,768 US11017545B2 (en) | 2018-06-07 | 2018-12-28 | Method and device of simultaneous localization and mapping |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810578095.3A CN108776976B (zh) | 2018-06-07 | 2018-06-07 | 一种同时定位与建图的方法、***及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108776976A true CN108776976A (zh) | 2018-11-09 |
CN108776976B CN108776976B (zh) | 2020-11-20 |
Family
ID=64024785
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810578095.3A Active CN108776976B (zh) | 2018-06-07 | 2018-06-07 | 一种同时定位与建图的方法、***及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108776976B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台(上海)信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
CN110361005A (zh) * | 2019-06-26 | 2019-10-22 | 深圳前海达闼云端智能科技有限公司 | 定位方法、定位装置、可读存储介质及电子设备 |
WO2019233090A1 (zh) * | 2018-06-07 | 2019-12-12 | 驭势科技(北京)有限公司 | 一种同时定位与建图的方法及装置 |
CN110880187A (zh) * | 2019-10-17 | 2020-03-13 | 北京达佳互联信息技术有限公司 | 一种相机位置信息确定方法、装置、电子设备及存储介质 |
CN111210476A (zh) * | 2018-11-22 | 2020-05-29 | 驭势科技(北京)有限公司 | 一种同时定位与建图的方法及装置 |
CN111260713A (zh) * | 2020-02-13 | 2020-06-09 | 青岛联合创智科技有限公司 | 一种基于图像的深度计算方法 |
CN111583331A (zh) * | 2020-05-12 | 2020-08-25 | 北京轩宇空间科技有限公司 | 用于同时定位和地图构建的方法及装置 |
US11417014B2 (en) | 2020-02-28 | 2022-08-16 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method and apparatus for constructing map |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103426149A (zh) * | 2013-07-24 | 2013-12-04 | 玉振明 | 大视角图像畸变的校正处理方法 |
US20140104424A1 (en) * | 2012-10-11 | 2014-04-17 | GM Global Technology Operations LLC | Imaging surface modeling for camera modeling and virtual view synthesis |
CN104835115A (zh) * | 2015-05-07 | 2015-08-12 | 中国科学院长春光学精密机械与物理研究所 | 用于航空相机的图像成像方法及*** |
CN106296812A (zh) * | 2016-08-18 | 2017-01-04 | 宁波傲视智绘光电科技有限公司 | 同步定位与建图方法 |
CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建*** |
CN107945104A (zh) * | 2017-11-17 | 2018-04-20 | 常州光电技术研究所 | 一种基于太空虚拟现实相机的全景成像方法 |
CN108090958A (zh) * | 2017-12-06 | 2018-05-29 | 上海阅面网络科技有限公司 | 一种机器人同步定位和地图构建方法和*** |
-
2018
- 2018-06-07 CN CN201810578095.3A patent/CN108776976B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140104424A1 (en) * | 2012-10-11 | 2014-04-17 | GM Global Technology Operations LLC | Imaging surface modeling for camera modeling and virtual view synthesis |
CN103426149A (zh) * | 2013-07-24 | 2013-12-04 | 玉振明 | 大视角图像畸变的校正处理方法 |
CN104835115A (zh) * | 2015-05-07 | 2015-08-12 | 中国科学院长春光学精密机械与物理研究所 | 用于航空相机的图像成像方法及*** |
CN106296812A (zh) * | 2016-08-18 | 2017-01-04 | 宁波傲视智绘光电科技有限公司 | 同步定位与建图方法 |
CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建*** |
CN107945104A (zh) * | 2017-11-17 | 2018-04-20 | 常州光电技术研究所 | 一种基于太空虚拟现实相机的全景成像方法 |
CN108090958A (zh) * | 2017-12-06 | 2018-05-29 | 上海阅面网络科技有限公司 | 一种机器人同步定位和地图构建方法和*** |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019233090A1 (zh) * | 2018-06-07 | 2019-12-12 | 驭势科技(北京)有限公司 | 一种同时定位与建图的方法及装置 |
US11017545B2 (en) | 2018-06-07 | 2021-05-25 | Uisee Technologies (Beijing) Ltd. | Method and device of simultaneous localization and mapping |
CN111210476A (zh) * | 2018-11-22 | 2020-05-29 | 驭势科技(北京)有限公司 | 一种同时定位与建图的方法及装置 |
CN111210476B (zh) * | 2018-11-22 | 2023-06-23 | 驭势科技(北京)有限公司 | 一种同时定位与建图的方法及装置 |
CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台(上海)信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
CN110361005A (zh) * | 2019-06-26 | 2019-10-22 | 深圳前海达闼云端智能科技有限公司 | 定位方法、定位装置、可读存储介质及电子设备 |
CN110880187A (zh) * | 2019-10-17 | 2020-03-13 | 北京达佳互联信息技术有限公司 | 一种相机位置信息确定方法、装置、电子设备及存储介质 |
CN110880187B (zh) * | 2019-10-17 | 2022-08-12 | 北京达佳互联信息技术有限公司 | 一种相机位置信息确定方法、装置、电子设备及存储介质 |
CN111260713A (zh) * | 2020-02-13 | 2020-06-09 | 青岛联合创智科技有限公司 | 一种基于图像的深度计算方法 |
US11417014B2 (en) | 2020-02-28 | 2022-08-16 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method and apparatus for constructing map |
CN111583331A (zh) * | 2020-05-12 | 2020-08-25 | 北京轩宇空间科技有限公司 | 用于同时定位和地图构建的方法及装置 |
CN111583331B (zh) * | 2020-05-12 | 2023-09-01 | 北京轩宇空间科技有限公司 | 用于同时定位和地图构建的方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN108776976B (zh) | 2020-11-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108776976A (zh) | 一种同时定位与建图的方法、***及存储介质 | |
CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
US10803365B2 (en) | System and method for relocalization and scene recognition | |
Cvišić et al. | Stereo odometry based on careful feature selection and tracking | |
Arth et al. | Wide area localization on mobile phones | |
US7889197B2 (en) | Method of capturing, processing, and rendering images | |
Arth et al. | Real-time self-localization from panoramic images on mobile devices | |
CN111445526B (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
JP7096274B2 (ja) | 自己位置推定と環境マップ作成を同時に行う方法及び装置 | |
JP5722502B2 (ja) | モバイルデバイスのための平面マッピングおよびトラッキング | |
CN108955718A (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
CN109166149A (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与*** | |
CN108489482A (zh) | 视觉惯性里程计的实现方法及*** | |
CN113272713B (zh) | 用于执行自改进的视觉测程法的***和方法 | |
CN111127524A (zh) | 一种轨迹跟踪与三维重建方法、***及装置 | |
JP6483832B2 (ja) | Rgb−dセンサを使用して物体を走査する方法とシステム | |
CN109461208A (zh) | 三维地图处理方法、装置、介质和计算设备 | |
JP2010519629A (ja) | 画像内の3次元物体のポーズを決定する方法及びデバイス並びに物体追跡のための少なくとも1つのキー画像を創出する方法及びデバイス | |
CN110335351B (zh) | 多模态ar处理方法、装置、***、设备及可读存储介质 | |
Oskiper et al. | Augmented reality binoculars | |
CN109508679A (zh) | 实现眼球三维视线跟踪的方法、装置、设备及存储介质 | |
Yu et al. | A tracking solution for mobile augmented reality based on sensor-aided marker-less tracking and panoramic mapping | |
US11373329B2 (en) | Method of generating 3-dimensional model data | |
CN113689503A (zh) | 目标对象的姿态检测方法、装置、设备及存储介质 | |
CN109785373A (zh) | 一种基于散斑的六自由度位姿估计***及方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |