[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN112541423A - 一种同步定位与地图构建方法和系统 - Google Patents

一种同步定位与地图构建方法和系统 Download PDF

Info

Publication number
CN112541423A
CN112541423A CN202011427763.6A CN202011427763A CN112541423A CN 112541423 A CN112541423 A CN 112541423A CN 202011427763 A CN202011427763 A CN 202011427763A CN 112541423 A CN112541423 A CN 112541423A
Authority
CN
China
Prior art keywords
frame images
adjacent frame
matrix
points
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.)
Pending
Application number
CN202011427763.6A
Other languages
English (en)
Inventor
刘书林
王雨桐
徐彬
刘春桃
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Chongqing Innovation Center of Beijing University of Technology
Original Assignee
Beijing Institute of Technology BIT
Chongqing Innovation Center of Beijing University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT, Chongqing Innovation Center of Beijing University of Technology filed Critical Beijing Institute of Technology BIT
Priority to CN202011427763.6A priority Critical patent/CN112541423A/zh
Publication of CN112541423A publication Critical patent/CN112541423A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]
    • G06V10/464—Salient features, e.g. scale invariant feature transforms [SIFT] using a plurality of salient features, e.g. bag-of-words [BoW] representations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/757—Matching configurations of points or features

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种同步定位与地图构建方法和系统。本发明利用特征点法初步求取两帧间的位姿变换关系，然后将它作为直接法优化的初始值。本发明检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；根据相邻帧图像间特征点的对应关系，计算出本质矩阵；根据所述本质矩阵计算出相邻帧图像的位姿变化关系；获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，对像素点重投影，优化位姿变化关系的光度误差；根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。本发明可以解决非线性优化中容易陷入局部最优值的问题，使系统在特征点不足或光照不稳定的应用场景中能够有较好的稳定性。

Description

