CN113378746B - 一种定位方法及装置 - Google Patents

一种定位方法及装置 Download PDF

Info

Publication number
CN113378746B
CN113378746B CN202110688891.4A CN202110688891A CN113378746B CN 113378746 B CN113378746 B CN 113378746B CN 202110688891 A CN202110688891 A CN 202110688891A CN 113378746 B CN113378746 B CN 113378746B
Authority
CN
China
Prior art keywords
static
feature points
frame image
feature
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.)
Active
Application number
CN202110688891.4A
Other languages
English (en)
Other versions
CN113378746A (zh
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.)
University of Science and Technology of China USTC
Original Assignee
University of Science and Technology of China USTC
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 University of Science and Technology of China USTC filed Critical University of Science and Technology of China USTC
Priority to CN202110688891.4A priority Critical patent/CN113378746B/zh
Publication of CN113378746A publication Critical patent/CN113378746A/zh
Application granted granted Critical
Publication of CN113378746B publication Critical patent/CN113378746B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供了一种定位方法，应用于移动设备中的处理器，该移动设备安装有RGB‑D相机，在对当前帧图像与参考帧图像进行特征点匹配后，利用运动一致性滤波算法对匹配得到的特征点进行处理，得到当前帧图像中的静态特征点，进而根据该静态特征点计算RGB‑D相机的初始化位姿。通过利用特征点长时间观测信息和空间关联性，建立特征点联结图，通过切割特征点联结图，准确区分动态特征点和静态特征点，利用相对准确的静态特征点对RGB‑D相机的初始化位姿进行优化，提高视觉SLAM系统的定位精度。本发明并不会过多增加传统SLAM算法的时间复杂度，也不会对系统应用场景造成限制，SLAM系统可以在动态场景下进行准确地位姿估计。

Description

