CN112819744A - Gnss和视觉slam融合的轨迹测量方法和装置 - Google Patents
Gnss和视觉slam融合的轨迹测量方法和装置 Download PDFInfo
- Publication number
- CN112819744A CN112819744A CN202110221070.XA CN202110221070A CN112819744A CN 112819744 A CN112819744 A CN 112819744A CN 202110221070 A CN202110221070 A CN 202110221070A CN 112819744 A CN112819744 A CN 112819744A
- Authority
- CN
- China
- Prior art keywords
- camera
- gnss
- current frame
- visual slam
- pose
- 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
- 230000000007 visual effect Effects 0.000 title claims abstract description 107
- 238000000034 method Methods 0.000 title claims abstract description 22
- 230000033001 locomotion Effects 0.000 claims abstract description 68
- 238000005457 optimization Methods 0.000 claims abstract description 56
- 238000000691 measurement method Methods 0.000 claims abstract description 19
- 238000001514 detection method Methods 0.000 claims description 13
- 238000013507 mapping Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 8
- 238000000605 extraction Methods 0.000 claims description 5
- 238000005259 measurement Methods 0.000 abstract description 25
- 239000011159 matrix material Substances 0.000 description 13
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 8
- 230000004927 fusion Effects 0.000 description 7
- 239000000126 substance Substances 0.000 description 6
- 230000008569 process Effects 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 239000000463 material Substances 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 239000013598 vector Substances 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/50—Image enhancement or restoration using two or more images, e.g. averaging or subtraction
-
- 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
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20212—Image combination
- G06T2207/20221—Image fusion; Image merging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种GNSS和视觉SLAM融合的轨迹测量方法和装置，所述方法，包括以下步骤：提取当前帧的特征点，并跟踪相机的位姿；根据当前帧的特征点，判断当前帧是否可作为关键帧；如果当前帧可作为关键帧，则获取相机的运动轨迹；根据相机的运动轨迹，对视觉SLAM进行初始化；在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。由此，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法，能够在低成本、高易用性的前提下提升轨迹测量精度。
Description
技术领域
本发明涉及图像处理技术领域，尤其涉及一种GNSS和视觉SLAM融合的轨迹测量方法和装置。
背景技术
相关技术中，参考轨迹有如下测量方案：基于仿真的数据集，相机运动轨迹已知；在室外，使用差分GNSS+INS融合导航的方式测量相机轨迹；在室内，使用相机集群或激光雷达测量设备的轨迹；使用视觉标记物，测量设备轨迹。
这些方案中，相机集群、激光雷达、差分GNSS+INS融合导航的定位精度高，但设备价格昂贵。使用视觉标记物定位，则需要在测试场景中放置多个标志物，并预先测定它们的位置，如果测试场景很大，那么该方案的可实现性很低。因此，相关技术方案成本较高、易用性差。
发明内容
本发明旨在至少从一定程度上解决相关技术中的技术问题之一。为此，本发明的第一个目的在于提出一种GNSS和视觉SLAM融合的轨迹测量方法，该方法能够在低成本、高易用性的前提下提升轨迹的测量精度。
本发明的第二个目的在于提出一种GNSS和视觉SLAM融合的轨迹测量装置。
本发明的第三个目的在于提出一种计算机设备。
本发明的第四个目的在于提出一种非临时性计算机可读存储介质。
为达到上述目的，本发明第一方面实施例提出了一种GNSS和视觉SLAM融合的轨迹测量方法，包括以下步骤：提取当前帧的特征，并跟踪相机的位姿；根据所述当前帧的特征，判断所述当前帧是否可作为关键帧；如果所述当前帧可作为所述关键帧，则获取相机运动轨迹；根据所述相机的运动轨迹，对视觉SLAM进行初始化；在所述初始化完成之后，对所述相机的位姿进行扩展局部优化和全局优化。
根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法，提取当前帧的特征，并跟踪相机的位姿，根据当前帧的特征判断当前帧是否可作为关键帧，如果当前帧可作为关键帧，则获取相机运动轨迹，并根据相机的运动轨迹，对视觉SLAM进行初始化，在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。由此，该方法能够在低成本、高易用性的前提下提升轨迹的测量精度。
另外，根据本发明上述实施例提出的GNSS和视觉SLAM融合的轨迹测量方法还可以具有如下附加的技术特征：
根据本发明的一个实施例，在获取到所述相机运动轨迹是沿直线运行时，所述根据所述相机的运动轨迹，进行初始化，包括：在所述相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点时，对视觉SLAM的尺度进行初始化。
根据本发明的一个实施例，在获取到所述相机运动轨迹是沿两个相互垂直方向时，所述根据所述相机的运动轨迹，进行初始化，包括：在所述相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值的情况下，对视觉SLAM的尺度进行初始化；在完成了所述尺度的初始化之后，对视觉SLAM的位姿进行初始化。
根据本发明的一个实施例，所述判断所述当前帧是否可作为关键帧之后，包括：将所述当前帧发送给视觉建图线程，以便构建局部地图；将所述当前帧发送给回环检测线程，以便检测所述相机的运动轨迹是否存在回环；如果检测到所述相机的运动轨迹存在回环，则进行回环连接。
为达到上述目的，本发明第二方面实施例提出了一种GNSS和视觉SLAM融合的轨迹测量装置，包括：提取模块，用于提取当前帧的特征，并跟踪相机的位姿；判断模块，用于根据所述当前帧的特征，判断所述当前帧是否可作为关键帧；获取模块，用于在所述当前帧可作为所述关键帧时，获取相机运动轨迹；初始化模块，用于根据所述相机的运动轨迹，对视觉SLAM进行初始化；优化模块，用于在所述初始化完成之后，对所述相机的位姿进行扩展局部优化和全局优化。
根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置，通过提取模块提取当前帧的特征，并跟踪相机的位姿，通过判断模块根据当前帧的特征，判断当前帧是否可作为关键帧，通过获取模块在当前帧可作为关键帧时，获取相机运动轨迹，通过初始化模块根据相机的运动轨迹，对视觉SLAM进行初始化，通过优化模块在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。由此，该装置能够在低成本、高易用性的前提下提升轨迹的测量精度。
另外，根据本发明上述实施例提出的GNSS和视觉SLAM融合的轨迹测量装置还可以具有如下附加的技术特征：
根据本发明的一个实施例，在所述获取模块获取到所述相机运动轨迹是沿直线运行时，所述初始化模块，具体用于：在所述相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点时，对视觉SLAM的尺度进行初始化。
根据本发明的一个实施例，在所述获取模块获取到所述相机运动轨迹是沿两个相互垂直方向时，所处初始化模块，具体用于：在所述相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值的情况下，对视觉SLAM的尺度进行初始化；在完成了所述尺度的初始化之后，对视觉SLAM的位姿进行初始化。
根据本发明的一个实施例，上述装置，还包括：建图模块，用于将所述当前帧发送给视觉建图线程，以便构建局部地图；检测模块，用于将所述当前帧发送给回环检测线程，以便检测所述相机的运动轨迹是否存在回环；连接模块，用于在检测到所述相机的运动轨迹存在回环时，进行回环连接。
为达到上述目的，本发明第三方面实施例提出了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时，实现上述的GNSS和视觉SLAM融合的轨迹测量方法。
本发明实施例的计算机设备，能够在低成本、高易用性的前提下提升轨迹的测量精度。
为达到上述目的，本发明第四方面实施例提出了一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的GNSS和视觉SLAM融合的轨迹测量方法。
本发明实施例的非临时性计算机可读存储介质，能够在低成本、高易用性的前提下提升轨迹的测量精度。
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1是根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法的流程图；
图2是根据本发明一个实施例的视觉跟踪、视觉建图、扩展局部优化、全局优化和回环检测线程间的关系以及时序图；
图3是根据本发明一个实施例的将GNSS信息绑定在RKF上的示意图；
图4是根据本发明一个实施例的全局优化的模型的示意图；
图5是根据本发明一个实施例显示了LM和ELBA线程中局部地图选取的示意图；
图6是根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置的方框示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
下面参考附图描述本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法和装置。
图1是根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法的流程图。如图1所示，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法，包括以下步骤：
S1，提取当前帧的特征点，并跟踪相机的位姿。
其中，特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点(即两个边缘的交点)。
S2，根据当前帧的特征点，判断当前帧是否可作为关键帧。
S3，如果当前帧可作为关键帧，则获取相机的运动轨迹。
S4，根据相机的运动轨迹，对视觉SLAM进行初始化。
S5，在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。
根据本发明的一个实施例，判断当前帧是否可作为关键帧之后，包括：将当前帧发送给视觉建图线程，以便构建局部地图；将当前帧发送给回环检测线程，以便检测相机的运动轨迹是否存在回环；如果检测到相机的运动轨迹存在回环，则进行回环连接。
具体地，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法主要分为视觉SLAM部分和信息融合部分。
其中，在视觉SLAM部分，跟踪线程先对图像提取特征，并且跟踪相机的位姿。跟踪完毕后，判断此帧是否可以作为关键帧(KF)。如果可以，则将其送入建图线程，进行局部地图构建。之后将KF送入回环检测线程，判断轨迹是否存在回环。
在信息融合部分，初始化模块对GNSS和视觉SLAM的坐标系进行对准，以方便之后二者信息的融合。如果设备运行距离满足标准，算法进行尺度初始化，用GNSS的观测确定视觉SLAM的绝对尺度。如果设备的运动轨迹在两个相互垂直的方向上的尺度都超过阈值，则算法进行位姿初始化，并将视觉SLAM坐标系(V)和世界坐标系(W)合并。
在上述任意一个初始化成功之后，开启扩展局部优化(Extended LBA，ELBA)和全局优化(Global BA，GBA)线程。在ELBA中，对与当前帧局部窗口内的几个KF及其观测点进行非线性优化，但其包含的KF数量多于局部建图时的KF数量，并且在优化时融合了GNSS的观测，因此将其称为扩展局部优化。而在GBA中，根据GNSS和视觉约束，对所有KF和地图点进行优化。ELBA线程的运算速度较快，大约在2s左右。而GBA由于要优化所有状态，其复杂度约为O(n3)，n为KF数量。本发明的GNSS和视觉SLAM融合的轨迹测量方法分别在不同线程运行ELBA和GBA，从而既可以尽快实现GNSS信息的融合，又可以在全局尺度上取得接近最优的估计结果。
本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法总共拥有5个线程：视觉跟踪(T)、视觉建图(LM)、扩展局部优化(ELBA)、全局优化(GBA)以及回环检测(LC)。各个线程间的关系以及时序如图2所示。
其中，跟踪线程对每一帧图像求解位姿，然后判断是否可以设为KF，如果可以并且LM线程空闲，则将此帧发送给LM线程。LM线程接收到KF后，构建局部地图并优化。之后，如果ELBA线程空闲，则进行ELBA。并把本次新生成的KF送给LC线程。ELBA线程在LM结束后启动，进行大的局部地图优化工作。GBA线程随时运行，它直接对整个地图进行操作，不等待其他线程的数据。LC线程接收到新的KF后开始检测回环，如果检测成功，则进行回环连接；如果LC在工作中接收到新的KF，则等其空闲时一并处理。
下面介绍坐标系选取与GNSS数据记录。
假设GNSS接收机和相机的位置一致。相机和GNSS接收机的坐标系记为C，视觉SLAM坐标系记为V，GNSS观测值所在的坐标系为大地坐标系，并将其转换到局部世界坐标系(W)中。
通常，GNSS和图像数据并非同步接收。假设GNSS的时间戳为tG，在这之前和之后采集的第i-1和i帧图像的时间戳分别为ti-1和ti。这时，在第i帧图像处加入一个KF(如果LM线程正在运行，则等它空闲了再建立KF)。把该KF设为参考帧RKF(Reference KF)，并将GNSS信息绑定在这个RKF上，如图3所示。
由于GNSS的测量时刻在的i-1和i中间，这时相机的位置在
和
之间(相机在视觉SLAM坐标系V中的位姿)。通过一阶插值，可以得到tG时刻，相机在V系中的位姿。之后，将其转换到RKF坐标系(R)下。之后，将GNSS在视觉SLAM中位置的估计值记录于KF中。
根据本发明的一个实施例，在获取到相机的运动轨迹是直线时，根据相机的运动轨迹，对视觉SLAM进行初始化，包括：如果相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点，则对视觉SLAM的尺度进行初始化。
具体地，当相机沿直线运行时，相机的绝对位姿不可观，只能根据GNSS确定其运动的绝对位置和尺度。
当相机移动了一定距离，并且接收到了足够多的GNSS观测。这时可以根据GNSS在W系中的实际测量值和其在视觉SLAM中的位置估计值，对视觉SLAM的尺度进行初始化。假设通过视觉测量，GNSS观测时刻的位置估计值为
GNSS直接测量得到的位置为
测量误差的标准差为σ＝{σ1，σ2，...，σN}(假设各轴误差一致且不相关)。那么W系相对于V系的尺度因子可以通过下述公式(1)进行估计：
其中，
为P的均值。
下面考虑执行尺度初始化的时机。假设在初始化之前，视觉SLAM的轨迹
去除了均值(即
)。并且，视觉SLAM刚开始运行，还未产生明显的积累误差，其误差可以忽略，即
轨迹在W和V系中的真值WP和VP符合SIM(3)变换，即为下述公式(2)：
GNSS的观测模型为下述公式(3)：
其中，ni为误差，其每个纬度均值为0，标准差为σi。则上述公式(1)可以写为下述公式(4)：
那么，
的方差为下述公式(5)：
经变换，得到下述公式(6)：
上述公式(6)可以根据GNSS的观测值及其噪声，大致估算尺度sWV的估计误差。只有当sWV估计误差较小时，才开始尺度的初始化。通过上述公式(6)得知，相机需要移动越20倍GNSS误差以上的距离，才可能得到小于5％的尺度估计误差。另外为了保证初始化时有足够多的观测数据，在采集20次以上的GNSS观测后，才进行初始化。
在估计了尺度变换之后，用
对整个地图进行更新。在一切更新完成后，对整个地图进行全局优化，进一步将GNSS的测量和视觉SLAM的轨迹进行匹配。之后，按照确定了尺度的状态运行。
下面先介绍确定尺度后的全局优化(GBA线程)。
尺度初始化后，相当于确定了W系和V系的尺度变换关系。对于任意两个GNSS观测，它们在视觉SLAM中的位置估计值为
GNSS实测值为
那么，虽然无法获得它们间的旋转和平移关系[RWV，WtV]，但仍然能用运动距离约束它们，因为两者间的距离为下述公式(7)：
与[RWV，WtV]无关。本发明利用所有地图点的视觉观测，以及任意两个带有GNSS的KF之间的距离作为约束，全局优化的模型如图4所示。
对于图4，待优化函数可以写成类似下述公式(8)的形式：
其中，
为待优化的参数，
为i时刻的相机位姿，pk为第k个地图点的位置，
为GNSS的权重。因为在在优化中，视觉观测的次数远远多于GNSS观测的次数，所以在该实施例中给GNSS观测分配一个更大的权值，以免其被视觉测量淹没。
其中，
代表加权二范数，
为视觉观测的信息矩阵，
为观测残差，
为地图点k在i帧图像中的位置观测值，
为根据i帧的位姿估计值
和k地图点位置估计值
对MP在图像中投影位置的观测如下述公式(10)表示：
其中，
为
对应的旋转矩阵和平移向量：
π为相机投影函数。
对
求偏导数，得到下述公式(11)：
其中，
为相机投影函数的Jacobian矩阵，用下述公式(12)表示：
那么，
的Jacobian矩阵，用下述公式(13)表示：
其中，两个非零矩阵块的列分别对应变量ξi，wpk在X中所在的行。
EG(i,j)是GNSS距离测量误差的代价(若i或j没有GNSS观测，则代价为0)，则用下述公式(14)表示：
其中，rG(i,j)为GNSS距离残差：
为GNSS噪声方差，
为载体于
时刻在视觉SLAM坐标系(V系)中的位置。它可以根据该KF的位姿估计值
和GNSS的相对于这个关键帧的位置
计算得到下述公式(15)：
对rG(i,j)求偏导数，得到下述公式(16)和(17)：
那么，EG(i,j)的Jacobian矩阵可用下述公式(18)表示：
其中，两个非零矩阵块的列分别对应变量ξi，ξj在x中所在的行。
最后，将图中所有边的残差向量和Jacobian矩阵拼在一起，并考虑各观测边的信息矩阵，得到下述公式(19)：
其中，W只有对角线上的矩阵块非零，代表着各个测量的权重，并且各个测量间不存在相关性。其中，
为视觉观测的信息矩阵，
为地图点在图像中的位置估计误差，它和特征点的尺度l有关，
是GNSS观测的信息矩阵，它和GNSS的观测误差有关。优化问题可写成类似下述公式(20)表示：
J(x)TWJ(x)Δx＝-J(x)TWr(x) (20)
由此可对整个地图里所有KF的位置和MP位置进行全局优化。
下面再介绍确定尺度后的扩展局部优化(ELBA线程)。
在完成了尺度初始化之后，开启ELBA线程。该线程和GBA所做的操作类似，只是该线程选择局部地图中的KF和MP进行优化。但是，ELBA局部地图选取范围比LM线程中的局部优化大很多(所以称为扩展局部优化)。另外，与LM线程中不同的是，ELBA中包含GNSS约束。
在LM线程中，算法选取和当前KF有观测到共同MP(有共视关系)的KF作为局部地图中待优化的KF，而将其他和待优化KF有共视关系的KF设为固定的帧。通过固定的KF，约束局部地图中的KF，并和MP共同进行优化。
在扩展局部优化中，局部地图的选取规则与LM进程中类似。只是ELBA通过多次利用共视关系，取得更多的KF，直到局部窗口中待优化的KF数量达到要求为止。之后，将与局部窗口有共视关系的KF作为固定帧，以通过视觉测量对局部窗口进行约束。此外，在优化时，还加入了GNSS观测的约束信息。图5是根据本发明一个实施例显示了LM和ELBA线程中局部地图选取的示意图。
ELBA优化的目标函数用下述公式(21)表示：
其中，各个参数与上述公式(8)中的一致，只是多了一个选择函数，如下述公式(22)表示：
其中，上述公式(21)中的参数x也比上述公式(8)中少一些，例如，如果KF不在局部地图中，则x不包含这个KF的位姿；如果观测到某个MP的KF不在局部地图中，则x中也不包含这个MP的位置。
当每次LM结束后，如果ELBA线程空闲，则进行ELBA操作。
根据本发明的一个实施例，在获取到相机运动轨迹是沿两个相互垂直方向时，根据相机的运动轨迹，进行初始化，包括：如果相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值，则对视觉SLAM的尺度进行初始化；在完成了尺度的初始化之后，对视觉SLAM的位姿进行初始化。
具体地，如果相机在任意两个正交的方向上都运动了足够的距离，则载体的绝对位姿可观。此时，经过位姿的初始化，GNSS和视觉SLAM的坐标系可以完全对准，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法工作于基于绝对位置约束的定位模式。
在完成了尺度的初始化之后，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法进行位姿初始化。
和
分别代表GNSS在V系的估计值和在W系的测量值。使用SVD方法求解W系和V系的相对位姿。首先，计算矩阵，如下述公式(23)：
对A进行SVD分解：A＝U∑VT。其中，∑的对角线元素由大到小排列，如果第二大的对角线大于某一阈值，则开始位姿的初始化。完成了上述判断和操作后，两个坐标系的相对旋转为：RWV＝UVT，相对平移
该方法不需要迭代，运算复杂度低，而且性能是最优的。
当初始的位姿关系确定后，GNSS和视觉SLAM融合的轨迹测量方法对整个地图进行变换，将W系和V系融合，均用W系表示。随后进行全局优化。
下面先介绍确定位姿后的全局优化(GBA线程)。
在位姿初始化完成后，本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法可以直接用GNSS的测量结果约束某帧在W系下的绝对位置，用下述公式(24)表示：
其中，视觉观测的代价函数
可通过上述公式(9)计算得到，GNSS测量的代价为下述公式(25)(如果这帧KF含有GNSS信息)：
上述公式(26)和上述公式(14)有一些区别：首先，GNSS的约束从两帧之间的距离变成了绝对位置的约束；其次，由于W系和V系已经合并，所以
代表了估计的GNSS的位置，可以用类似上述公式(15)的公式计算得到。那么，通过下述公式(26)对
求偏导数：
据此，本发明构造形如上述公式(20)的公式，进行全局优化。
下面再介绍确定位姿后的扩展局部优化(ELBA线程)。
在位姿初始化完成后，每次LM结束后，如果ELBA进程空闲，则进行ELBA。优化目标同位姿初始化后的GBA优化类似，只是仅优化局部地图。局部地图的选择和确定尺度后的扩展局部优化的选择类似，在此不做赘述。
综上所述，根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法，提取当前帧的特征，并跟踪相机的位姿，根据当前帧的特征判断当前帧是否可作为关键帧，如果当前帧可作为关键帧，则获取相机运动轨迹，并根据相机的运动轨迹，对视觉SLAM进行初始化，在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。由此，该方法能够在低成本、高易用性的前提下提升轨迹的测量精度。
图6是根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置的方框示意图。如图6所示，本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置，包括：提取模块61、判断模块62、获取模块63、初始化模块64和优化模块65。
其中，提取模块61，用于提取当前帧的特征，并跟踪相机的位姿；判断模块62，用于根据当前帧的特征和相机的位姿，判断当前帧是否可作为关键帧；获取模块63，用于在当前帧可作为关键帧时，获取相机运动轨迹；初始化模块64，用于根据相机的运动轨迹，对视觉SLAM进行初始化；优化模块65，用于在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。
在本发明的一个实施例中，在获取模块63获取到相机运动轨迹是沿直线运行时，初始化模块64，具体用于：在相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点时，对视觉SLAM的尺度进行初始化。
在本发明的一个实施例中，在获取模块63获取到相机运动轨迹是沿两个相互垂直方向时，所处初始化模块64，具体用于：在相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值的情况下，对视觉SLAM的尺度进行初始化；在完成了尺度的初始化之后，对视觉SLAM的位姿进行初始化。
在本发明的一个实施例中，上述的装置，还包括：建图模块，用于将当前帧发送给视觉建图线程，以便构建局部地图；检测模块，用于将当前帧发送给回环检测线程，以便检测相机的运动轨迹是否存在回环；连接模块，用于在检测到相机的运动轨迹存在回环时，进行回环连接。
需要说明的是，本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置中未披露的细节，请参考本发明实施例的GNSS和视觉SLAM融合的轨迹测量方法中所披露的细节，具体这里不再详述。
根据本发明实施例的GNSS和视觉SLAM融合的轨迹测量装置，通过提取模块提取当前帧的特征，并跟踪相机的位姿，通过判断模块根据当前帧的特征，判断当前帧是否可作为关键帧，通过获取模块在当前帧可作为关键帧时，获取相机运动轨迹，通过初始化模块根据相机的运动轨迹，对视觉SLAM进行初始化，通过优化模块在初始化完成之后，对相机的位姿进行扩展局部优化和全局优化。由此，该装置能够在低成本、高易用性的前提下提升轨迹的测量精度。
为达到上述目的，本发明第三方面实施例提出了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时，实现上述的GNSS和视觉SLAM融合的轨迹测量方法。
本发明实施例的计算机设备，能够在低成本、高易用性的前提下提升轨迹的测量精度。
为达到上述目的，本发明第四方面实施例提出了一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的GNSS和视觉SLAM融合的轨迹测量方法。
本发明实施例的非临时性计算机可读存储介质，能够在低成本、高易用性的前提下提升轨迹的测量精度。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种GNSS和视觉SLAM融合的轨迹测量方法，其特征在于，包括以下步骤：
提取当前帧的特征点，并跟踪相机的位姿；
根据所述当前帧的特征点，判断所述当前帧是否可作为关键帧；
如果所述当前帧可作为所述关键帧，则获取相机的运动轨迹；
根据所述相机的运动轨迹，对视觉SLAM进行初始化；
在所述初始化完成之后，对所述相机的位姿进行扩展局部优化和全局优化。
2.根据权利要求1所述的方法，其特征在于，在获取到所述相机的运动轨迹是直线时，所述根据所述相机的运动轨迹，对视觉SLAM进行初始化，包括：
如果所述相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点，则对视觉SLAM的尺度进行初始化。
3.根据权利要求1所述的方法，其特征在于，在获取到所述相机运动轨迹是沿两个相互垂直方向时，所述根据所述相机的运动轨迹，进行初始化，包括：
如果所述相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值，则对视觉SLAM的尺度进行初始化；
在完成了所述尺度的初始化之后，对视觉SLAM的位姿进行初始化。
4.根据权利要求1所述的方法，其特征在于，所述判断所述当前帧是否可作为关键帧之后，包括：
将所述当前帧发送给视觉建图线程，以便构建局部地图；
将所述当前帧发送给回环检测线程，以便检测所述相机的运动轨迹是否存在回环；
如果检测到所述相机的运动轨迹存在回环，则进行回环连接。
5.一种GNSS和视觉SLAM融合的轨迹测量装置，其特征在于，包括：
提取模块，用于提取当前帧的特征，并跟踪相机的位姿；
判断模块，用于根据所述当前帧的特征和所述相机的位姿，判断所述当前帧是否可作为关键帧；
获取模块，用于在所述当前帧可作为所述关键帧时，获取相机运动轨迹；
初始化模块，用于根据所述相机的运动轨迹，对视觉SLAM进行初始化；
优化模块，用于在所述初始化完成之后，对所述相机的位姿进行扩展局部优化和全局优化。
6.根据权利要求5所述的装置，其特征在于，在所述获取模块获取到所述相机运动轨迹是沿直线运行时，所述初始化模块，具体用于：
在所述相机的运动距离超过预设距离，且接收到预设数量的GNSS观测点时，对视觉SLAM的尺度进行初始化。
7.根据权利要求5所述的装置，其特征在于，在所述获取模块获取到所述相机运动轨迹是沿两个相互垂直方向时，所处初始化模块，具体用于：
在所述相机运动轨迹在两个相互垂直方向上的尺度均超过对应预设阈值的情况下，对视觉SLAM的尺度进行初始化；
在完成了所述尺度的初始化之后，对视觉SLAM的位姿进行初始化。
8.根据权利要求5所述的装置，其特征在于，还包括：
建图模块，用于将所述当前帧发送给视觉建图线程，以便构建局部地图；
检测模块，用于将所述当前帧发送给回环检测线程，以便检测所述相机的运动轨迹是否存在回环；
连接模块，用于在检测到所述相机的运动轨迹存在回环时，进行回环连接。
9.一种计算机设备，其特征在于，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时，实现如权利要求1-4中任一所述的GNSS和视觉SLAM融合的轨迹测量方法。
10.一种非临时性计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-4中任一所述的GNSS和视觉SLAM融合的轨迹测量方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110221070.XA CN112819744B (zh) | 2021-02-26 | 2021-02-26 | Gnss和视觉slam融合的轨迹测量方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110221070.XA CN112819744B (zh) | 2021-02-26 | 2021-02-26 | Gnss和视觉slam融合的轨迹测量方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112819744A true CN112819744A (zh) | 2021-05-18 |
| CN112819744B CN112819744B (zh) | 2024-05-14 |
Family
ID=75862302
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110221070.XA Active CN112819744B (zh) | 2021-02-26 | 2021-02-26 | Gnss和视觉slam融合的轨迹测量方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112819744B (zh) |
Citations (12)
| 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 |
| WO2018049581A1 (zh) * | 2016-09-14 | 2018-03-22 | 浙江大学 | 一种同时定位与地图构建方法 |
| CN109405824A (zh) * | 2018-09-05 | 2019-03-01 | 武汉契友科技股份有限公司 | 一种适用于智能网联汽车的多源感知定位系统 |
| CN109671120A (zh) * | 2018-11-08 | 2019-04-23 | 南京华捷艾米软件科技有限公司 | 一种基于轮式编码器的单目slam初始化方法及系统 |
| CN109887032A (zh) * | 2019-02-22 | 2019-06-14 | 广州小鹏汽车科技有限公司 | 一种基于单目视觉slam的车辆定位方法及系统 |
| CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| CN112268559A (zh) * | 2020-10-22 | 2021-01-26 | 中国人民解放军战略支援部队信息工程大学 | 复杂环境下融合slam技术的移动测量方法 |
| US20210042996A1 (en) * | 2019-07-02 | 2021-02-11 | Tata Consultancy Services Limited | System and method for integrating objects in monocular slam |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| CN115902978A (zh) * | 2023-01-04 | 2023-04-04 | 厦门大学 | 一种gnss/slam组合导航系统位姿初始化方法 |
-
2021
- 2021-02-26 CN CN202110221070.XA patent/CN112819744B/zh active Active
Patent Citations (12)
| 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 |
| WO2018049581A1 (zh) * | 2016-09-14 | 2018-03-22 | 浙江大学 | 一种同时定位与地图构建方法 |
| CN109405824A (zh) * | 2018-09-05 | 2019-03-01 | 武汉契友科技股份有限公司 | 一种适用于智能网联汽车的多源感知定位系统 |
| CN109671120A (zh) * | 2018-11-08 | 2019-04-23 | 南京华捷艾米软件科技有限公司 | 一种基于轮式编码器的单目slam初始化方法及系统 |
| CN109887032A (zh) * | 2019-02-22 | 2019-06-14 | 广州小鹏汽车科技有限公司 | 一种基于单目视觉slam的车辆定位方法及系统 |
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| US20210042996A1 (en) * | 2019-07-02 | 2021-02-11 | Tata Consultancy Services Limited | System and method for integrating objects in monocular slam |
| CN110501017A (zh) * | 2019-08-12 | 2019-11-26 | 华南理工大学 | 一种基于orb_slam2的移动机器人导航地图生成方法 |
| WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| CN112268559A (zh) * | 2020-10-22 | 2021-01-26 | 中国人民解放军战略支援部队信息工程大学 | 复杂环境下融合slam技术的移动测量方法 |
| CN115902978A (zh) * | 2023-01-04 | 2023-04-04 | 厦门大学 | 一种gnss/slam组合导航系统位姿初始化方法 |
Non-Patent Citations (2)
| Title |
|---|
| JINXU LIU等: "Optimization-Based Visual-Inertial SLAM Tightly Coupled with Raw GNSS Measurement", 《ARXIV:2010.11675V1》, 22 October 2020 (2020-10-22), pages 1 - 7 * |
| 张剑华;王燕燕;王曾媛;陈胜勇;管秋;: "单目同时定位与建图中的地图恢复融合技术", 中国图象图形学报, no. 03, 31 March 2018 (2018-03-31), pages 74 - 85 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112819744B (zh) | 2024-05-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及系统 | |
| JP7326720B2 (ja) | 移動体位置推定システムおよび移動体位置推定方法 | |
| JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| CN112233177B (zh) | 一种无人机位姿估计方法及系统 | |
| CN108955718A (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
| CN112115874B (zh) | 一种融合云端的视觉slam系统及方法 | |
| CN108519102B (zh) | 一种基于二次投影的双目视觉里程计算方法 | |
| CN111354043A (zh) | 一种基于多传感器融合的三维姿态估计方法及装置 | |
| US11195297B2 (en) | Method and system for visual localization based on dual dome cameras | |
| CN114037762A (zh) | 基于图像与高精度地图配准的实时高精度定位方法 | |
| CN112762929B (zh) | 一种智能导航方法、装置和设备 | |
| CN112747749B (zh) | 一种基于双目视觉和激光融合定位导航系统 | |
| CN111862146B (zh) | 一种目标对象的定位方法及装置 | |
| CN112819744B (zh) | Gnss和视觉slam融合的轨迹测量方法和装置 | |
| CN116380079A (zh) | 一种融合前视声呐与orb-slam3的水下slam方法 | |
| CN116563352A (zh) | 融合深度视觉信息的单线激光雷达回环检测方法及系统 | |
| JP5032415B2 (ja) | 運動推定装置及びプログラム | |
| CN114723811A (zh) | 一种非结构化环境四足机器人立体视觉定位建图方法 | |
| CN114429515A (zh) | 一种点云地图构建方法、装置和设备 | |
| CN112001970A (zh) | 一种基于点线特征的单目视觉里程计方法 | |
| CN116934829B (zh) | 无人机目标深度估计的方法、装置、存储介质及电子设备 | |
| CN117576218B (zh) | 一种自适应的视觉惯导里程计输出方法 | |
| CN115700507B (zh) | 地图更新方法及其装置 | |
| KR102421067B1 (ko) | 카메라 자세와 대상 물체의 3d 좌표를 동시에 추정하는 동시 추정 장치 및 방법 | |
| KR20240062709A (ko) | 차량에 탑재된 카메라 교정 방법 및 이를 위한 탈부착 교정 장치 |
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 |