一种同步定位与地图构建方法和系统
技术领域
本发明涉及无人机定位与地图构建领域，尤其是一种同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)方法和系统。
背景技术
未知环境下同步定位与建图，是一个智能移动设备实现自主移动的基础关键能力，也只有在定位的前提下，才能进一步导航规划，控制底层驱动实现动力系统的类人行走，由此才能进行其他的工作。传统的视觉SLAM多基于特征点或者直接比较相邻帧之间的像素差异。
特征点法需要依赖重复性较强的特征提取器，关键点提取、描述子、匹配耗时长，需要正确的特征匹配，才能正确地计算相机运动。在纹理缺失的环境中，特征点容易丢失，导致算法失效。
直接法直接比较相邻帧之间的像素差异，而不对像素本身进行描述，所以需要去除因为光线对像素灰度的影响。直接法的理论基础是灰度一致性假设，即假设相机运动的同时，相邻帧的图像像素信息在完全相同的光线条件下得到的。灰度不变假设设定同一个空间点的像素灰度在各个图像中是保持固定。灰度不变假设是一个很强的假设，在移动机器人实际的运动中，满足此假设的对环境条件的要求很苛刻而很难满足。而且由于物体的材质不同，像素会在像素平面中出现高光和阴影部分。当相机自动调整曝光参数时，图像整体的亮度自动变化。此时灰度不变假设都是不成立的。
由此可知，传统的视觉SLAM方法存在以下不足：
1、特征点信息利用率低，不足以满足稳定的图像匹配跟踪。
2、基于特征点的SLAM算法需要环境中有丰富的点特征。
3、基于直接法的SLAM算法基于灰度不变假设，是一种强假设,易受光照变化影响。
发明内容
本发明的发明目的在于：针对上述存在的问题，提供一种同步定位与地图构建方法，以在在快速运动情况下需求时不再有严重的特征丢失，而且降低对灰度不变假设的依赖，在纹理不足的场景也有较好的稳定性。
本发明采用的技术方案如下：
一种同步定位与地图构建方法，包括：
检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；根据相邻帧图像间特征点的对应关系，计算出本质矩阵；根据所述本质矩阵计算出相邻帧图像的位姿变化关系；获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上；优化位姿变化关系的光度误差；根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
上述方法把特征点检测和匹配与相邻帧图像之间的像素梯度信息联系起来，即便于相邻关键帧的信息提取与匹配，又具备对光度误差计算的简单便捷特点，对特征点依赖低，不易丢帧，对灰度不变性依赖低。
进一步的，所述根据相邻帧图像间特征点的对应关系，计算出本质矩阵，包括：根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。
进一步的，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵采用五点算法计算。
进一步的，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵，包括：采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。
进一步的，方法还包括：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值，则重新对相邻帧图像的ORB特征进行检测。
进一步的，所述对相邻帧图像间的ORB特征进行匹配，包括：
计算图像函数f(x,y)在像素点(x,y)的梯度：
Figure BDA0002825571510000031
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure BDA0002825571510000032
其中，Gx和Gy分别表示x方向和y方向的梯度；
在梯度方向和梯度幅值偏差第二阈值以内时，则判定对应的像素点匹配成功。对像素块的像素梯度计算，提高了图像的像素信息，降低对灰度不变假设的依赖,减小光照变化影响，减少对环境中的特征点的依赖，有效降低了在欠点特征环境中视觉前端的丢帧率。
进一步的，在将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上时，若ORB特征匹配失败，则将所述位姿变化关系初始化为单位矩阵。
进一步的，所述优化位姿变化关系的光度误差，包括：
Figure BDA0002825571510000033
其中，Tk,k-1为位姿变化关系，δI(Tk,k-1,ui)为像素投影后的光度差，u为像素点坐标，I为灰度值。
进一步的，在得到定位轨迹之后，所述的同步定位与地图构建方法还包括：利用词袋模型进行回环检测；将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
进一步的，在检测相邻帧图像的ORB特征前，先对相邻帧图像进行去畸变处理。
为解决上述全部或部分问题，本发明还提供了一种同步定位与地图构建系统，包括：特征检测与跟踪单元、本质矩阵估计单元、位姿变化关系计算单元、重投影单元和建图单元，其中：
所述特征检测与评估单元被配置为：检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；
所述本质矩阵估计单元被配置为：根据相邻帧图像间特征点的对应关系，计算出本质矩阵；
所述位姿变化关系计算单元被配置为：根据所述本质矩阵计算出相邻帧图像的位姿变化关系；
所述重投影单元被配置为：根据相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系计算单元计算出的位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上，优化位姿变化关系的光度误差；
所述建图单元被配置为：根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
进一步的，所述本质矩阵估计单元，根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。
进一步的，所述本质矩阵估计单元被配置采用五步法计算所述若干组相邻帧图像间的本质矩阵。
进一步的，所述本质矩阵估计单元，采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。
进一步的，所述特征检测与跟踪单元包括特征重检测模块，所述特征重检测模块被配置为：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值，则重新对相邻帧图像的ORB特征进行检测。
进一步的，所述特征检测与跟踪单元包括特征匹配模块，所述特征匹配模块被配置为：对相邻帧图像的ORB特征进行匹配，包括：
计算图像函数f(x,y)在像素点(x,y)的梯度：
Figure BDA0002825571510000051
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure BDA0002825571510000052
其中，Gx和Gy分别表示x方向和y方向的梯度；
在梯度方向和梯度幅值偏差第二阈值以内时，则判定对应的像素点匹配成功。
进一步的，所述重投影单元在将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上时，若ORB特征匹配失败，则将所述位姿变化关系初始化为单位矩阵。
进一步的，所述重投影单元包括误差优化模块，所述误差优化模块被配置为：优化位姿变化关系的光度误差，包括：
Figure BDA0002825571510000053
其中，Tk,k-1为位姿变化关系，δI(Tk,k-1,ui)为像素投影后的光度差，u为像素点坐标，I为灰度值。
进一步的，同步定位与地图构建系统还包括回环检测与优化单元，所述回环检测与优化单元被配置为：利用词袋模型进行回环检测；将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
进一步的，同步定位与地图构建系统还包括预处理单元，所述预处理单元的输出端连接所述特征检测与跟踪单元的输入端；所述预处理单元被配置为：对相邻帧图像进行去畸变处理。。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1、本发明融合了特征点法和直接法的特点，利用特征点法初步求取两帧间的位姿变换关系，然后将它作为直接法优化的初始值，解决非线性优化中容易陷入局部最优值的问题。使系统在特征点不足或光照不稳定的应用场景中能够有较好的稳定性。
2、本发明将特征点检测和匹配与相邻帧图像之间的像素梯度信息联系起来，使在快速运动情况下不再有严重的特征丢失。
3、本发明有效降低了在欠点特征环境中视觉前端的丢帧率，提高定位算法的稳定性。
4、本发明对像素块的像素梯度计算，提高了图像的像素信息，降低对灰度不变假设的依赖,减小光照变化影响。
附图说明
本发明将通过例子并参照附图的方式说明，其中：
图1是SLAM算法框架示意图。
图2是同步定位与地图构建方法计算流程图。
图3是特征点重投影示图。
图4是同步定位与地图构建系统构造图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
本说明书(包括任何附加权利要求、摘要)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。
实施例一
无论是单目还是双目相机的视觉传感器，在SLAM中它们使用的整体框架大致相同，只是由于获取数据方式的不同，导致前端数据处理方式有不同的设计。如图1所示，SLAM算法框架大致分为传感器数据处理、前端视觉里程计、后端滤波与优化、回环检测和建图五大模块。其中，视觉里程计是传感器数据输入后，进行初步姿态估计的一个重要模块，估计相邻图像之间的运动位姿变化，还原出局部环境信息。
针对动态场景下，特征点利用率低，且不足以满足稳定的图像匹配跟踪的问题，参见附图2，本实施例公开了一种同步定位与地图构建(SLAM)方法，包括：
A.检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配。
相邻帧图像即顺序上连续的相邻两帧图像，顺序靠前的为第一帧图像，靠后的为第二帧图像，因此，此处的“第一”、“第二”并非特指，仅为顺序上的区分。图像的ORB特征采用FAST特征点检测算法进行检测，ORB特征检测有两个步骤：检测FAST角点，并计算了角点的主方向，构建高斯金字塔，在每一层金字塔图像上检测角点；BRIEF描述子计算，对提取出的角点周围区域进行描述。BRIEF是一种二进制描述子，其描述向量由多个0和1组成，这里0和1编码了关键点附近两个随机选取的像素值(比如p和q)的大小关系：如果p比q大，则取1，反之就取0。在一些实施例中，定义在特征点FAST周围随机选取128个p和q点对，得到128维由0、1组成的向量对ORB特征点进行组合描述。计算FAST特征点的方向，使得BRIEF描述子具有旋转不变性，提高特征检测鲁棒性。
对于特征的跟踪，本实施例中采用像素块的梯度来作为跟踪特征，这样可以提高图像的像素信息，减少对环境中的特征点的依赖，降低对灰度不变假设的依赖，减小光照变化的影响。
定义(x,y)为图像函数f(x,y)上的点，图像函数f(x,y)在点(x,y)的梯度是一个具有大小和方向的矢量，因此，本实施例用梯度来作为关键字匹配后的信息跟踪。设Gx和Gy分别表示x方向和y方向的梯度，这个梯度的矢量可以表示为：
Figure BDA0002825571510000081
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure BDA0002825571510000082
对应的FAST角点检测匹配成功的基础上，计算上述式中的像素梯度方向和梯度幅值大小，在偏差一定范围(如30％)以内，即可认定为是跟踪成功的像素点。
进一步的，在进行像素梯度跟踪时，由于视野在每个时刻的变化不同，如某些环境信息偏移出镜头视野太多，最终会丢失一些点，因此，在对图像检车出的特征点总数低于某个阈值(如300)，则触发重新检测图像的特征。
B.根据相邻帧图像间特征点的对应关系，计算出本质矩阵。
本实施例根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。在一些实施例中，采用五点算法计算若干组相邻帧图像间的本质矩阵。采用五点算法的计算过程包括：采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵。则上述计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵，包括：采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。之所以需要先计算若干组本质矩阵，是因为在像素点的匹配和跟踪过程中并不是所有的点对应关系都是完美的，特征点的跟踪并不完美，特征点不可避免地会出现错误的对应关系，此类错误的对应关系即为异常值，采用随机采样一致性计算多组本质矩阵，进一步确定最终使用的本质矩阵，可以剔除异常值。
C.根据所述本质矩阵计算出相邻帧图像的位姿变化关系。
对于本质矩阵E,通过SVD分解，可以计算出第二幅图像的位姿变化关系[R|t]。
E＝U∑VT
R＝UW-1VT
t^＝VW∑VT
其中,t^为向量t的反对称矩阵，
Figure BDA0002825571510000101
D.获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上；优化位姿变化关系的光度误差。
步骤A-C得到相邻帧之间的位姿变化关系[R|t]，将其作为后续处理的初始值：Tk,k-1＝[R|t]。若上述特征跟踪失败，则将Tk,k-1初始化为单位矩阵。即利用特征点法初步求取两帧间的位姿变换关系，然后将直接法优化的初始值，解决非线性优化中容易陷入局部最优值的问题。
如图3所示，知道第一像素平面Ik-1中像素位置为(u,v)以及深度d,利用位姿变化关系Tk,k-1，获得该点在当前帧的三维坐标pk。通过相机内参，投影到的第二像素平面Ik图像平面(u′,v′)，完成重投影。短时间内相邻帧特征点的亮度值不变，得到该三维点在周围的像素梯度光度误差。不断优化位姿使得残差最小：
Figure BDA0002825571510000102
E.根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
通过前面步骤可以得到两帧计算的相对位置关系，即旋转Rk,k-1和平移tk,k-1，因为第二幅图像对应相机矩阵是在假设第一幅图像的相机矩阵为零的情况，因此我们对每次计算的相对量进行累加即可得到定位轨迹:
Rk＝Rk-1Rk,k-1
tk＝tk-1+tk-1,Rk,k-1。
在同一个位置的新采集到的数据与历史数据前后产生关联，并依靠这种联系建立位姿之间的约束关系，从而在优化系统中消除位姿约束，进一步提高定位精度，消除长时间运行时大范围的累计误差。优选的，方法还包括：利用词袋模型进行回环检测；将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
本发明方法不同于传统的特征点法或直接法，可以看作半特征点法和半直接法的结合，在方法的整体设计中开创性地融合了两种方法的优势，但又与纯单一算法相区别，本发明方法把特征点检测和匹配与相邻帧图像之间的像素梯度信息联系起来，即利用特征点的特殊性优势便于相邻关键帧的信息提取和匹配，也利用直接法中光度误差(PhotometricError)计算的简单快捷，而非将传统的特征点法和直接法的简单结合。
实施例二
参见附图2，本实施例公开了一种同步定位与地图构建(SLAM)方法，本实施例的设计思路为：在实时的视频流进入系统后，对相隔两帧进行去畸变处理，使用FAST特征检测算法做初步角点检测，通过RANSAC算法进行特征提纯去除噪点，然后根据计算得到的特征点周围的像素梯度，做进一步配准，最小化光度误差。利用高斯牛顿法迭代下降，来获取当前位姿的粗估计。前端利用传感器进行数据关联和闭环检测完成图模型的构建，前端视觉里程计把传感器数据组织成图模型后，需要在后端利用图论对图模型进行优化。
本实施例的同步定位与地图构建方法包括：
对视频流中的相邻帧图像进行预处理。进行预处理的目的是去除帧图像对特征检测的影响。通常来讲，预处理的步骤包括去畸变处理、去噪处理等。
1)对预处理后的帧图像提取FAST特征点。对帧图像的ORB特征检测包括两个步骤：检测FAST角点，并计算角点的主方向，构建高斯金字塔，在每一层金字塔图像上检测角点；BRIEF描述子计算，对提取出的角点周围区域进行描述。BRIEF是一种二进制描述子，其描述向量由多个0和1组成，这里0和1编码了关键点附近两个随机选取的像素值(比如p和q)的大小关系：如果p比q大，则取1，反之就取0。定义在FAST特征点周围随机选取128个p和q点对，得到128维由0、1组成的向量对ORB特征点进行组合描述。计算FAST特征点的方向，使得BRIEF描述子具有旋转不变性，提高特征检测鲁棒性。
2)对相邻帧图像进行特征跟踪。图像函数f(x,y)在点(x,y)的梯度是一个具有大小和方向的矢量，用来作为关键字匹配后的信息跟踪，在像素中便于快速计算。设Gx和Gy分别表示x方向和y方向的梯度，这个梯度的矢量可以表示为：
Figure BDA0002825571510000121
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure BDA0002825571510000122
对应的FAST角点检测匹配成功的基础上，计算上述式中的像素梯度方向和梯度幅值大小，在偏差一定阈值范围(如30％)以内，即可认定为是跟踪成功的像素点。本发明对像素块的像素梯度计算，提高了图像的像素信息，降低对灰度不变假设的依赖，减小光照变化影响，同时也减少了对环境中特征点的依赖，有效降低了在欠点特征环境中视觉前端的丢帧率。
3)特征重检测。在进行特征点跟踪时，由于视野在每个时刻的变化不同，如某些环境信息偏移出镜头视野太多，最终会丢失一些点，因此，只要跟踪到的特征总数低于某个阈值如300，系统将触发重新检测。
4)本质矩阵评估。采用五点算法求解本质矩阵。已知摄像机矩阵P＝K[R|t]，K为相机内参变换矩阵，令x为图像上一点，X为该点对应的世界坐标，则x＝PX，令
Figure BDA0002825571510000131
即
Figure BDA0002825571510000132
为归一化图像坐标。两帧图像的对应点
Figure BDA0002825571510000133
对应的归一化图像坐标与本质矩阵E满足
Figure BDA0002825571510000134
选取多点，求解对应非线性方程组则可求解出本质矩阵E。由于基本矩阵只有五个自由度，采用五点算法，来解决此非线性问题，需要的点数最少。
5)剔除噪点。有本质矩阵，我们只需要两个连续帧图像之间的五个特征对应关系即可准确地估计运动。但是在像素点的匹配和跟踪过程中并不是所有的点对应关系都是完美的，特征跟踪算法不可避免的有几个错误的对应关系，在进行运动估计时，有必要剔除此类异常值。在一个实施例中，采用RANSAC(随机采样一致性)算法进行噪点剔除计算。在每次迭代中，从一组对应关系中随机采样五个点，估计基本矩阵，然后在使用此基本矩阵时检查其他点是否为内点。在进行预订次数的迭代后，选用最大内点数时的基本矩阵。
6)从本质矩阵求解出旋转平移信息。对于本质矩阵E,通过SVD分解，可以计算出第二幅帧图像的摄像机外参矩阵[R|t]。
E＝U∑VT
R＝UW-1VT
t^＝VW∑VT
其中,t^为向量t的反对称矩阵，
Figure BDA0002825571510000141
7)重投影并优化光度误差。
上述步骤1)～6)计算得到相邻帧图像之间的位姿关系[R|t]，并将其作为直接法的初始值：Tk,k-1＝[R|t]。如果特征法跟踪失败，则将Tk,k-1初始化为单位矩阵。知道图像平面Ik-1中某点(三维点)像素位置为(u,v)以及深度d，利用位姿Tk,k-1，获得该点在当前帧的三维坐标pk，通过相机内参，投影到的图像平面Ik中为(u′,v′)，完成重投影，如图3所示。短时间内相邻帧图像特征点的亮度值不变，得到该三维点在周围的像素梯度光度误差。不断优化位姿使得残差最小：
Figure BDA0002825571510000142
8)通过前述步骤可以计算得到两帧图像的相对位置关系，即旋转关系Rk,k-1和平移关系tk,k-1。因为第二幅帧图像对应相机矩阵是在假设第一幅帧图像的相机矩阵为已知的情况，因此，我们对每次计算的相对量进行累加即可得到:
Rk＝Rk-1Rk,k-1
tk＝tk-1+tk-1,Rk,k-1
9)利用词袋模型，进行回环检测。将特征训练成词袋，然后用词袋进行对比，进而可以提高回环检测的效率。在同一个位置的新采集到的数据与历史数据前后产生关联，并依靠这种联系建立位姿之间的约束关系，从而在优化系统中消除位姿约束，进一步提高定位精度，消除长时间运行时大范围的累计误差。
10)创建因子图模型并进行优化计算。将每一组获得的观测值均加入到因子图模型中，每添加一组观测值，对新观测值在因子图模型中添加与历史信息的约束关系，并设定初始化值，然后在当前已有的位姿信息矩阵中进行线性化，随后进QR分解信息矩阵，最后利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
实施例三
本实施例公开了一种同步定位与地图构建系统，如图4所示，包括预处理单元、特征检测与跟踪单元、本质矩阵估计单元、位姿变化关系计算单元、重投影单元和建图单元，其中：
预处理单元被配置为：对相邻帧图像进行去畸变处理。预处理单元的输出端连接特征检测与跟踪单元的输入端，特征检测与跟踪单元处理的原始文件则为预处理单元去畸变后的结果。
特征检测与跟踪单元被配置为：检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配。
特征检测与跟踪单元包括特征检测模块和特征匹配模块。特征检测模块被配置为检测帧图像的ORB特征，由于涉及相邻两帧间的关系计算，特征检测模块检测相邻帧图像的ORB特征。ORB特征检测有两个步骤：检测FAST角点，并计算了角点的主方向，构建高斯金字塔，在每一层金字塔图像上检测角点；BRIEF描述子计算，对提取出的角点周围区域进行描述。BRIEF是一种二进制描述子，其描述向量由多个0和1组成，这里0和1编码了关键点附近两个随机选取的像素值(比如p和q)的大小关系：如果p比q大，则取1，反之就取0。在一些实施例中，定义在特征点FAST周围随机选取128个p和q点对，得到128维由0、1组成的向量对ORB特征点进行组合描述。计算FAST特征点的方向，使得BRIEF描述子具有旋转不变性，提高特征检测鲁棒性。
特征匹配模块被配置为：对相邻帧图像的ORB特征进行匹配，包括：
计算图像函数f(x,y)在像素点(x,y)的梯度：
Figure BDA0002825571510000161
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure BDA0002825571510000162
其中，Gx和Gy分别表示x方向和y方向的梯度；
在梯度方向和梯度幅值偏差第二阈值以内时，则判定对应的像素点匹配成功。
进一步的，特征检测与跟踪单元还包括特征重检测模块，特征重检测模块被配置为：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值(如采用比例作为阈值，例如取30％)，则重新对相邻帧图像的ORB特征进行检测。
本质矩阵估计单元被配置为：根据相邻帧图像间特征点的对应关系，计算出本质矩阵。具体的，为降低特征匹配算法不完美导致的匹配误差，本质矩阵估计单元根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。
在一些实施例中，由于特征跟踪算法的不完美性，为避免特征匹配错误对特征跟踪的影响，所述本质矩阵估计单元，采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。
位姿变化关系计算单元被配置为：根据本质矩阵计算出相邻帧图像的位姿变化关系。
对于本质矩阵E,通过SVD分解，可以计算出第二幅图像的摄像机矩阵[R|t]。
E＝U∑VT
R＝UW-1VT
t^＝VW∑VT
其中,t^为向量t的反对称矩阵，
Figure BDA0002825571510000171
重投影单元被配置为：根据相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系计算单元计算出的位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上，优化位姿变化关系的光度误差。
重投影单元在将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上时，若ORB特征匹配失败，则将所述位姿变化关系初始化为单位矩阵。
重投影单元包括误差优化模块，所述误差优化模块被配置为：优化位姿变化关系的光度误差，包括：
Figure BDA0002825571510000172
其中，Tk,k-1为位姿变化关系，δI(Tk,k-1,ui)为像素投影后的光度差，u为像素点在x轴方向的坐标，I为灰度值。
知道图像平面Ik-1中某点(三维点)像素位置为(u,v)以及深度d，利用位姿Tk,k-1，获得该点在当前帧的三维坐标pk，通过相机内参，投影到的图像平面Ik中为(u′,′)，完成重投影，如图3所示。短时间内相邻帧特征点的亮度值不变，得到该三维点在周围的像素梯度光度误差。不断优化位姿使得残差最小。
建图单元被配置为：根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
前面的各单元可以得到两帧计算的相对位置关系，即旋转Rk,k-1和平移kk,k-1，因为第二幅图像对应相机矩阵是在假设第一幅图像的相机矩阵为零的情况，因此我们对每次计算的相对量进行累加即可:
Rk＝Rk-1Rk,k-1
tk＝tk-1+tk-1,Rk,k-1。
在一些实施例中，同步定位与地图构建系统还包括回环检测与优化单元，该回环检测与优化单元被配置为：利用词袋模型进行回环检测；将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。