一种定位方法及装置
技术领域
本发明涉及人工智能技术领域，更具体的，涉及一种定位方法及装置。
背景技术
随着人工智能领域的快速发展，移动机器人技术在工业、服务业等领域的应用越来越广泛。为了应对复杂的室内环境，机器人需要具有智能化和鲁棒性地自主导航能力。作为机器人导航中必不可缺的同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术便成为了当前机器人领域研究的热点。
SLAM技术是指搭载特定传感器的机器人，在没有环境先验信息的情况下，在运动过程中建立环境模型，并且估计自己的运动位姿。但是为了简化定位问题，绝大多数SLAM系统都是基于周围环境是静态这个假设。然而，在现实室内场景中，移动对象(如人)总是存在的，因此许多现有的SLAM系统适用的场景受到了一定限制。在这些传统的SLAM系统中，移动物体的运动容易被融合到相机的姿态估计中，导致定位精度变差，甚至系统故障。
发明内容
有鉴于此，本发明提供了一种定位方法及装置，提高视觉SLAM系统的定位精度。
为了实现上述发明目的，本发明提供的具体技术方案如下：
一种定位方法，应用于移动设备中的处理器，所述移动设备安装有RGB-D相机，所述方法包括：
获取RGB-D相机采集的当前帧图像；
对所述当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点，所述参考帧图像为满足预设条件的关键帧图像；
利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点；
基于所述当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
对所述当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点，所述局部地图点为预设历史时间段内与所述当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合；
根据所述第二特征点构建特征点联结图，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点；
利用所述特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化。
可选的，所述利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点，包括：
对所述第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
计算每个聚类块的位姿变换矩阵；
利用所述第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的所述第一特征点确定为所述当前帧图像中的静态特征点。
可选的，所述对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，包括：
根据所述第一特征点对应的3D-2D点对，构建重投影误差方程；
基于所述重投影误差方程，分别计算最符合背景相对运动属性的位姿变换矩阵中每个第一特征点对的误差。
可选的，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点，包括：
为所述特征点联结图中的每个所述第二特征点设置标签，得到所述特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
构建所述标签集合对应的能量函数；
以所述能量函数的值最小为优化目标，求解所述能量函数，得到静态标签对应的静态特征点。
可选的，所述能量函数如下：
Figure BDA0003125731660000031
其中，
Figure BDA0003125731660000032
li，lj分别表示第i和第j个特征点的动态静态标签；
一元能量项
Figure BDA0003125731660000033
决定了将分配动态标签和静态标签给予第二特征点的惩罚；
关联能量项B(uic，ujc)
Figure BDA0003125731660000034
表示越邻近的两个第二特征点被分配相同标签的概率越高；
(uic，ujc)分别是当前帧图像特征点i和j；
λ为一元能量项与关联能量项之间的比例系数；
K表示所述特征点联结图中有联结关系的点对集合。
一种定位装置，应用于移动设备中的处理器，所述移动设备安装有RGB-D相机，所述装置包括：
当前帧图像获取单元，用于获取RGB-D相机采集的当前帧图像；
第一匹配单元，用于对所述当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点，所述参考帧图像为满足预设条件的关键帧图像；
第一静态特征点确定单元，用于利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点；
初始化位姿计算单元，用于基于所述当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
第二匹配单元，用于对所述当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点，所述局部地图点为预设历史时间段内与所述当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合；
第二静态特征点确定单元，用于根据所述第二特征点构建特征点联结图，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点；
初始化位姿优化单元，用于利用所述特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化。
可选的，所述第一静态特征点确定单元，包括：
聚类子单元，用于对所述第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
位姿变换矩阵计算子单元，用于计算每个聚类块的位姿变换矩阵；
极线约束验证子单元，用于利用所述第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
误差检测子单元，用于对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的所述第一特征点确定为所述当前帧图像中的静态特征点。
可选的，所述误差检测子单元，具体用于：
根据所述第一特征点对应的3D-2D点对，构建重投影误差方程；
基于所述重投影误差方程，分别计算最符合背景相对运动属性的位姿变换矩阵中每个第一特征点对的误差；
将误差小于阈值的第一特征点对加入点对集合。
可选的，所述第二静态特征点确定单元，具体用于：
为所述特征点联结图中的每个所述第二特征点设置标签，得到所述特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
构建所述标签集合对应的能量函数；
以所述能量函数的值最小为优化目标，求解所述能量函数，得到静态标签对应的静态特征点。
可选的，所述能量函数如下：
Figure BDA0003125731660000051
其中，
Figure BDA0003125731660000052
li，lj分别表示第i和第j个特征点的动态静态标签；
一元能量项
Figure BDA0003125731660000053
决定了将分配动态标签和静态标签给予第二特征点的惩罚；
关联能量项B(uic，ujc)
Figure BDA0003125731660000054
表示越邻近的两个第二特征点被分配相同标签的概率越高；
(uic，ujc)分别是当前帧图像特征点i和j；
λ为一元能量项与关联能量项之间的比例系数；
K表示所述特征点联结图中有联结关系的点对集合。
相对于现有技术，本发明的有益效果如下：
本发明公开的一种定位方法，在对当前帧图像与参考帧图像进行特征点匹配后，利用运动一致性滤波算法对匹配得到的特征点进行处理，得到当前帧图像中的静态特征点，进而根据该静态特征点计算RGB-D相机的初始化位姿。通过利用特征点长时间观测信息和空间关联性，建立特征点联结图，通过切割特征点联结图，准确区分动态特征点和静态特征点，利用相对准确的静态特征点对RGB-D相机的初始化位姿进行优化，提高视觉SLAM系统的定位精度。本发明并不会过多增加传统SLAM算法的时间复杂度，也不会对系统应用场景造成限制，SLAM系统可以在动态场景下进行准确地位姿估计。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例公开的一种定位方法的流程示意图；
图2为本发明实施例公开的运动一致性滤波算法处理方法的流程示意图；
图3为本发明实施例公开的一种特征点联结图切割方法的流程示意图；
图4为本发明实施例公开的一种定位装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供了一种定位方法，应用于移动设备中的处理器，该移动设备安装有RGB-D相机，该移动设备可以为机器人，本发明利用特征点长时间观测信息和空间关联性，建立特征点联结图，通过切割特征点联结图，准确区分动态特征点和静态特征点，通过利用剔除动态特征点之后的静态特征点对RGB-D相机的初始化位姿进行优化，提高视觉SLAM系统的定位精度。本发明并不会过多增加传统SLAM算法的时间复杂度，也不会对系统应用场景造成限制，SLAM系统可以在动态场景下进行准确地位姿估计。
具体的，请参阅图1，本发明实施例公开的一种定位方法包括以下步骤：
S101：获取RGB-D相机采集的当前帧图像；
当前帧图像为当天时刻的图像帧。
S102：对当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点；
特征点是通过对当前帧图像进行特征提取得到的，特征点是图像中具有代表性的点，是图像信息的另一种数字表达形式，本实施例对当前帧图像提取的是ORB特征，它由关键点和描述子组成。
关键帧图像为具有代表性的图像帧，可以根据具体应用场景定义初始关键帧图像。在定义初始关键帧图像之后，其他关键帧的选取方式有很多，如取与上一关键帧图像相隔预设数量图像帧的图像帧，即隔固定数量图像帧取一帧为关键帧，或取与上一关键帧图像相隔预设时间的图像帧，即固定时间间隔取一帧为关键帧，或与上一关键帧图像的共视程度小于阈值的图像帧，在此不做具体限定。
参考帧图像为满足预设条件的关键帧图像，如满足预设共视比例的关键帧图像，或与当前帧图像采集时刻最接近的关键帧图像等。
在对当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配的过程中，为了加速匹配，采用DBOW2词袋技术，对当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点。
S103：利用运动一致性滤波算法对第一特征点进行处理，得到当前帧图像中的静态特征点；
在动态场景中，由于移动目标的影响，一部分匹配成功的第一特征点不符合多视图几何约束，导致相机位姿估计不准确，为了获取较为可靠的初始化位姿，本发明实施例提供了一个运动一致性滤波算法，对第一特征点进行处理，得到当前帧图像中的静态特征点。
具体的，请参阅图2，本实施例公开的运动一致性滤波算法处理方法包括以下步骤：
S201：对第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
可选的，运用K-means++算法对第一特征点进行聚类，分为N类，每类用Ci表示。聚类的依据是第一特征点对应的3D地图点的三维坐标，3D地图点为平面特征点经过投影变换到3D空间的三维坐标形式。我们聚类分类的出发点是假设同一聚类块中的第一特征点具有运动一致性。这个假设是合理的，因为我们后续的目标是寻找与静态背景具有相同运动属性的聚类特征，而不是在这一步中消除所有的动态特征点。
S202：计算每个聚类块的位姿变换矩阵；
S203：利用第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
由于只使用当前帧图像中具有可靠深度值的匹配得到的第一特征点进行聚类，必然会有一些匹配的第一特征点没有深度值或深度值不可靠。我们将这些没有用于聚类的匹配的2D-2D特征点对对每个聚类的位姿变换矩阵
Figure BDA0003125731660000081
进行投票，投票的原则是基于极线约束，即满足以下方程
Figure BDA0003125731660000082
其中，
Figure BDA0003125731660000083
分别是参考帧图像和当前帧图像上特征点的齐次形式，F是基础矩阵，其形式为：
Figure BDA0003125731660000087
K为相机的内参矩阵，t和R为位姿变换矩阵的旋转部分和平移部分。
A和B为极线参数。σd为预先根据实验结果设定的阈值，决定聚类能否获得投票。
Figure BDA0003125731660000084
选出获得投票最多的聚类块对应的位姿变换矩阵
Figure BDA0003125731660000085
为最符合背景相对运动属性的位姿变换矩阵Tinit，让其作为后续迭代的初始化位姿矩阵。
S204：对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
S205：利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的第一特征点确定为当前帧图像中的静态特征点。
首先，将参考帧图像中的第一特征点投影为3D地图点，与当前帧图像中的第一特征点构成3D-2D点对，构建重投影误差方程：
Figure BDA0003125731660000086
其中，F(·)为Huber核函数，其作用是当出现特征点误匹配情况时，重投影误差会很大，通过核函数可以限制重投影误差的增长，而且因为Huber核函数是光滑的，方便求导，利于重投影误差方程的求解。
π(·)是基于相机模型的投影变换，可以将相机坐标系下3D坐标变化为相机图像中的2D像素坐标。
ui＝[xi，yi]T为3D-2D点对中2D点在像素平面的坐标。
Pi＝[Xi，Yi，Zi]T为3D-2D点对中3D点在空间中的坐标。
最后通过EPNP算法进行非线性优化求解。
对所有3D-2D点对进行筛选，将满足下式的点对加入点对集合χ。
Figure BDA0003125731660000091
其中，∑i为与特征点尺度相关的协方差矩阵，σr为预先根据实验数据定义的阈值。之后再利用点对集合χ中的数据在EPNP算法框架下重新计算Tinit。如此迭代一定次数，最后x中的点对即为最后得到的静态特征点。
S104：基于当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
具体的，利用EPNP算法，基于当前帧图像中的静态特征点计算RGB-D相机的初始化位姿。
S105：对当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点；
需要说明的是，上述运动一致性滤波算法只能识别一定数量的静态特征点，并提供粗糙的初始化位姿估计。为了提高相机位姿估计精度，本发明实施例基于graph-cut框架，构建特征点联结图，利用长时间观测、静态先验和特征点的区域分布相似性区分动态静态特征，并利用静态特征点对上述初始化位姿进行优化。
具体的，获取局部地图点，局部地图点为预设历史时间段内与当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合，获取局部地图点的目的在于：获取更多特征点匹配对，提高位姿优化的鲁棒性和准确性。
S106：根据第二特征点构建特征点联结图，对特征点联结图进行切割，得到特征点联结图中的静态特征点；
基于动态特征点周围的特征点是动态特征点的可能性更大，静态特征点周围的特征点是静态特征点的可能性更大这一原则，本发明实施例获取特征点的联结关系，以获取特征点的领域相关点，通过Delaunay triangulation将一系列离散的特征点联结为一个稀疏点图，这种联结方式的特点是对于第二特征点构成的三角形，其外接圆是没有其他特征点在内的。这个稀疏点图我们用G表示，图中点表示每个第二特征点的动态静态标签，连接点的边表示两个第二特征点具有联结关系。
本发明实施例将特征点联结图中动态、静态特征点区分这个问题转化为二分类的打标签问题，在graph-cut框架下，建立优化能量方程进行问题求解。
具体的，请参阅图3，本实施例提供了一种特征点联结图切割方法，包括以下步骤：
S301：为特征点联结图中的每个第二特征点设置标签，得到特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
将特征点联结图中的特征点的标签集合定义为：L＝{l1，l2，...，l|L|}，|L|表示的是局部地图点与当前帧图像中匹配得到的第二特征点数量，为了简便起见，我们令li∈{0，1}，0代表静态标签，1代表动态标签。
S302：构建所述标签集合对应的能量函数；
将区分动态静态特征点的转化为寻找一个最优的标签向量L*，使能量函数值最小的问题。能量函数如下：
Figure BDA0003125731660000101
其中，
Figure BDA0003125731660000102
li，lj分别表示第i和第j个特征点的动态静态标签。
在这个能量函数中，第一项
Figure BDA0003125731660000103
被称为一元能量项，第二项B(uic，ujc)
Figure BDA0003125731660000104
被称为关联能量项，用λ来协调两项比例。(uic，ujc)分别是当前帧图像特征点i和j。K代表图G中有联结关系的点对集合。E(L)代表所有能量项的能量总和。
需要说明的是，一元能量项
Figure BDA0003125731660000105
决定了将分配动态标签和静态标签给予第二特征点的惩罚。对于第二特征点uic，该项的定义为：
Figure BDA0003125731660000106
其中，
Figure BDA0003125731660000107
和
Figure BDA0003125731660000108
代表特征点uic的动态和静态概率，
Figure BDA0003125731660000109
设置为
Figure BDA00031257316600001010
静态概率
Figure BDA00031257316600001011
是有两方面信息决定的，长时间观测信息
Figure BDA00031257316600001012
和先验静态信息
Figure BDA00031257316600001013
Figure BDA00031257316600001014
第一项长时间观测信息
Figure BDA00031257316600001015
通过分配高静态概率给平均累计重投影误差低的第i个特征点，以强调信任长期观测的信息。它的定义如下：
Figure BDA00031257316600001016
其中，eproj(uic)代表的是长期信息，它是通过平均特征点uic和它对应的所有在关键帧图像上的特征点的重投影误差获取的，数学形式为：
Figure BDA0003125731660000111
Figure BDA0003125731660000112
和
Figure BDA0003125731660000113
分别代表第i个特征点对应的在第k关键帧图像上的2D观测和第k关键帧时刻相机的位姿。Ni是能够观测到第i个特征点的关键帧数目，zi是特征点i对应的深度值。这项公式的出发点在于，短期信息可能导致对于动态特征点的不准确检测，然而，通过将特征点投影到可以观测到其对应3D地图点的长时间范围的关键帧图像做比较，所获取的信息更可靠。
第二项
Figure BDA0003125731660000114
代表的是先验静态概率，这项考虑的是在室内环境下远距离观测更可能是静态背景以及场景中的静态成分倾向于小段时间内持续移动。该项定义如下：
Figure BDA0003125731660000115
其中，zmax是一个自适应的阈值，它的大小设为当前帧图像所有匹配的特征点的平均深度值的二分之一。uml是上一帧中被判定为动态的第m个视觉特征。d2D(·)表示两个特征点在像素平面的距离，dth为一个经验阈值。
关联能量项B(uic，ujc)
Figure BDA0003125731660000116
的出发点是邻近的特征点，如果它们的eproj(·)大小越相近，那么它们更有可能被分配相同的标签。作为能量项，这样可以惩罚在图G中相邻，却标签不同的特征点对。其中B(uic，ujc)的定义如下：
Figure BDA0003125731660000117
其中，d3D(uic，ujc)表示特征点uic，ujc所对应的3D地图点之间的距离。ω是预定义的常值。当两个特征点对应的平均重投影误差很接近时候，B(uic，ujc)会变小，并且对应的能量处罚也会随着d3D(·)的增大而减小。
S303：以能量函数的值最小为优化目标，求解能量函数，得到静态标签对应的静态特征点。
最后我们使用高效的graph-cut算法，对所构建的图进行切割，求取最优的标签向量L*，使得能量函数最小时的静态特征点。
S107：利用特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化。
对于特征点联结图中被判断为静态特征点，我们使用这些静态特征点对进行bundle adjustment进一步优化相机位姿。
本实施例公开的一种定位方法，在对当前帧图像与参考帧图像进行特征点匹配后，利用运动一致性滤波算法对匹配得到的特征点进行处理，得到当前帧图像中的静态特征点，进而根据该静态特征点计算RGB-D相机的初始化位姿。通过利用特征点长时间观测信息和空间关联性，建立特征点联结图，通过切割特征点联结图，准确区分动态特征点和静态特征点，利用相对准确的静态特征点对RGB-D相机的初始化位姿进行优化，提高视觉SLAM系统的定位精度。本发明并不会过多增加传统SLAM算法的时间复杂度，也不会对系统应用场景造成限制，SLAM系统可以在动态场景下进行准确地位姿估计。
本发明综合利用环境信息和观测信息以及特征点与特征点的联系进行动态静态特征点区分，不同于深度学习的预训练数据和额外传感器补偿的方法，不需要对场景环境有预先的了解，也不需要对额外传感器先验，减少了对硬件水平的需求，是一种实用性很强的处理动态环境的SLAM算法。
基于上述实施例公开的一种定位方法，本实施例对应公开了一种定位装置，应用于移动设备中的处理器，所述移动设备安装有RGB-D相机，请参阅图4，所述装置包括：
当前帧图像获取单元100，用于获取RGB-D相机采集的当前帧图像；
第一匹配单元200，用于对所述当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点，所述参考帧图像为满足预设条件的关键帧图像；
第一静态特征点确定单元300，用于利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点；
初始化位姿计算单元400，用于基于所述当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
第二匹配单元500，用于对所述当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点，所述局部地图点为预设历史时间段内与所述当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合；
第二静态特征点确定单元600，用于根据所述第二特征点构建特征点联结图，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点；
初始化位姿优化单元700，用于利用所述特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化。
可选的，所述第一静态特征点确定单元300，包括：
聚类子单元，用于对所述第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
位姿变换矩阵计算子单元，用于计算每个聚类块的位姿变换矩阵；
极线约束验证子单元，用于利用所述第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
误差检测子单元，用于对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
迭代计算子单元，用于利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的所述第一特征点确定为所述当前帧图像中的静态特征点。
可选的，所述误差检测子单元，具体用于：
根据所述第一特征点对应的3D-2D点对，构建重投影误差方程；
基于所述重投影误差方程，分别计算最符合背景相对运动属性的位姿变换矩阵中每个第一特征点对的误差；
将误差小于阈值的第一特征点对加入点对集合。
可选的，所述第二静态特征点确定单元600，具体用于：
为所述特征点联结图中的每个所述第二特征点设置标签，得到所述特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
构建所述标签集合对应的能量函数；
以所述能量函数的值最小为优化目标，求解所述能量函数，得到静态标签对应的静态特征点。
可选的，所述能量函数如下：
Figure BDA0003125731660000131
其中，
Figure BDA0003125731660000141
li，lj分别表示第i和第j个特征点的动态静态标签；
一元能量项
Figure BDA0003125731660000142
决定了将分配动态标签和静态标签给予第二特征点的惩罚；
关联能量项B(uic，ujc)
Figure BDA0003125731660000143
表示越邻近的两个第二特征点被分配相同标签的概率越高；
(uic，ujc)分别是当前帧图像特征点i和j；
λ为一元能量项与关联能量项之间的比例系数；
K表示所述特征点联结图中有联结关系的点对集合。
本实施例公开的一种定位装置，在对当前帧图像与参考帧图像进行特征点匹配后，利用运动一致性滤波算法对匹配得到的特征点进行处理，得到当前帧图像中的静态特征点，进而根据该静态特征点计算RGB-D相机的初始化位姿。通过利用特征点长时间观测信息和空间关联性，建立特征点联结图，通过切割特征点联结图，准确区分动态特征点和静态特征点，利用相对准确的静态特征点对RGB-D相机的初始化位姿进行优化，提高视觉SLAM系统的定位精度。本发明并不会过多增加传统SLAM算法的时间复杂度，也不会对系统应用场景造成限制，SLAM系统可以在动态场景下进行准确地位姿估计。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
上述各个实施例之间可任意组合，对所公开的实施例的上述说明，本说明书中各实施例中记载的特征可以相互替换或者组合，使本领域专业技术人员能够实现或使用本申请。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.一种定位方法，其特征在于，应用于移动设备中的处理器，所述移动设备安装有RGB-D相机，所述方法包括：
获取RGB-D相机采集的当前帧图像；
对所述当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点，所述参考帧图像为满足预设条件的关键帧图像；
利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点；
基于所述当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
对所述当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点，所述局部地图点为预设历史时间段内与所述当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合；
根据所述第二特征点构建特征点联结图，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点；
利用所述特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化；
对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点，包括：
为所述特征点联结图中的每个所述第二特征点设置标签，得到所述特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
构建所述标签集合对应的能量函数；
以所述能量函数的值最小为优化目标，求解所述能量函数，得到静态标签对应的静态特征点；
所述能量函数如下：
Figure FDA0003690761170000011
其中，
Figure FDA0003690761170000012
li，lj分别表示第i和第j个特征点的动态静态标签；
一元能量项
Figure FDA0003690761170000013
决定了将分配动态标签和静态标签给予第二特征点的惩罚，对于第二特征点uic，该项的定义为：
Figure FDA0003690761170000021
其中，
Figure FDA0003690761170000022
和
Figure FDA0003690761170000023
代表特征点uic的动态和静态概率，
Figure FDA0003690761170000024
设置为
Figure FDA0003690761170000025
静态概率
Figure FDA0003690761170000026
由长时间观测信息
Figure FDA0003690761170000027
和先验静态信息
Figure FDA0003690761170000028
决定，
Figure FDA0003690761170000029
第一项长时间观测信息
Figure FDA00036907611700000210
通过分配高静态概率给平均累计重投影误差低的第i个特征点，以强调信任长期观测的信息；
第二项
Figure FDA00036907611700000211
代表的是先验静态概率，这项考虑的是在室内环境下远距离观测更可能是静态背景以及场景中的静态成分倾向于小段时间内持续移动；
关联能量项
Figure FDA00036907611700000212
表示越邻近的两个第二特征点被分配相同标签的概率越高；
(uic，ujc)分别是当前帧图像特征点i和j；
λ为一元能量项与关联能量项之间的比例系数；
K表示所述特征点联结图中有联结关系的点对集合。
2.根据权利要求1所述的方法，其特征在于，所述利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点，包括：
对所述第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
计算每个聚类块的位姿变换矩阵；
利用所述第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的所述第一特征点确定为所述当前帧图像中的静态特征点。
3.根据权利要求2所述的方法，其特征在于，所述对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，包括：
根据所述第一特征点对应的3D-2D点对，构建重投影误差方程；
基于所述重投影误差方程，分别计算最符合背景相对运动属性的位姿变换矩阵中每个第一特征点对的误差。
4.一种定位装置，其特征在于，应用于移动设备中的处理器，所述移动设备安装有RGB-D相机，所述装置包括：
当前帧图像获取单元，用于获取RGB-D相机采集的当前帧图像；
第一匹配单元，用于对所述当前帧图像中的特征点与参考帧图像中的特征点反投影至3D空间中的特征点进行匹配，得到匹配成功的第一特征点，所述参考帧图像为满足预设条件的关键帧图像；
第一静态特征点确定单元，用于利用运动一致性滤波算法对所述第一特征点进行处理，得到所述当前帧图像中的静态特征点；
初始化位姿计算单元，用于基于所述当前帧图像中的静态特征点计算RGB-D相机的初始化位姿；
第二匹配单元，用于对所述当前帧图像中的特征点与局部地图点进行匹配，得到匹配成功的第二特征点，所述局部地图点为预设历史时间段内与所述当前帧图像有共视关系的关键帧图像中的特征点反投影至3D空间中的特征点集合；
第二静态特征点确定单元，用于根据所述第二特征点构建特征点联结图，对所述特征点联结图进行切割，得到所述特征点联结图中的静态特征点；
初始化位姿优化单元，用于利用所述特征点联结图中的静态特征点，对RGB-D相机的初始化位姿进行优化；
所述第二静态特征点确定单元，具体用于：
为所述特征点联结图中的每个所述第二特征点设置标签，得到所述特征点联结图对应的标签集合，标签的值为静态标签或动态标签；
构建所述标签集合对应的能量函数；
以所述能量函数的值最小为优化目标，求解所述能量函数，得到静态标签对应的静态特征点；
所述能量函数如下：
Figure FDA0003690761170000031
其中，
Figure FDA0003690761170000032
li，lj分别表示第i和第j个特征点的动态静态标签；
一元能量项
Figure FDA0003690761170000033
决定了将分配动态标签和静态标签给予第二特征点的惩罚，对于第二特征点uic，该项的定义为：
Figure FDA0003690761170000041
其中，
Figure FDA0003690761170000042
和
Figure FDA0003690761170000043
代表特征点uic的动态和静态概率，
Figure FDA0003690761170000044
设置为
Figure FDA0003690761170000045
静态概率
Figure FDA0003690761170000046
由长时间观测信息
Figure FDA0003690761170000047
和先验静态信息
Figure FDA0003690761170000048
决定，
Figure FDA0003690761170000049
第一项长时间观测信息
Figure FDA00036907611700000410
通过分配高静态概率给平均累计重投影误差低的第i个特征点，以强调信任长期观测的信息；
第二项
Figure FDA00036907611700000411
代表的是先验静态概率，这项考虑的是在室内环境下远距离观测更可能是静态背景以及场景中的静态成分倾向于小段时间内持续移动；
关联能量项
Figure FDA00036907611700000412
表示越邻近的两个第二特征点被分配相同标签的概率越高；
(uic，ujc)分别是当前帧图像特征点i和j；
λ为一元能量项与关联能量项之间的比例系数；
K表示所述特征点联结图中有联结关系的点对集合。
5.根据权利要求4所述的装置，其特征在于，所述第一静态特征点确定单元，包括：
聚类子单元，用于对所述第一特征点中具有可靠深度值的特征点进行聚类，得到具有运动一致性的多个聚类块；
位姿变换矩阵计算子单元，用于计算每个聚类块的位姿变换矩阵；
极线约束验证子单元，用于利用所述第一特征点中没有深度值或深度值不可靠的特征点，对每个聚类块进行极线约束验证，得到最符合背景相对运动属性的位姿变换矩阵，作为迭代的初始化位姿变换矩阵；
误差检测子单元，用于对最符合背景相对运动属性的位姿变换矩阵中的第一特征点对进行误差检测，将误差小于阈值的第一特征点对加入点对集合；
迭代计算子单元，用于利用点对集合中的第一特征点对重新计算位姿变换矩阵，如此迭代计算预设次数，将最终点对集合中的第一特征点对对应的所述第一特征点确定为所述当前帧图像中的静态特征点。
6.根据权利要求5所述的装置，其特征在于，所述误差检测子单元，具体用于：
根据所述第一特征点对应的3D-2D点对，构建重投影误差方程；
基于所述重投影误差方程，分别计算最符合背景相对运动属性的位姿变换矩阵中每个第一特征点对的误差；
将误差小于阈值的第一特征点对加入点对集合。
CN202110688891.4A 2021-06-22 2021-06-22 一种定位方法及装置 Active CN113378746B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110688891.4A CN113378746B (zh) 2021-06-22 2021-06-22 一种定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110688891.4A CN113378746B (zh) 2021-06-22 2021-06-22 一种定位方法及装置

