CN107300917A - 一种基于分层架构的视觉slam后端优化方法 - Google Patents
一种基于分层架构的视觉slam后端优化方法 Download PDFInfo
- Publication number
- CN107300917A CN107300917A CN201710368725.XA CN201710368725A CN107300917A CN 107300917 A CN107300917 A CN 107300917A CN 201710368725 A CN201710368725 A CN 201710368725A CN 107300917 A CN107300917 A CN 107300917A
- Authority
- CN
- China
- Prior art keywords
- frame
- msub
- mrow
- mtd
- optimized
- 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
- 238000005457 optimization Methods 0.000 title claims abstract description 84
- 238000000034 method Methods 0.000 title claims abstract description 50
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 7
- 238000013519 translation Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000009432 framing Methods 0.000 claims 1
- 230000000694 effects Effects 0.000 abstract description 8
- 230000035508 accumulation Effects 0.000 abstract description 3
- 238000009825 accumulation Methods 0.000 abstract description 3
- 230000009183 running Effects 0.000 abstract description 3
- 230000006870 function Effects 0.000 description 14
- 238000004804 winding Methods 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 238000012937 correction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004040 coloring Methods 0.000 description 1
- 235000013399 edible fruits Nutrition 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 231100000572 poisoning Toxicity 0.000 description 1
- 230000000607 poisoning effect Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000003325 tomography Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
- Processing Or Creating Images (AREA)
Abstract
本发明涉及一种基于分层架构的视觉SLAM后端优化方法,其特征在于,包括:建立底层优化层,对关键帧进行优化,得到当前关键帧的最佳位置;建立高层优化层,对关联帧单元内的关键帧进行优化,得到关联帧单元的最佳位置。本发明通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差,减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象,提高了SLAM定位结果和地图的准确性和稳定性,通过将本发明方法和目前效果最好的SLAM方法得到的定位结果进行比较,证明了本发明的有效性。
Description
技术领域
本发明涉及导航技术领域,尤其涉及一种基于分层架构的视觉SLAM后端优化方法。
背景技术
同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)最早于1988年提出,最开始的时候用于描述机器人在未知环境的未知地点中同步的进行地图构建和自身定位。机器人通过传感器获取的环境数据对位置环境进行地图创建,然后根据当前观测到的环境特征与地图中的特征进行匹配从而进行自身的定位。根据使用的传感器不同,SLAM主要可以分为视觉SLAM和激光SLAM。由于图像具有丰富的颜色信息和纹理信息,再加上近年来计算能力的提升,基于图像的视觉SLAM成为研究热点。
在SLAM算法问题中,可以分为前端和后端两个部分。前端的主要作用是计算帧与帧间的相对关系。包括特征点提取、特征点的匹配、利用匹配点计算位姿。后端的作用主要是对前端的输出结果进行优化,得到最优的位姿估计。传统的视觉SLAM方法中,后端优化一般分为两部分,一个是基于当前时刻图像帧的局部优化,另一个是当运动载体回到运动初始点或者行驶过的路径时进行回环检测,利用回环检测结果,从回环起点到终点进行一个整体优化。
传统SLAM方法的缺点在于,局部小窗口的优化对定位误差的矫正效果有限,当运动路线较长时,局部小窗的优化方式仍然避免不了大的位姿漂移。闭环整体优化可以大幅度矫正这种漂移,然而当SLAM技术运用在车辆上时,由于车辆行驶路线的不确定性,在行驶过程中未必能够形成闭环,况且由于车辆行驶路线较长,局部误差累计到足够大时,闭环检测对误差的矫正能力也会受限。
发明内容
鉴于上述的分析,本发明旨在提供一种基于分层架构的视觉SLAM后端优化方法,用以解决现有技术存在位姿漂移、传统SLAM优化方法依赖闭环检测的问题。
本发明的目的主要是通过以下技术方案实现的:
一种基于分层架构的视觉SLAM后端优化方法,包括以下步骤:
步骤S1:建立底层优化层,对关键帧进行优化,得到当前关键帧的最佳位置;
步骤S2:建立高层优化层,对关联帧单元进行优化,得到关联帧单元的最佳位置。
所述步骤S1包括以下步骤:
步骤S101:当生成新的关键帧时,将历史关键帧中与当前关键帧存在连接关系的关键帧作为待优化关键帧,并放入待优化关键帧列表,将待优化关键帧对应的所有地图点作为待优化地图点;
步骤S102:取出所有能够观察到待优化地图点的关键帧,将待优化关键帧以外的其他关键帧全部固定,作为优化参考;
步骤S103:使用图优化的方法,将待优化关键帧和待优化地图点作为优化顶点,所有待优化地图点和能观察到待优化地图点的关键帧的投影关系作为优化边,进行优化迭代,得到最终的优化结果,并将优化过的关键帧设置标记;
步骤S104:从关键帧缓存列表起点开始,取出未被标记的关键帧,进行优化。
所述图优化的方法是通过G2O的开源c++框架来建立的,G2O中定义好了图优化的优化步骤和优化流程,用户在使用G2O时,只需建立误差函数以及优化边。
所建立的误差函数为待优化关键帧对应的三维点投影在图像平面上时,与对应的图像中的二维特征点的位置的误差,称为重投影误差函数,如下式所示:
式中为图像中的特征点二维坐标;K为相机内参矩阵;Rk为待优化关键帧相对于世界坐标系的三维旋转矩阵;tk为待优化关键帧相对于世界坐标系的三维平移向量;Pi为待优化空间三维点坐标。
进一步地,从关键帧缓存列表中的第一帧开始,逐帧检查关键帧是否被标记,将未被标记的关键帧取出,放入待优化关键帧列表;
在待优化关键帧列表中,从未被标记帧开始向前数两帧,向后数两帧,这五帧固定第一帧和最后一帧,优化中间的三帧;倘若最后一帧也为未标记帧,则放弃该帧,固定第一帧,优化后三帧。
所述步骤S2包括以下步骤:
步骤S201:将一定数量的关键帧划分为一个关联帧单元,确定关联帧单元的大小;
步骤S202:通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系;
步骤S203:将关联帧单元的单元位姿和地图点位姿分别作为优化顶点,通过图优化的方法对关联帧单元进行优化。
进一步地,取一个尺寸固定大小的窗口,窗口不断地在待优化关键帧列表中滑动,取出窗口内的关联帧单元进行优化,窗口滑动的间隔有一定的重叠。
进一步地,计算每一个关联帧单元内关键帧相对于关联帧单元的相对位姿Rkr、tkr,并使其保持不变,进行优化时,将关联帧单元位姿和单元间共视点位姿作为优化顶点,通过关键帧与三维点的位置关系,构建关联帧单元与三维点的位置关系,得到误差函数。
关联帧单元位姿与关联帧单元内关键帧位姿的关系如下式所示:
其中Tk为关联帧单元内关键帧位姿,Tr为关联帧单元位姿,Tkr代表单元内关键帧相对于单元的六自由度位姿变换,Rr、tr为待优化单元位姿;T表示关键帧相对于世界坐标系的六自由度三维变换矩阵,包括旋转和平移。
关联帧单元的误差函数为:
通过图优化的方法求出关联帧单元的误差函数最小时对应的Rr和tr的值,得到关联帧单元的最佳位置。
本发明有益效果如下:
本发明提出了一种基于通用分层架构的视觉SLAM优化方法,从多个层次多个尺度对SLAM定位结果进行在线优化,且计算量不会随着优化层的增加而显著增加,相较于传统SLAM优化方法,本方法不依赖回环检测的方式来进行偏差矫正,即使没有形成闭环也可以显著的优化定位结果。同时本方法也支持回环检测,在本方法的基础上进行回环检测会得到更好的定位效果,提高了SLAM定位结果的鲁棒性,通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差,减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象,提高了SLAM定位结果和地图的准确性和稳定性。
本发明的其他特征和优点将在随后的说明书中阐述,并且,部分的从说明书中变得显而易见,或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
附图仅用于示出具体实施例的目的,而并不认为是对本发明的限制,在整个附图中,相同的参考符号表示相同的部件。
图1分层优化流程图;
图2优化单元及优化窗口尺寸示意图;
图3是单元内外点示意图;
图4是底层优化层优化窗口示意图;
具体实施方式
下面结合附图来具体描述本发明的优选实施例,其中,附图构成本申请一部分,并与本发明的实施例一起用于阐释本发明的原理。
本发明实施例提供了一种基于分层架构的视觉SLAM后端优化方法,该方法流程图如图1所示,提出的后端分层优化架构总体上分为底层和上层,底层的任务是将当前关键帧与临近的关键帧和地图点建立连接,通过最小化重投影误差来得到当前关键帧的最佳位置;上层的任务是将固定数量的关键帧绑定作为一个独立的局部关联帧单元,将多个局部关联帧单元建立连接,通过最小化重投影误差来得到关联帧单元的最佳位置。
将程序运行时的第一个图像帧作为关键帧,在以后的关键帧选取中,若某时刻的图像帧与上一个关键帧所提取到的特征点的重复率小于一定阈值(在本实施例中为0.25~0.3),则把该时刻的图像帧作为关键帧。
在当前关键帧中可以提取若干二维特征点,每个二维特征点都在三维世界中对应一个三维地图点,这些三维地图点也称为当前关键帧对应的三维地图点。若当前关键帧所对应的三维地图点与某一个关键帧所对应的三维地图点中存在重合的地图点,则认为当前帧与该关键帧存在连接关系。所述将当前关键帧与临近的关键帧和地图点建立连接,是指找出与当前关键帧存在连接关系的所有关键帧,与当前帧一起通过最小化重投影误差的方式来得到当前关键帧的最佳位置。
具体地,所述基于分层架构的视觉SLAM后端优化方法包括以下步骤:
步骤S1:建立底层优化层,对关键帧进行优化,得到当前关键帧的最佳位置。
在车辆行驶的过程中,SLAM前端会不断生成新的关键帧,从所生成的第一帧关键帧一直到当前关键帧,按照时间序列存入关键帧缓存列表,提取出与当前关键帧存在连接关系的关键帧和地图点,通过固定一部分关键帧的位姿来优化其余的关键帧和地图点,将被优化过的关键帧做上标记。
具体地,所述步骤S1包括以下步骤:
步骤S101:当生成新的关键帧时,将历史关键帧中与当前关键帧存在连接关系的关键帧作为待优化关键帧,并放入待优化关键帧列表,然后将待优化关键帧对应的所有地图点作为待优化地图点。
步骤S102:取出所有能够观察到待优化地图点的关键帧,将待优化关键帧以外的其他关键帧全部固定,作为优化参考。
步骤S103:使用图优化的方法,将待优化关键帧和待优化地图点作为优化顶点,所有待优化地图点和能观察到待优化地图点的关键帧的投影关系作为优化边,进行一定次数优化迭代,得到最终的优化结果,并将优化过的关键帧设置标记。
所述投影关系是指本发明领域中通用的透视投影相机模型所建立的投影关系;
理论上,优化迭代的次数越多,优化效果越好,但是耗费时间也越长,所以优化过程中需要平衡优化精度和消耗时间,经过试验,优化迭代的次数为10-20时能够满足要求;
所述图优化的方法是通过G2O的开源c++框架来建立的,G2O中定义好了图优化的优化步骤和优化流程,用户在使用G2O时,只需建立误差函数以及优化边;
在本实施例中,所建立的误差函数即为待优化关键帧对应的三维点投影在图像平面上时,与对应的图像中的二维特征点的位置的误差,称为重投影误差函数,如下式所示:
式中为图像中的特征点二维坐标;K为相机内参矩阵;Rk为待优化关键帧相对于世界坐标系的三维旋转矩阵,是一个3*3的正交矩阵,表示关键帧相对于世界坐标系的三自由度旋转;tk为待优化关键帧相对于世界坐标系的三维平移向量,是一个3*1的向量,表示关键帧相对于世界坐标系的三自由度平移;Pi为待优化空间三维点坐标。
步骤S104:从关键帧缓存列表起点开始,取出未被标记的关键帧,进行优化。
由于车辆的不断向前行驶,会不断的产生新的关键帧,而步骤S101~S103中的优化过程需要一定时间才能完成,因此在优化的过程中产生的关键帧有可能会被跳过,所以从关键帧缓存列表起点开始,将未被标记的关键帧取出,进行优化;
具体地,从关键帧缓存列表中的第一帧开始,逐帧检查关键帧是否被标记,将未被标记的关键帧取出,放入待优化关键帧列表,如图2所示,在待优化关键帧列表中,从未被标记帧开始向前数两帧,向后数两帧,这五帧固定第一帧和最后一帧,优化中间的三帧;倘若最后一帧也为未标记帧,则放弃该帧,固定第一帧,优化后三帧;优化过程同步骤S103相同。
步骤S2:建立高层优化层,对关联帧单元进行优化,得到关联帧单元的最佳位置。
如图3所示,高层优化层随着程序的运行不断地创建新的高层优化层,每层高层优化层中以一定数量的关键帧绑定作为一个单独的优化单元,作为关联帧单元,随着层数的增加,关联帧单元内的关键帧的数量也在不断地增加。
具体地,所述步骤S2包括以下子步骤:
步骤S201:根据一定数量规律将一定数量的关键帧划分为一个关联帧单元,确定关联帧单元的大小;
所述一定数量规律是指关联帧单元中关键帧数量n与所在层数l呈指数关系,n=2l+1,采用指数映射的关系可以避免相邻高层优化层中关联帧单元内待优化关键帧的数量过于接近,从而避免产生因计算资源分布不均而导致的优化阻滞的现象;
本实施例中,关联帧单元中关键帧的数量为4,根据实验效果得知,此时的优化效果较好。
步骤S202:通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系;
如图4所示,将只有关联帧单元内的关键帧可以观察到的地图点称为单元内点,将相邻关联帧单元间的关键帧所共同观察到的地图点称为单元间共视点;
所述关联帧单元的单元位姿用处于关联帧单元中间位置的关键帧的位姿来表示,单元位姿与关联帧单元内的关键帧的位姿存在相对位置关系,关联帧单元内的关键帧与单元间共视点之间存在投影关系,因此,通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系;
步骤S203:将关联帧单元的单元位姿和地图点位姿分别作为优化顶点,通过图优化的方法对关联帧单元进行优化;
在进行优化时,关联帧单元作为整体进行优化,关联帧单元位姿发生改变后,单元内点、单元内关键帧位姿都要随之发生改变,取一个尺寸固定大小的窗口,窗口不断地在待优化关键帧列表中滑动,取出窗口内的关联帧单元进行优化,为了使优化后的关联帧单元位姿和地图点保持连续而不发生断层的现象,窗口滑动的间隔有一定的重叠;
由于单元间共视点的数量只由相邻关联帧单元临近的几帧关键帧决定,数量基本固定,不随着关联帧单元内关键帧数量的增加而增加,并且每次优化的窗口大小也是一定的,所以优化时的计算量不会随着关联帧单元内关键帧的增加而增加,随着优化层的层数增加,优化的计算量基本保持不变;
计算每一个关联帧单元内关键帧相对于关联帧单元的相对位姿Rkr、tkr,并使其保持不变,进行优化时,将关联帧单元位姿和单元间共视点位姿作为优化顶点,通过关键帧与三维点的位置关系,构建关联帧单元与三维点的位置关系,得到误差函数。关联帧单元位姿与关联帧单元内关键帧位姿的关系如下式所示:
其中Tk为关联帧单元内关键帧位姿,Tr为关联帧单元位姿,Tkr代表单元内关键帧相对于单元的六自由度位姿变换,Rr、tr为待优化单元位姿。
T为三维变换矩阵,是一个4*4的矩阵,表示关键帧相对于世界坐标系的六自由度变换,包括旋转和平移。其左上角的3*3个元素由R构成,右上角的3*1个元素由t构成,第四行的四个元素中前三个为0,最后一个为1,最终可以写成的形式。
将式(2-2)带入(2-1)中,可得:
式(2-3)为关联帧单元内一个关键帧的重投影误差函数,因为优化的是一个整体单元,需要将单元内所有关键帧的重投影误差考虑在内,因此关联帧单元总的误差函数为:
通过图优化的方法求出误差函数(2-4)最小时对应的变量值,即Rr,tr,得到关联帧单元的最佳位置。
综上所述,本发明实施例提供了一种基于分层架构的视觉SLAM后端优化方法,通过将SLAM运行过程中产生的关键帧进行逐层分块优化的方法来减小误差,减少了由于多帧误差积累等原因造成的SLAM地图和定位结果漂移的现象,提高了SLAM定位结果和地图的准确性和稳定性,通过将本发明方法和目前效果最好的SLAM方法得到的定位结果进行比较,证明了本发明的有效性。
本领域技术人员可以理解,实现上述实施例方法的全部或部分流程,可以通过计算机程序来指令相关的硬件来完成,所述的程序可存储于计算机可读存储介质中。其中,所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种基于分层架构的视觉SLAM后端优化方法,其特征在于,包括以下步骤:
步骤S1:建立底层优化层,对关键帧进行优化,得到当前关键帧的最佳位置;
步骤S2:建立高层优化层,对关联帧单元进行优化,得到关联帧单元的最佳位置。
2.根据权利要求1所述的方法,其特征在于,所述步骤S1包括以下步骤:
步骤S101:当生成新的关键帧时,将历史关键帧中与当前关键帧存在连接关系的关键帧作为待优化关键帧,并放入待优化关键帧列表,将待优化关键帧对应的所有地图点作为待优化地图点;
步骤S102:取出所有能够观察到待优化地图点的关键帧,将待优化关键帧以外的其他关键帧全部固定,作为优化参考;
步骤S103:使用图优化的方法,将待优化关键帧和待优化地图点作为优化顶点,所有待优化地图点和能观察到待优化地图点的关键帧的投影关系作为优化边,进行优化迭代,得到最终的优化结果,并将优化过的关键帧设置标记;
步骤S104:从关键帧缓存列表起点开始,取出未被标记的关键帧,进行优化。
3.根据权利要求2所述的方法,其特征在于,所述图优化的方法是通过G2O的开源c++框架来建立的,G2O中定义好了图优化的优化步骤和优化流程,用户在使用G2O时,只需建立误差函数以及优化边。
4.根据权利要求3所述的方法,其特征在于,所建立的误差函数为待优化关键帧对应的三维点投影在图像平面上时,与对应的图像中的二维特征点的位置的误差,称为重投影误差函数,如下式所示:
<mrow>
<mi>e</mi>
<mo>=</mo>
<msup>
<mrow>
<mo>(</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mi>K</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
<mo>&CenterDot;</mo>
<msup>
<mi>P</mi>
<mi>i</mi>
</msup>
<mo>+</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
式中为图像中的特征点二维坐标;K为相机内参矩阵;Rk为待优化关键帧相对于世界坐标系的三维旋转矩阵;tk为待优化关键帧相对于世界坐标系的三维平移向量;Pi为待优化空间三维点坐标。
5.根据权利要求4所述的方法,其特征在于,从关键帧缓存列表中的第一帧开始,逐帧检查关键帧是否被标记,将未被标记的关键帧取出,放入待优化关键帧列表;
在待优化关键帧列表中,从未被标记帧开始向前数两帧,向后数两帧,这五帧固定第一帧和最后一帧,优化中间的三帧;倘若最后一帧也为未标记帧,则放弃该帧,固定第一帧,优化后三帧。
6.根据权利要求5所述的方法,其特征在于,所述步骤S2包括以下步骤:
步骤S201:将一定数量的关键帧划分为一个关联帧单元,确定关联帧单元的大小;
步骤S202:通过关联帧单元内的关键帧建立单元位姿与单元间共视点的关系;
步骤S203:将关联帧单元的单元位姿和地图点位姿分别作为优化顶点,通过图优化的方法对关联帧单元进行优化。
7.根据权利要求6所述的方法,其特征在于,取一个尺寸固定大小的窗口,窗口不断地在待优化关键帧列表中滑动,取出窗口内的关联帧单元进行优化,窗口滑动的间隔有一定的重叠。
8.根据权利要求6或7所述的方法,其特征在于,计算每一个关联帧单元内关键帧相对于关联帧单元的相对位姿Rkr、tkr,并使其保持不变,进行优化时,将关联帧单元位姿和单元间共视点位姿作为优化顶点,通过关键帧与三维点的位置关系,构建关联帧单元与三维点的位置关系,得到误差函数。
9.根据权利要求8所述的方法,其特征在于,关联帧单元位姿与关联帧单元内关键帧位姿的关系如下式所示:
<mrow>
<msub>
<mi>T</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>T</mi>
<mi>r</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>R</mi>
<mi>r</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>t</mi>
<mi>r</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>R</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>R</mi>
<mi>r</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>R</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>R</mi>
<mi>r</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>t</mi>
<mi>r</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>R</mi>
<mi>k</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中Tk为关联帧单元内关键帧位姿,Tr为关联帧单元位姿,Tkr代表单元内关键帧相对于单元的六自由度位姿变换,Rr、tr为待优化单元位姿;T表示关键帧相对于世界坐标系的六自由度三维变换矩阵,包括旋转和平移。
10.根据权利要求9所述的方法,其特征在于,关联帧单元的误差函数为:
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<msub>
<mi>e</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>u</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>v</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mi>K</mi>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>R</mi>
<mi>r</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>R</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>&CenterDot;</mo>
<msup>
<mi>P</mi>
<mi>i</mi>
</msup>
<mo>+</mo>
<msub>
<mi>R</mi>
<mi>r</mi>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>t</mi>
<mrow>
<mi>k</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>t</mi>
<mi>r</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
通过图优化的方法求出关联帧单元的误差函数最小时对应的Rr和tr的值,得到关联帧单元的最佳位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710368725.XA CN107300917B (zh) | 2017-05-23 | 2017-05-23 | 一种基于分层架构的视觉slam后端优化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710368725.XA CN107300917B (zh) | 2017-05-23 | 2017-05-23 | 一种基于分层架构的视觉slam后端优化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107300917A true CN107300917A (zh) | 2017-10-27 |
CN107300917B CN107300917B (zh) | 2019-07-09 |
Family
ID=60137699
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710368725.XA Active CN107300917B (zh) | 2017-05-23 | 2017-05-23 | 一种基于分层架构的视觉slam后端优化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107300917B (zh) |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108151728A (zh) * | 2017-12-06 | 2018-06-12 | 华南理工大学 | 一种用于双目slam的半稠密认知地图创建方法 |
CN108682037A (zh) * | 2018-04-27 | 2018-10-19 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
CN108876854A (zh) * | 2018-04-27 | 2018-11-23 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
CN109029433A (zh) * | 2018-06-28 | 2018-12-18 | 东南大学 | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 |
CN109141395A (zh) * | 2018-07-10 | 2019-01-04 | 深圳市沃特沃德股份有限公司 | 一种基于视觉回环校准陀螺仪的扫地机定位方法及装置 |
CN109544632A (zh) * | 2018-11-05 | 2019-03-29 | 浙江工业大学 | 一种基于层次主题模型的语义slam对象关联方法 |
WO2019100354A1 (zh) * | 2017-11-25 | 2019-05-31 | 华为技术有限公司 | 状态感知方法以及相关设备 |
CN110152293A (zh) * | 2018-02-13 | 2019-08-23 | 腾讯科技(深圳)有限公司 | 操控对象的定位方法及装置、游戏对象的定位方法及装置 |
CN111091602A (zh) * | 2018-10-24 | 2020-05-01 | 舜宇光学(浙江)研究院有限公司 | Slam后端优化方法、优化装置和电子设备 |
CN111539305A (zh) * | 2020-04-20 | 2020-08-14 | 肇庆小鹏汽车有限公司 | 一种地图构建方法及***、车辆及存储介质 |
CN111752265A (zh) * | 2019-03-26 | 2020-10-09 | 通用汽车环球科技运作有限责任公司 | 情景存储器中的超关联 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103901891A (zh) * | 2014-04-12 | 2014-07-02 | 复旦大学 | 一种基于层次结构的动态粒子树slam算法 |
US20150125045A1 (en) * | 2013-11-04 | 2015-05-07 | Steffen Gauglitz | Environment Mapping with Automatic Motion Model Selection |
CN104851094A (zh) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | 一种基于rgb-d的slam算法的改进方法 |
CN105856230A (zh) * | 2016-05-06 | 2016-08-17 | 简燕梅 | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 |
-
2017
- 2017-05-23 CN CN201710368725.XA patent/CN107300917B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20150125045A1 (en) * | 2013-11-04 | 2015-05-07 | Steffen Gauglitz | Environment Mapping with Automatic Motion Model Selection |
CN103901891A (zh) * | 2014-04-12 | 2014-07-02 | 复旦大学 | 一种基于层次结构的动态粒子树slam算法 |
CN104851094A (zh) * | 2015-05-14 | 2015-08-19 | 西安电子科技大学 | 一种基于rgb-d的slam算法的改进方法 |
CN105856230A (zh) * | 2016-05-06 | 2016-08-17 | 简燕梅 | 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法 |
Non-Patent Citations (2)
Title |
---|
李博: "《基于场景外观建模的移动机器人视觉闭环检测研究》", 《中国博士学位论文全文数据库 信息科技辑》 * |
马洪斌: "《基于RGB_D传感器的增进式融合地图研究》", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (21)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019100354A1 (zh) * | 2017-11-25 | 2019-05-31 | 华为技术有限公司 | 状态感知方法以及相关设备 |
CN111373336B (zh) * | 2017-11-25 | 2022-03-29 | 华为技术有限公司 | 状态感知方法以及相关设备 |
CN111373336A (zh) * | 2017-11-25 | 2020-07-03 | 华为技术有限公司 | 状态感知方法以及相关设备 |
CN108151728A (zh) * | 2017-12-06 | 2018-06-12 | 华南理工大学 | 一种用于双目slam的半稠密认知地图创建方法 |
CN110152293A (zh) * | 2018-02-13 | 2019-08-23 | 腾讯科技(深圳)有限公司 | 操控对象的定位方法及装置、游戏对象的定位方法及装置 |
CN108876854B (zh) * | 2018-04-27 | 2022-03-08 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
CN108682037B (zh) * | 2018-04-27 | 2022-03-08 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
CN108682037A (zh) * | 2018-04-27 | 2018-10-19 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
US11276183B2 (en) | 2018-04-27 | 2022-03-15 | Tencent Technology (Shenzhen) Company Limited | Relocalization method and apparatus in camera pose tracking process, device, and storage medium |
CN108876854A (zh) * | 2018-04-27 | 2018-11-23 | 腾讯科技(深圳)有限公司 | 相机姿态追踪过程的重定位方法、装置、设备及存储介质 |
CN109029433A (zh) * | 2018-06-28 | 2018-12-18 | 东南大学 | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 |
CN109141395B (zh) * | 2018-07-10 | 2020-06-09 | 深圳市无限动力发展有限公司 | 一种基于视觉回环校准陀螺仪的扫地机定位方法及装置 |
CN109141395A (zh) * | 2018-07-10 | 2019-01-04 | 深圳市沃特沃德股份有限公司 | 一种基于视觉回环校准陀螺仪的扫地机定位方法及装置 |
CN111091602A (zh) * | 2018-10-24 | 2020-05-01 | 舜宇光学(浙江)研究院有限公司 | Slam后端优化方法、优化装置和电子设备 |
CN111091602B (zh) * | 2018-10-24 | 2023-03-28 | 舜宇光学(浙江)研究院有限公司 | Slam后端优化方法、优化装置和电子设备 |
CN109544632B (zh) * | 2018-11-05 | 2021-08-03 | 浙江工业大学 | 一种基于层次主题模型的语义slam对象关联方法 |
CN109544632A (zh) * | 2018-11-05 | 2019-03-29 | 浙江工业大学 | 一种基于层次主题模型的语义slam对象关联方法 |
CN111752265A (zh) * | 2019-03-26 | 2020-10-09 | 通用汽车环球科技运作有限责任公司 | 情景存储器中的超关联 |
CN111752265B (zh) * | 2019-03-26 | 2024-04-05 | 通用汽车环球科技运作有限责任公司 | 情景存储器中的超关联 |
CN111539305A (zh) * | 2020-04-20 | 2020-08-14 | 肇庆小鹏汽车有限公司 | 一种地图构建方法及***、车辆及存储介质 |
CN111539305B (zh) * | 2020-04-20 | 2024-03-12 | 肇庆小鹏汽车有限公司 | 一种地图构建方法及***、车辆及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN107300917B (zh) | 2019-07-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107300917A (zh) | 一种基于分层架构的视觉slam后端优化方法 | |
CN107677279B (zh) | 一种定位建图的方法及*** | |
CN104537709B (zh) | 一种基于位姿变化的实时三维重建关键帧确定方法 | |
Johannsson et al. | Temporally scalable visual SLAM using a reduced pose graph | |
Persson et al. | Robust stereo visual odometry from monocular techniques | |
WO2022022256A1 (zh) | 回环检测方法及***、可读存储介质、电子设备 | |
CN110501017A (zh) | 一种基于orb_slam2的移动机器人导航地图生成方法 | |
CN106548151A (zh) | 面向智能机器人的目标物检测追踪识别方法和*** | |
CN106441303A (zh) | 一种基于可搜索连续邻域a*算法的路径规划方法 | |
CN109186606A (zh) | 一种基于slam和图像信息的机器人构图及导航方法 | |
US12038303B1 (en) | Vision-and-laser-fused 2.5D map building method | |
CN105606102A (zh) | 一种基于格网模型的pdr室内定位方法及*** | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
CN109740537A (zh) | 人群视频图像中行人图像属性的精确标注方法及*** | |
CN110530371B (zh) | 一种基于深度强化学习的室内地图匹配方法 | |
CN116592897B (zh) | 基于位姿不确定性的改进orb-slam2定位方法 | |
CN113358112A (zh) | 一种地图构建方法及一种激光惯性里程计 | |
Churchill et al. | Experience based navigation: Theory, practice and implementation | |
CN111882602A (zh) | 基于orb特征点和gms匹配过滤器的视觉里程计实现方法 | |
Wen et al. | PLS-VIO: Stereo vision-inertial odometry based on point and line features | |
Liu et al. | A light-weight lidar-inertial slam system with high efficiency and loop closure detection capacity | |
CN110136174A (zh) | 一种目标对象跟踪方法和装置 | |
CN110516527A (zh) | 一种基于实例分割的视觉slam回环检测改进方法 | |
CN115248044A (zh) | 用于加速同步定位与地图构建的方法和装置 | |
Diao et al. | Orb-slam2s: A fast orb-slam2 system with sparse optical flow tracking |
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 |