Claims (12)

1.一种同步定位与地图构建方法，其特征在于，包括：
检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；
根据相邻帧图像间特征点的对应关系，计算出本质矩阵；
根据所述本质矩阵计算出相邻帧图像的位姿变化关系；
获取相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上；优化位姿变化关系的光度误差；
根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
2.如权利要求1所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算出本质矩阵，包括：
根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵。
3.如权利要求2所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵采用五点算法计算。
4.如权利要求3所述的同步定位与地图构建方法，其特征在于，所述根据相邻帧图像间特征点的对应关系，计算若干组相邻帧图像间的本质矩阵，从若干组所述本质矩阵中确定出使用的本质矩阵，包括：
采用RANSAC算法进行预订次数的迭代，每次迭代中，从一组对应关系中随机采样五个点，计算出对应的本质矩阵，检查随机采样以外的像素点是否为内点；
选择各次迭代中，与检查出的最大内点数对应的本质矩阵作为使用的本质矩阵。
5.如权利要求1所述的同步定位与地图构建方法，其特征在于，还包括：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值，则重新对相邻帧图像的ORB特征进行检测。
6.如权利要求1或5所述的同步定位与地图构建方法，其特征在于，所述对相邻帧图像间的ORB特征进行匹配，包括：
计算图像函数f(x,y)在像素点(x,y)的梯度：
Figure FDA0002825571500000021
梯度方向为：
Φ(x,y)＝tan-1(Gx/Gy)
梯度幅值为：
Figure FDA0002825571500000022
其中，Gx和Gy分别表示x方向和y方向的梯度；
在梯度方向和梯度幅值偏差第二阈值以内时，则判定对应的像素点匹配成功。
7.如权利要求1所述的同步定位与地图构建方法，其特征在于，在将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上时，若ORB特征匹配失败，则将所述位姿变化关系初始化为单位矩阵。
8.如权利要求1所述的同步定位与地图构建方法，其特征在于，所述优化位姿变化关系的光度误差，包括：
Figure FDA0002825571500000023
其中，Tk,k-1为位姿变化关系，δI(Tk,k-1,ui)为像素投影后的光度差，u为像素点坐标，I为灰度值。
9.如权利要求1所述的同步定位与地图构建方法，其特征在于，在得到定位轨迹之后，所述的同步定位与地图构建方法还包括：
利用词袋模型进行回环检测；
将每一组新输入的帧图像均加入到因子图模型中，每添加一组帧图像，均添加对应的位姿约束关系，并设定初始化值，构成位姿信息矩阵；
在当前已有的位姿信息矩阵中进行线性化，QR分解位姿信息矩阵，利用因子图的稀疏化处理方式，去掉因子节点之间的关系，得到更新后的位姿信息。
10.如权利要求1所述的同步定位与地图构建方法，其特征在于，在检测相邻帧图像的ORB特征前，先对相邻帧图像进行去畸变处理。
11.一种同步定位与地图构建系统，其特征在于，包括：特征检测与跟踪单元、本质矩阵估计单元、位姿变化关系计算单元、重投影单元和建图单元，其中：
所述特征检测与评估单元被配置为：检测相邻帧图像的ORB特征，对相邻帧图像的ORB特征进行匹配；
所述本质矩阵估计单元被配置为：根据相邻帧图像间特征点的对应关系，计算出本质矩阵；
所述位姿变化关系计算单元被配置为：根据所述本质矩阵计算出相邻帧图像的位姿变化关系；
所述重投影单元被配置为：根据相邻帧图像中第一帧图像上像素点的三维坐标，利用所述位姿变化关系计算单元计算出的位姿变化关系，将第一帧图像对应图像平面上的像素点重投影到相邻帧图像中第二帧图像对应的图像平面上，优化位姿变化关系的光度误差；
所述建图单元被配置为：根据每两帧相邻帧图像间的位姿变化关系，将每两帧相邻帧图像间的相对量进行累加，得到定位轨迹。
12.如权利要求11所述的同步定位与地图构建系统，其特征在于，所述特征检测与跟踪单元包括特征重检测模块，所述特征重检测模块被配置为：在对相邻帧图像的ORB特征进行匹配过程中，若匹配的特征点总数或比例低于第一阈值，则重新对相邻帧图像的ORB特征进行检测。
CN202011427763.6A 2020-12-09 2020-12-09 一种同步定位与地图构建方法和系统 Pending CN112541423A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011427763.6A CN112541423A (zh) 2020-12-09 2020-12-09 一种同步定位与地图构建方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011427763.6A CN112541423A (zh) 2020-12-09 2020-12-09 一种同步定位与地图构建方法和系统