Publications (2)

Publication Number Publication Date
CN113378746A CN113378746A (zh) 2021-09-10
CN113378746B true CN113378746B (zh) 2022-09-02

Family

ID=77578255

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110688891.4A Active CN113378746B (zh) 2021-06-22 2021-06-22 一种定位方法及装置

Country Status (1)

Country Link
CN (1) CN113378746B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110274602A (zh) * 2018-03-15 2019-09-24 奥孛睿斯有限责任公司 室内地图自动构建方法及系统
CN111724439A (zh) * 2019-11-29 2020-09-29 中国科学院上海微系统与信息技术研究所 一种动态场景下的视觉定位方法及装置
CN112381841A (zh) * 2020-11-27 2021-02-19 广东电网有限责任公司肇庆供电局 一种动态场景下基于gms特征匹配的语义slam方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR102016551B1 (ko) * 2014-01-24 2019-09-02 한화디펜스 주식회사 위치 추정 장치 및 방법
CN111354042B (zh) * 2018-12-24 2023-12-01 深圳市优必选科技有限公司 机器人视觉图像的特征提取方法、装置、机器人及介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110274602A (zh) * 2018-03-15 2019-09-24 奥孛睿斯有限责任公司 室内地图自动构建方法及系统
CN111724439A (zh) * 2019-11-29 2020-09-29 中国科学院上海微系统与信息技术研究所 一种动态场景下的视觉定位方法及装置
CN112381841A (zh) * 2020-11-27 2021-02-19 广东电网有限责任公司肇庆供电局 一种动态场景下基于gms特征匹配的语义slam方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Indoor Localization for Skid-Steering Mobile Robot by Fusing Encoder,Gyroscope,and Magnetometer";Wenjun Lv 等;《IEEE TRANSACTIONS ON SYSTEMS》;20190630;第49卷(第6期);1241-1253 *
"基于GC-RANSAC算法的单目视觉同时定位与地图构建";段震灏 等;《长春理工大学学报（自然科学版）》;20200229;第43卷(第1期);20-26 *