Publications (1)

Publication Number Publication Date
CN112541423A true CN112541423A (zh) 2021-03-23

Family

ID=75019644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011427763.6A Pending CN112541423A (zh) 2020-12-09 2020-12-09 一种同步定位与地图构建方法和系统

Country Status (1)

Country Link
CN (1) CN112541423A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112801077A (zh) * 2021-04-15 2021-05-14 智道网联科技(北京)有限公司 用于自动驾驶车辆的slam初始化的方法及相关装置
CN113284176A (zh) * 2021-06-04 2021-08-20 深圳积木易搭科技技术有限公司 一种结合几何和纹理的在线匹配优化方法和三维扫描系统
CN113808169A (zh) * 2021-09-18 2021-12-17 南京航空航天大学 基于orb-slam的大型装备结构表面检测路径规划方法
CN114972514A (zh) * 2022-05-30 2022-08-30 歌尔股份有限公司 Slam定位方法、装置、电子设备和可读存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN107481315A (zh) * 2017-06-29 2017-12-15 重庆邮电大学 一种基于Harris‑SIFT‑BRIEF算法的单目视觉三维环境重建方法
CN108537848A (zh) * 2018-04-19 2018-09-14 北京工业大学 一种面向室内场景重建的两级位姿优化估计方法
CN109509211A (zh) * 2018-09-28 2019-03-22 北京大学 同时定位与建图技术中的特征点提取与匹配方法及系统
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN111461998A (zh) * 2020-03-11 2020-07-28 中国科学院深圳先进技术研究院 一种环境重建方法及装置
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105856230A (zh) * 2016-05-06 2016-08-17 简燕梅 一种可提高机器人位姿一致性的orb关键帧闭环检测slam方法
CN107481315A (zh) * 2017-06-29 2017-12-15 重庆邮电大学 一种基于Harris‑SIFT‑BRIEF算法的单目视觉三维环境重建方法
CN108537848A (zh) * 2018-04-19 2018-09-14 北京工业大学 一种面向室内场景重建的两级位姿优化估计方法
CN109509211A (zh) * 2018-09-28 2019-03-22 北京大学 同时定位与建图技术中的特征点提取与匹配方法及系统
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN111461998A (zh) * 2020-03-11 2020-07-28 中国科学院深圳先进技术研究院 一种环境重建方法及装置
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建系统及方法