Also Published As

Publication number Publication date
CN113378746A (zh) 2021-09-10

Similar Documents

Publication Publication Date Title
Zhang et al. A sparsity‐inducing optimization‐based algorithm for planar patches extraction from noisy point‐cloud data
Fehr et al. TSDF-based change detection for consistent long-term dense reconstruction and dynamic object discovery
Paul et al. FAB-MAP 3D: Topological mapping with spatial and visual appearance
Kang et al. Automatic targetless camera–lidar calibration by aligning edge with gaussian mixture model
EP2907082B1 (en) Using a probabilistic model for detecting an object in visual data
CN108229347B (zh) 用于人识别的拟吉布斯结构采样的深层置换的方法和装置
US7844133B2 (en) Method and system for filtering, registering, and matching 2.5D normal maps
CN107369131B (zh) 图像的显著性检测方法、装置、存储介质和处理器
Delmerico et al. Building facade detection, segmentation, and parameter estimation for mobile robot localization and guidance
Pascoe et al. Robust direct visual localisation using normalised information distance.
CN110610505A (zh) 一种融合深度和色彩信息的图像分割方法
Dubois et al. A comparison of geometric and energy-based point cloud semantic segmentation methods
Jellal et al. LS-ELAS: Line segment based efficient large scale stereo matching
WO2009152509A1 (en) Method and system for crowd segmentation
Cupec et al. Place recognition based on matching of planar surfaces and line segments
Payet et al. Scene shape from texture of objects
Jung et al. A line-based progressive refinement of 3D rooftop models using airborne LiDAR data with single view imagery
CN114140527A (zh) 一种基于语义分割的动态环境双目视觉slam方法
Dey et al. Machine learning-based segmentation of aerial LiDAR point cloud data on building roof
Fehr et al. Reshaping our model of the world over time
US6665440B1 (en) System and method for performing corner guided curve matching of multiple images representing a scene
Lladó et al. Non-rigid metric reconstruction from perspective cameras
CN115578539B (zh) 室内空间高精度视觉位置定位方法、终端及存储介质
Tung et al. MF3D: Model-free 3D semantic scene parsing
CN113378746B (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