Non-Patent Citations (12)

* Cited by examiner, † Cited by third party
Title
任桢: "图优化的移动机器人SLAM算法研究", 《中国优秀硕士学位论文全文数据库》 *
任桢: "图优化的移动机器人SLAM算法研究", 《中国优秀硕士学位论文全文数据库》, 15 September 2019 (2019-09-15), pages 26 *
李同等: "基于ORB词袋模型的SLAM回环检测研究", 《信息通信》 *
李同等: "基于ORB词袋模型的SLAM回环检测研究", 《信息通信》, no. 10, 15 October 2017 (2017-10-15) *
梁明杰等: "基于图优化的同时定位与地图创建综述", 《机器人》 *
梁明杰等: "基于图优化的同时定位与地图创建综述", 《机器人》, no. 04, 15 July 2013 (2013-07-15) *
段震灏等: "基于GC-RANSAC算法的单目视觉同时定位与地图构建", 《长春理工大学学报(自然科学版)》 *
段震灏等: "基于GC-RANSAC算法的单目视觉同时定位与地图构建", 《长春理工大学学报(自然科学版)》, no. 01, 15 February 2020 (2020-02-15) *
艾青林等: "基于ORB关键帧匹配算法的机器人SLAM实现", 《机电工程》 *
艾青林等: "基于ORB关键帧匹配算法的机器人SLAM实现", 《机电工程》, vol. 3, no. 05, 20 May 2016 (2016-05-20), pages 3 - 4 *
蒋林等: "一种点线特征融合的双目同时定位与地图构建方法", 《科学技术与工程》 *
蒋林等: "一种点线特征融合的双目同时定位与地图构建方法", 《科学技术与工程》, no. 12, 28 April 2020 (2020-04-28) *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112801077A (zh) * 2021-04-15 2021-05-14 智道网联科技(北京)有限公司 用于自动驾驶车辆的slam初始化的方法及相关装置
CN113284176A (zh) * 2021-06-04 2021-08-20 深圳积木易搭科技技术有限公司 一种结合几何和纹理的在线匹配优化方法和三维扫描系统
CN113284176B (zh) * 2021-06-04 2022-08-16 深圳积木易搭科技技术有限公司 一种结合几何和纹理的在线匹配优化方法和三维扫描系统
CN113808169A (zh) * 2021-09-18 2021-12-17 南京航空航天大学 基于orb-slam的大型装备结构表面检测路径规划方法
CN113808169B (zh) * 2021-09-18 2024-04-26 南京航空航天大学 基于orb-slam的大型装备结构表面检测路径规划方法
CN114972514A (zh) * 2022-05-30 2022-08-30 歌尔股份有限公司 Slam定位方法、装置、电子设备和可读存储介质
CN114972514B (zh) * 2022-05-30 2024-07-02 歌尔股份有限公司 Slam定位方法、装置、电子设备和可读存储介质

Similar Documents

Publication Publication Date Title
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
Kang et al. Detection and tracking of moving objects from a moving platform in presence of strong parallax
CN112541423A (zh) 一种同步定位与地图构建方法和系统
CN111258313A (zh) 多传感器融合slam系统及机器人
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN111462207A (zh) 一种融合直接法与特征法的rgb-d同时定位与地图创建方法
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及系统
CN108648194B (zh) 基于cad模型三维目标识别分割和位姿测量方法及装置
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN110570474B (zh) 一种深度相机的位姿估计方法及系统
CN111882602B (zh) 基于orb特征点和gms匹配过滤器的视觉里程计实现方法
CN111998862B (zh) 一种基于bnn的稠密双目slam方法
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN116128966A (zh) 一种基于环境物体的语义定位方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN113744315A (zh) 一种基于双目视觉的半直接视觉里程计
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法
Wu et al. Self-supervised monocular depth estimation scale recovery using ransac outlier removal
Richardson et al. PAS: visual odometry with perspective alignment search
CN117671022B (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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210323

RJ01 Rejection of invention patent application after publication