CN111914832B - 一种rgb-d相机在动态场景下的slam方法 - Google Patents

一种rgb-d相机在动态场景下的slam方法 Download PDF

Info

Publication number
CN111914832B
CN111914832B CN202010492646.1A CN202010492646A CN111914832B CN 111914832 B CN111914832 B CN 111914832B CN 202010492646 A CN202010492646 A CN 202010492646A CN 111914832 B CN111914832 B CN 111914832B
Authority
CN
China
Prior art keywords
dynamic
current frame
points
static
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
CN202010492646.1A
Other languages
English (en)
Other versions
CN111914832A (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.)
South China University of Technology SCUT
Guangzhou Institute of Modern Industrial Technology
Original Assignee
South China University of Technology SCUT
Guangzhou Institute of Modern Industrial 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 South China University of Technology SCUT, Guangzhou Institute of Modern Industrial Technology filed Critical South China University of Technology SCUT
Priority to CN202010492646.1A priority Critical patent/CN111914832B/zh
Publication of CN111914832A publication Critical patent/CN111914832A/zh
Application granted granted Critical
Publication of CN111914832B publication Critical patent/CN111914832B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/62—Text, e.g. of license plates, overlay texts or captions on TV images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
    • G06F18/2415—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches based on parametric or probabilistic models, e.g. based on likelihood ratio or false acceptance rate versus a false rejection rate
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Probability & Statistics with Applications (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种RGB‑D相机在动态场景下的SLAM方法，使RGB‑D相机有效地克服动态物体的影响，提高RGB‑D相机在动态场景下的定位精度。为了实时检测动态目标上的特征点，本发明提出一种二重聚类的动态特征点检测机制；然后，同时在几何和时间维度上估计特征点是静态的可能性。在几何维度中，为当前帧的每个特征点建立静态概率来表示特征点是静态的；在时间维度中，根据所有帧而不是当前帧来判断特征点是否是静态特征点。最后，为了使RANSAC算法更适用于动态环境，本发明对传统的RANSAC算法进行改进。实验结果表明，本发明有效地提高了RGB‑D相机在动态环境下的定位精度，并且可以实时地在CPU上运行。

Description

一种RGB-D相机在动态场景下的SLAM方法
技术领域
本发明涉及计算机视觉技术领域，具体涉及一种RGB-D相机在动态场景下的SLAM方法。
背景技术
经过多年的发展，SLAM(同时定位与地图构建)技术已经很成熟并应用到多个领域。传统上，大多数视觉SLAM系统都假定在静态的环境运行的，然而，在SLAM的真实应用场景里，存在着许许多多的动态物体，比如在工厂运输线上，有许多来回走动的工人；在商场里总有四处走动的客人等等。因此现实生活中的大部分场景都是动态的，这也意味着许多SLAM系统很难在实际的动态环境中工作。
因此，很多研究者对动态环境下的SLAM做了大量的研究工作，一般有三种方法可以用来降低由动态物体引起的相机位姿估计的误差。
第一种是直接将运动对象上的像素点或特征点视为离群点并将其去除，这种方法中最常用的是通过使用RANSAC及其改进算法去剔除动态物体上的点。这些方法在低动态场景具有不错的位姿估计精度。但是，这些方法很难在动态对象过多的场景中工作。
第二种方法主要利用帧间信息来剔除动态物体上的点，也是运用得比较广泛的动态环境下的SLAM方法。这种方法通过多个以前帧建立好一个只包含静态场景的三维背景模型，根据静态背景模型通过重投影的方式去除动态物体的干扰，从而提高相机的位姿估计精度。这种方法能够较好地在动态环境下工作，但是建立静态背景模型的计算量非常大，难以实时地在CPU上运行。
第三种方法是基于深度学习的方法。这种方法一般通过基于深度学习的目标检测算法将图像帧可能的动态物体检测出来，再对这些动态物体进行像素级分割，从而去除动态物体。基于深度学习的方法在高度动态的场景下具有相当不错的性能，然而这种方法使用了基于深度学习的目标检测算法和像素级的物体分割算法，导致计算开销大，非常耗时，同样难以实时在CPU上运行。
发明内容
本发明的目的是为了解决现有技术中的上述缺陷，提供一种RGB-D相机在动态场景下的SLAM方法。
本发明的目的可以通过采取如下技术方案达到：
一种RGB-D相机在动态场景下的SLAM方法，所述方法包括以下步骤：
S1、在RGB-D相机采集的当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算，将当前帧的RGB图像与上一帧的RGB图像进行描述子匹配，通过最小化重投影误差初步计算出当前帧的位姿；
S2、将当前帧的位姿与所有关键帧的位姿进行比较，找出位姿与当前帧相近并且时间上与当前帧相远的前m个关键帧，设为参考关键帧集，表示为：
Figure BDA0002521656860000021
其中，a,b,c,d为大于0的常数，id是关键帧的序号，idc是当前帧的序号，R是关键帧的旋转矩阵，t是关键帧的平移矩阵，Rc是当前帧的旋转矩阵，tc是当前帧的平移矩阵；
S3、将参考关键帧集中的每一帧的所有深度值大于0的特征点投影到当前帧上，找出投影后的深度值zproj与当前帧投影点位置的实际深度值z之差的绝对值Δz大于阈值Tz的点，记为投影动态点，表示为：
Δz＝|zproj-z|＞Tz；
S4、根据深度值对当前帧的特征点进行K-Means聚类，找出初始动态集；
S5、在根据深度值对当前帧的特征点进行K-Means聚类的基础上，再根据二维图像坐标(u，v)对当前帧的每个初始动态集中的特征点进行第二次K-Means聚类，得到最终动态集，最终动态集中的特征点记为动态点，当前帧除动态点外的所有特征点记为静态特征点；
S6、在时间维度上，计算当前帧每个特征点的静态观测数，得到每个特征点的静态观测权重；
S7、在几何维度上，计算当前帧每个特征点的静态概率，通过特征点的静态观测权重和静态概率得到该特征点的静态权重；
S8、对传统的RANSAC算法的特征点进行改进，进一步剔除当前帧误匹配的特征点和处于动态对象上的特征点，再将剩下的特征点对应的地图点重投影到当前帧，对每个特征点进行静态权重加权，最小化加权重投影之和，求得相机当前帧的最终位姿。
进一步地，所述步骤S4过程如下：
S41、根据深度值对当前帧的特征点通过K-Means聚类算法将特征点聚成K类，并且计算出属于第i类的聚类中心ci，表示为：
Figure BDA0002521656860000041
其中，Ni为属于第i类的特征点数量，dik为第i类的第k个特征点的深度值；
S42、将每个投影动态点的深度值与每个聚类中心进行比较，找出与每个投影动态点的深度值之差的绝对值最小的聚类中心的类sj，表示为：
Figure BDA0002521656860000045
其中，sj为第j个投影动态点对应的类，depthj为第j个投影动态点的深度值，然后，统计每个类的投影动态点的数量，表示为：
Figure BDA0002521656860000042
其中，
Figure BDA0002521656860000043
Nproj为投影动态点的数量，numi为属于第i类的投影动态点的数量；
S43、统计属于每个类的投影动态点的数量，将投影动态点的数量numi大于阈值Td的类记为初始动态集。
进一步地，所述步骤S5过程如下：
S51、在根据深度值对当前帧的特征点进行聚类的基础上，根据二维图像坐标(u，v)对每个初始动态集的特征点进行K-Means聚类，并且计算出第i个初始动态集的第j个类的聚类中心cij，表示为：
Figure BDA0002521656860000044
其中，Nij为属于第i个初始动态集的第j个类的特征点数量，uijk和vijk为属于第i个初始动态集的第j个类的第k个特征点的二维图像坐标；
进一步写成：
cij＝(uij,vij)
其中，(uij,vij)为聚类中心cij的二维坐标；
S52、将属于第i个初始动态集的投影动态点的二维图像坐标与第i个初始动态集的每个聚类中心cij进行比较，计算出与投影动态点pik的二维图像坐标的距离最小的类s(i，k)，表示为：
Figure BDA0002521656860000053
其中，(upik,vpik)为投影动态点pik的二维图像坐标；
S53、统计所有初始动态集的每个类的投影动态点的数量，表示如下：
Figure BDA0002521656860000051
Figure BDA0002521656860000052
其中，Nproj(i)为第i个初始动态集的投影动态点的数量，numij为第i个初始动态集的第j类的投影动态点的数量；
S54、统计属于每个类的投影动态点的数量，投影动态点的数量numij大于阈值Tuv的类为最终动态集。
进一步地，所述步骤S6过程如下：
S61、在时间维度上统计当前帧每个特征点对应的地图点的静态观测数，表示规则如下：
从第一帧开始到当前帧，如果特征点pi被观测到并且被判定为静态特征点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)+1
其中，Vst(pi)为特征点pi的静态观测数，初始值为0，如果Vst(pi)大于静态观测阈值Tv,则特征点pi的静态观测数更新为：
Vst(pi)＝Tv
如果特征点pi被观测到，但特征点被判定为动态点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)-1；
S62、计算当前帧内所有特征点的静态观测数的均值μ及标准差S：
Figure BDA0002521656860000061
Figure BDA0002521656860000062
其中Nm为当前帧对应的地图点的数量；
S63、计算当前帧内的每个特征点的静态观测权重Wvst(pi)：
Figure BDA0002521656860000063
其中β为大于0的常数。
进一步地，所述步骤S7过程如下：
S71、对当前帧建立静态概率图，将当前帧划分为Nr×Nc个块，每个块为Lu×Lv的长方形，块(i,j)的静态概率Wgst(i,j)为：
Figure BDA0002521656860000064
其中：
Figure BDA0002521656860000071
Figure BDA0002521656860000072
其中(i,j)为位于第i行第j列的方块的序号，numst(x,y)和numdy(x,y)分别为序号为(x,y)的方块的静态特征点数量和动态特征点数量；
S72、对于二维坐标为(ui,vi)的特征点pi，其静态概率Wgst(pi)被计算如下：
Figure BDA0002521656860000073
其中，符号int(·)为对括号内的数进行取整操作，至此，所有方块对应的静态概率构成当前帧的静态概率图；
S73、将静态概率Wgst(pi)与静态观测权重Wvst(pi)结合起来，得到特征点pi的静态权重Wst(pi)：
Wst(pi)＝Wgst(pi)+αWvst(pi)
其中，Wvst(pi)是当前帧内的每个特征点pi的静态观测权重，α为大于0的常数。
进一步地，所述步骤S8过程如下：
对传统的RANSAC算法进行改进，提出了一种基于静态权重的RANSAC算法，具体改进为特征点选取方法和目标函数两个方面；
S81、对传统的RANSAC算法的特征点选取方法进行改进，在当前帧的所有特征点中，根据静态权重，每个特征点被选取到的概率为；
Figure BDA0002521656860000074
其中，Wst(pi)为特征点pi的静态权重，pro(pi)为特征点pi被选中的概率，N为当前帧的特征点的数量；
S82、对传统的RANSAC算法的目标函数进行改进，将传统的RANSAC算法的目标函数改进为：
Figure BDA0002521656860000081
其中：
Figure BDA0002521656860000082
其中，f(·)为目标函数，θk为当前变换参数，η为取值范围为(0,1)的常数，pi为当前帧第i个特征点；
S83、将改进的RANSAC算法选取出的每个特征点(内点)对应的地图点重投影到当前帧，建立静态权重加权的重投影误差，表示为：
Figure BDA0002521656860000083
其中，ew为静态权重加权的重投影误差，Tcw为RGB-D相机当前帧的最终位姿，K为RGB-D相机的内参数矩阵，zi为特征点pi的对应的地图点；
S84、最小化静态权重加权的重投影误差ew，即可求得RGB-D相机当前帧的最终位姿Tcw。
本发明相对于现有技术具有如下的优点及效果：
针对传统的SLAM方法无法克服动态物体的干扰的问题，本发明提出一种二重聚类的动态特征点检测机制去检测出动态特征点，有效地克服了传统的SLAM方法在动态环境下定位精度低的缺点。针对深度学习的动态SLAM方法，这些方法大部分使用了基于深度学习的目标检测算法和像素级的物体分割算法，计算开销非常大，难以实时地在CPU上运行。针对非深度学习的动态SLAM方法，这些方法大部分是使用静态背景模型通过重投影的方式去除动态物体的干扰，并没有考虑特征点与特征点之间的关系。在本发明中，当前帧的每个特征点都有一个静态概率，用来表示该特征点是静态的可能性，这个静态概率是根据该特征点与周围的特征点的关系得出的。同时，大多数非深度学习的动态SLAM方法也仅仅是根据以前多帧去判断点是静态的可能性，在本发明中，每个特征点为静态的可能性，是根据所有帧而不是以前多帧来判断的。最后，为了使RANSAC算法更适用于动态环境，本发明对传统的RANSAC算法进行改进。实验结果表明，本发明不仅有效地提高了RGB-D相机在动态环境下的定位精度，而且可以实时地在CPU上运行。
附图说明
图1是本发明公开的一种RGB-D相机在动态场景下的SLAM方法的流程图；
图2是本发明实施例中在某引用场景下的所有ORB特征点示意图；
图3是本发明实施例中在某引用场景下的投影动态点示意图；
图4是本发明实施例中在某引用场景下的动态特征点示意图；
图5是本发明实施例中动态特征点检测流程图；
图6是本发明实施例中动态特征点检测结果图；
图7是本发明实施例中运动轨迹对比图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例
如图1所示，本实施例具体公开了一种RGB-D相机在动态场景下的SLAM方法，该方法包括以下步骤：
S1、在RGB-D相机采集的当前帧的RGB图像上进行ORB特征点(如图2所示)提取和对所提取的ORB特征点进行描述子计算，将当前帧的RGB图像与上一帧的RGB图像进行描述子匹配，通过最小化重投影误差初步计算出当前帧的位姿；
S2、将当前帧的位姿与所有关键帧的位姿进行比较，尽可能找出找出位姿与当前帧相近并且时间上与当前帧相远的前m个关键帧，设为参考关键帧集，表示为：
Figure BDA0002521656860000101
其中，a,b,c,d为大于0的常数，id是关键帧的序号，idc是当前帧的序号，R关键帧的旋转矩阵，t是关键帧的平移矩阵，Rc是当前帧的旋转矩阵，tc是当前帧的平移矩阵；
引入c的目的是为了要从关键帧数据库中尽可能找出与当前帧的位姿相近，并且在时间上相远的参考关键帧。因为位姿和序号都与当前帧相近的参考关键帧往往在时间上与当前帧相近，可能会因为两帧中的同一动态物体的运动幅度过小而难以找出动态物体上的特征点。
S3、将参考关键帧集中的每一帧的所有深度值大于0的特征点投影到当前帧上，找出投影后的深度值zproj与当前帧投影点位置的实际深度值z之差的绝对值Δz大于阈值Tz的点，记为投影动态点，如图3所示，表示为：
Δz＝|zproj-z|＞Tz
S4、根据深度值对当前帧的特征点进行K-Means聚类，找出初始动态集；
由上述步骤得到的投影动态点并不是当前帧的特征点，这些投影动态点只是参考关键帧上的特征点投影到当前帧的二维点，由于测量误差或动态物体的存在，这些投影动态点并不会与当前帧的动态特征点一一重合，因此，此时的投影动态点集并不能作为最终的动态点集。
但是，投影动态点一般会出现在当前帧的动态特征点附近，因此可以通过对当前帧的所有特征点进行聚类，来求解出当前帧最终的动态特征点集。
本实施例中步骤S4过程如下：
S41、根据深度值对当前帧的特征点通过K-Means聚类算法将特征点聚成K类，并且计算出属于第i类的聚类中心ci，表示为：
Figure BDA0002521656860000111
其中，Ni为属于第i类的特征点数量，dik为第i类的第k个特征点的深度值；
S42、将每个投影动态点的深度值与每个聚类中心进行比较，找出与每个投影动态点的深度值之差的绝对值最小的聚类中心的类sj，表示为：
Figure BDA0002521656860000112
其中，sj为第j个投影动态点对应的类，depthj为第j个投影动态点的深度值，然后，统计每个类的投影动态点的数量，表示为：
Figure BDA0002521656860000121
其中，
Figure BDA0002521656860000122
Nproj为投影动态点的数量，numi为属于第i类的投影动态点的数量；
S43、统计属于每个类的投影动态点的数量，将投影动态点的数量numi大于阈值Td的类记为初始动态集。
S5、在根据深度值对当前帧的特征点进行K-Means聚类的基础上，再根据二维图像坐标(u，v)对当前帧的每个初始动态集中的特征点进行第二次K-Means聚类，得到最终动态集，最终动态集中的特征点记为动态点，当前帧除动态点外的所有特征点记为静态特征点；
本实施例中，步骤S5过程如下：
S51、在根据深度值对当前帧的特征点进行聚类的基础上，根据二维图像坐标(u，v)对每个初始动态集的特征点进行K-Means聚类，并且计算出第i个初始动态集的第j个类的聚类中心cij，表示为：
Figure BDA0002521656860000123
其中，Nij为属于第i个初始动态集的第j个类的特征点数量，uijk和vijk为属于第i个初始动态集的第j个类的第k个特征点的二维图像坐标；
进一步写成：
cij＝(uij,vij)
其中，(uij,vij)为聚类中心cij的二维坐标；
S52、将属于第i个初始动态集的投影动态点的二维图像坐标与第i个初始动态集的每个聚类中心cij进行比较，计算出与投影动态点pik的二维图像坐标的距离最小的类s(i，k)，表示为：
Figure BDA0002521656860000131
其中，(upik,vpik)为投影动态点pik的二维图像坐标；
S53、统计所有初始动态集的每个类的投影动态点的数量，表示如下：
Figure BDA0002521656860000132
Figure BDA0002521656860000133
其中，Nproj(i)为第i个初始动态集的投影动态点的数量，numij为第i个初始动态集的第j类的投影动态点的数量；
S54、统计属于每个类的投影动态点的数量，投影动态点的数量numij大于阈值Tuv的类为最终动态集。最终动态集的特征点记为动态特征点，如图4所示，本发明的动态特征点检测流程如图5所示。
S6、在时间维度上，计算当前帧每个特征点的静态观测数，得到每个特征点的静态观测权重；
动态特征点意味着这个特征点仅仅是在当前时刻被检测为动态特征点，并不是多帧都被检测为动态特征点。因此，由于偶然误差的存在，在当前时刻下，动态特征点检测机制可能将某个静态特征点误判为动态点，或者将某个动态点误判为静态特征点。因此，如果一个特征点事实上为静态特征点，那这个特征点在多帧中被判断为静态特征点的次数会很多。
本实施例中，步骤S6过程如下：
S61、在时间维度上统计当前帧每个特征点对应的地图点的静态观测数，表示规则如下：
从第一帧开始到当前帧，如果特征点pi被观测到并且被判定为静态特征点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)+1
其中，Vst(pi)为特征点pi的静态观测数，初始值为0，如果Vst(pi)大于静态观测阈值Tv,则特征点pi的静态观测数更新为：
Vst(pi)＝Tv
如果特征点pi被观测到，但特征点被判定为动态点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)-1；
S62、计算当前帧内所有特征点的静态观测数的均值μ及标准差S：
Figure BDA0002521656860000141
/>
Figure BDA0002521656860000142
其中Nm为当前帧对应的地图点的数量；
S63、计算当前帧内的每个特征点的静态观测权重Wvst(pi)：
Figure BDA0002521656860000143
其中β为大于0的常数。
S7、在几何维度上，计算当前帧每个特征点的静态概率，通过特征点的静态观测权重和静态概率得到该特征点的静态权重；
当动态特征点数与总特征点数之比较大时，此时如果直接剔除当前帧所有属于动态点集的特征点的话，可能会导致跟踪的特征点数过小，从而导致跟踪失败。
同时，可能会有一些动态物体上的特征点因为偶然误差没有被判断为动态特征点。但是，在这些没有被检测出来的动态特征点周围，很大概率存在已经被检测出来的动态特征点。因此，本发明不直接对动态特征点进行剔除，而是对当前帧建立静态概率图。
本实施例中，步骤S7过程如下：
S71、对当前帧建立静态概率图，将当前帧划分为Nr×Nc个块，每个块为Lu×Lv的长方形，块(i,j)的静态概率Wgst(i,j)为：
Figure BDA0002521656860000151
其中：
Figure BDA0002521656860000152
Figure BDA0002521656860000153
其中(i,j)为位于第i行第j列的方块的序号，numst(x,y)和numdy(x,y)分别为序号为(x,y)的方块的静态特征点数量和动态特征点数量；
S72、对于二维坐标为(ui,vi)的特征点pi，其静态概率Wgst(pi)被计算如下：
Figure BDA0002521656860000154
其中，符号int(·)为对括号内的数进行取整操作，至此，所有方块对应的静态概率构成当前帧的静态概率图；
S73、将静态概率Wgst(pi)与静态观测权重Wvst(pi)结合起来，得到特征点pi的静态权重Wst(pi)：
Wst(pi)＝Wgst(pi)+αWvst(pi)
其中，Wvst(pi)是当前帧内的每个特征点pi的静态观测权重，α为大于0的常数。
S8、对传统的RANSAC算法的特征点进行改进，进一步剔除当前帧误匹配的特征点和处于动态对象上的特征点，再将剩下的特征点对应的地图点重投影到当前帧，对每个特征点进行静态权重加权，最小化加权重投影之和，求得相机当前帧的最终位姿。
传统的RANSAC算法可以有效地对误匹配的点对和动态对象进行过滤，对噪声和低动态场景具有很强的鲁棒性。但是，当环境中的动态点过多时，RANSAC可能无法正常工作。因此，为了让RANSAC算法能更鲁棒地在动态场景下工作，本实施例对传统的RANSAC算法进行改进，提出了一种基于静态权重的RANSAC算法，具体改进分为特征点选取方法和目标函数两个方面。
在选取最小子集时，对于传统的RANSAC算法，每个特征点被选中的概率都是相同的，但是在动态场景下，对于属于静态物体上的特征点，它们应该有更高的概率被选中；同时，对于属于动态物体上的特征点，它们被选中的概率应该更低。
S81、基于上述理由，对传统的RANSAC算法的特征点选取方法进行改进，在当前帧的所有特征点中，根据静态权重，每个特征点被选取到的概率为；
Figure BDA0002521656860000171
其中，Wst(pi)为特征点pi的静态权重，pro(pi)为特征点pi被选中的概率，N为当前帧的特征点的数量；
然后，在传统的RANSAC算法中，目标函数f可以被看作：在第k次迭代过程中，在当前变换参数θk作用下，数据集中满足变换参数θk的点的个数，也就是在当前变换条件下内点的数量。而传统的RANSAC的目标函数也就是用来找出内点的数量最多的变换参数。但是在动态点较多的情况下，如果直接将内点的数量作为目标函数的评价标准，可能会将较多的动态点包含在内点集中，从而导致定位精度的降低。
S82、对传统的RANSAC算法的目标函数进行改进，将传统的RANSAC算法的目标函数改进为：
Figure BDA0002521656860000172
其中：
Figure BDA0002521656860000173
其中，f(·)为目标函数，θk为当前变换参数，0＜η＜1，pi为当前帧第i个特征点；
S83、将改进的RANSAC算法选取出的每个特征点(内点)对应的地图点重投影到当前帧，建立静态权重加权的重投影误差，表示为：
Figure BDA0002521656860000174
其中，ew为静态权重加权的重投影误差，Tcw为RGB-D相机当前帧的最终位姿，K为RGB-D相机的内参数矩阵，zi为特征点pi的对应的地图点；
S84、最小化静态权重加权的重投影误差ew，即可求得RGB-D相机当前帧的最终位姿Tcw。
本实施例在已公开的TUM数据集上对算法的性能进行了评估。所有评估都在华硕ZX50JX笔记本电脑上进行，该笔记本电脑配备英特尔酷睿i5-4200H CPU和4GB RAM，不使用GPU加速，系统为Ubuntu16.04。本发明使用TUM数据集的高动态、低动态和静态三种图像序列与ORB-SLAM2进行比较。其中高动态序列包括fr3/walking/static,fr3/walking/xyz和fr3/walking/halfphere三个图像序列；低动态序列包括fr3/sitting/static,fr3/sitting/xyz,fr3/sitting/rpy和fr3/sitting/halfphere四个图像序列；静态序列包括fr2/desk和fr3/long/office两个图像序列。图6为本发明的动态特征点检测结果图，可以看出本发明的算法能够较好地检测出动态物体上的特征点。图7为本发明与ORB-SLAM2的运动轨迹对比图，ORB-SLAM2是一个经典的实时SLAM系统。由图7可以看出，在高动态场景下，本发明的运动轨迹能较好地贴合真实轨迹，而ORB-SLAM2存在较大的定位误差。
表1.本发明的定位精度对比表
Figure BDA0002521656860000181
Figure BDA0002521656860000191
由表1可看出，对于高动态序列，与ORB-SLAM2相比，本发明定位精度提高了94.0％；对于静态序列，本发明同样能取得较高的定位精度。因此，本发明在静态和动态环境下都是鲁棒的。
最后，本发明的平均运行时间为48毫秒每帧，ORB-SLAM2的平均运行时间为44毫秒每帧，但是ORB-SLAM2在动态环境下并不鲁棒。因此，本发明不仅在动态环境下具有较高的精度，而且可以实时地在CPU上运行。
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (5)

1.一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述方法包括以下步骤：
S1、在RGB-D相机采集的当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算，将当前帧的RGB图像与上一帧的RGB图像进行描述子匹配，通过最小化重投影误差初步计算出当前帧的位姿；
S2、将当前帧的位姿与所有关键帧的位姿进行比较，找出位姿与当前帧相近并且时间上与当前帧相远的前m个关键帧，设为参考关键帧集，表示为：
Figure FDA0004173122990000011
其中，a,b,c,d为大于0的常数，id是关键帧的序号，idc是当前帧的序号，R是关键帧的旋转矩阵，t是关键帧的平移矩阵，Rc是当前帧的旋转矩阵，tc是当前帧的平移矩阵；
S3、将参考关键帧集中的每一帧的所有深度值大于0的特征点投影到当前帧上，找出投影后的深度值zproj与当前帧投影点位置的实际深度值z之差的绝对值Δz大于阈值Tz的点，记为投影动态点，表示为：
Δz＝|zproj-z|＞Tz；
S4、根据深度值对当前帧的特征点进行K-Means聚类，找出初始动态集；
S5、根据二维图像坐标(u，v)对当前帧的每个初始动态集中的特征点进行第二次K-Means聚类，得到最终动态集，最终动态集中的特征点记为动态点，当前帧除动态点外的所有特征点记为静态特征点；
所述步骤S5过程如下：
S51、根据二维图像坐标(u，v)对每个初始动态集的特征点进行K-Means聚类，并且计算出第i个初始动态集的第j个类的聚类中心cij，表示为：
Figure FDA0004173122990000021
其中，Nij为属于第i个初始动态集的第j个类的特征点数量，uijk和vijk为属于第i个初始动态集的第j个类的第k个特征点的二维图像坐标；
进一步写成：
cij＝(uij,vij)
其中，(uij,vij)为聚类中心cij的二维坐标；
S52、将属于第i个初始动态集的投影动态点的二维图像坐标与第i个初始动态集的每个聚类中心cij进行比较，计算出与投影动态点pik的二维图像坐标的距离最小的类s(i，k)，表示为：
Figure FDA0004173122990000022
其中，(upik,vpik)为投影动态点pik的二维图像坐标；
S53、统计所有初始动态集的每个类的投影动态点的数量，表示如下：
Figure FDA0004173122990000023
Figure FDA0004173122990000024
其中，Nproj(i)为第i个初始动态集的投影动态点的数量，numij为第i个初始动态集的第j类的投影动态点的数量；
S54、统计属于每个类的投影动态点的数量，投影动态点的数量numij大于阈值Tuv的类为最终动态集；
S6、在时间维度上，计算当前帧每个特征点的静态观测数，得到每个特征点的静态观测权重；
S7、在几何维度上，计算当前帧每个特征点的静态概率，通过特征点的静态观测权重和静态概率得到该特征点的静态权重；
S8、对传统的RANSAC算法的特征点进行改进，剔除当前帧误匹配的特征点和处于动态对象上的特征点，再将剩下的特征点对应的地图点重投影到当前帧，对每个特征点进行静态权重加权，最小化加权重投影之和，求得相机当前帧的最终位姿。
2.根据权利要求1所述的一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述步骤S4过程如下：
S41、根据深度值对当前帧的特征点通过K-Means聚类算法将特征点聚成K类，并且计算出属于第i类的聚类中心ci，表示为：
Figure FDA0004173122990000031
其中，Ni为属于第i类的特征点数量，dik为第i类的第k个特征点的深度值；
S42、将每个投影动态点的深度值与每个聚类中心进行比较，找出与每个投影动态点的深度值之差的绝对值最小的聚类中心的类sj，表示为：
Figure FDA0004173122990000032
其中，sj为第j个投影动态点对应的类，depthj为第j个投影动态点的深度值，然后，统计每个类的投影动态点的数量，表示为：
Figure FDA0004173122990000041
其中，
Figure FDA0004173122990000042
Nproj为投影动态点的数量，numi为属于第i类的投影动态点的数量；
S43、统计属于每个类的投影动态点的数量，将投影动态点的数量numi大于阈值Td的类记为初始动态集。
3.根据权利要求2所述的一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述步骤S6过程如下：
S61、在时间维度上统计当前帧每个特征点对应的地图点的静态观测数，表示规则如下：
从第一帧开始到当前帧，如果特征点pi被观测到并且被判定为静态特征点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)+1
其中，Vst(pi)为特征点pi的静态观测数，初始值为0，如果Vst(pi)大于静态观测阈值Tv,则特征点pi的静态观测数更新为：
Vst(pi)＝Tv
如果特征点pi被观测到，但特征点被判定为动态点，则特征点pi的静态观测数更新为：
Vst(pi)＝Vst(pi)-1；
S62、计算当前帧内所有特征点的静态观测数的均值μ及标准差S：
Figure FDA0004173122990000043
Figure FDA0004173122990000051
其中Nm为当前帧对应的地图点的数量；
S63、计算当前帧内的每个特征点的静态观测权重Wvst(pi)：
Figure FDA0004173122990000052
其中β为大于0的常数。
4.根据权利要求3所述的一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述步骤S7过程如下：
S71、对当前帧建立静态概率图，将当前帧划分为Nr×Nc个块，每个块为Lu×Lv的长方形，块(i,j)的静态概率Wgst(i,j)为：
Figure FDA0004173122990000053
其中：
Figure FDA0004173122990000054
Figure FDA0004173122990000055
其中(i,j)为位于第i行第j列的方块的序号，numst(x,y)和numdy(x,y)分别为序号为(x,y)的方块的静态特征点数量和动态特征点数量；
S72、对于二维坐标为(ui,vi)的特征点pi，其静态概率Wgst(pi)被计算如下：
Figure FDA0004173122990000056
其中，符号int(·)为对括号内的数进行取整操作，至此，所有方块对应的静态概率构成当前帧的静态概率图；
S73、将静态概率Wgst(pi)与静态观测权重Wvst(pi)结合起来，得到特征点pi的静态权重Wst(pi)：
Wst(pi)＝Wgst(pi)+αWvst(pi)
其中，Wvst(pi)是当前帧内的每个特征点pi的静态观测权重，α为大于0的常数。
5.根据权利要求4所述的一种RGB-D相机在动态场景下的SLAM方法，其特征在于，所述步骤S8过程如下：
S81、对传统的RANSAC算法的特征点选取方法进行改进，在当前帧的所有特征点中，根据静态权重，每个特征点被选取到的概率为；
Figure FDA0004173122990000061
其中，Wst(pi)为特征点pi的静态权重，pro(pi)为特征点pi被选中的概率，N为当前帧的特征点的数量；
S82、对传统的RANSAC算法的目标函数进行改进，将传统的RANSAC算法的目标函数改进为：
Figure FDA0004173122990000062
其中：
Figure FDA0004173122990000063
其中，f(·)为目标函数，θk为当前变换参数，η为取值范围为(0,1)的常数，pi为当前帧第i个特征点；
S83、将改进的RANSAC算法选取出的每个特征点对应的地图点重投影到当前帧，建立静态权重加权的重投影误差，表示为：
Figure FDA0004173122990000071
其中，ew为静态权重加权的重投影误差，Tcw为RGB-D相机当前帧的最终位姿，K为RGB-D相机的内参数矩阵，zi为特征点pi的对应的地图点；
S84、最小化静态权重加权的重投影误差ew，即可求得RGB-D相机当前帧的最终位姿Tcw。
CN202010492646.1A 2020-06-03 2020-06-03 一种rgb-d相机在动态场景下的slam方法 Active CN111914832B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010492646.1A CN111914832B (zh) 2020-06-03 2020-06-03 一种rgb-d相机在动态场景下的slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010492646.1A CN111914832B (zh) 2020-06-03 2020-06-03 一种rgb-d相机在动态场景下的slam方法

Publications (2)

Publication Number Publication Date
CN111914832A CN111914832A (zh) 2020-11-10
CN111914832B true CN111914832B (zh) 2023-06-13

Family

ID=73237453

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010492646.1A Active CN111914832B (zh) 2020-06-03 2020-06-03 一种rgb-d相机在动态场景下的slam方法

Country Status (1)

Country Link
CN (1) CN111914832B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112378409B (zh) * 2020-12-01 2022-08-12 杭州宇芯机器人科技有限公司 动态环境下基于几何与运动约束的机器人rgb-d slam方法
CN113111973A (zh) * 2021-05-10 2021-07-13 北京华捷艾米科技有限公司 一种基于深度相机的动态场景处理方法及装置
CN116051915A (zh) * 2023-02-22 2023-05-02 东南大学 基于聚类与几何残差的动态场景rgb-d 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方法
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN110349250A (zh) * 2019-06-28 2019-10-18 浙江大学 一种基于rgbd相机的室内动态场景的三维重建方法
CN110378345A (zh) * 2019-06-04 2019-10-25 广东工业大学 基于yolact实例分割模型的动态场景slam方法
CN110378997A (zh) * 2019-06-04 2019-10-25 广东工业大学 一种基于orb-slam2的动态场景建图与定位方法
CN111046856A (zh) * 2020-03-13 2020-04-21 南京晓庄学院 基于动静态特征提取并行位姿跟踪与地图创建的方法

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方法
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN110378345A (zh) * 2019-06-04 2019-10-25 广东工业大学 基于yolact实例分割模型的动态场景slam方法
CN110378997A (zh) * 2019-06-04 2019-10-25 广东工业大学 一种基于orb-slam2的动态场景建图与定位方法
CN110349250A (zh) * 2019-06-28 2019-10-18 浙江大学 一种基于rgbd相机的室内动态场景的三维重建方法
CN111046856A (zh) * 2020-03-13 2020-04-21 南京晓庄学院 基于动静态特征提取并行位姿跟踪与地图创建的方法

Also Published As

Publication number Publication date
CN111914832A (zh) 2020-11-10

Similar Documents

Publication Publication Date Title
CN111563442B (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN111914832B (zh) 一种rgb-d相机在动态场景下的slam方法
CN111696118B (zh) 动态场景下基于语义分割与图像修复的视觉回环检测方法
CN112132897A (zh) 一种基于深度学习之语义分割的视觉slam方法
Delmerico et al. Building facade detection, segmentation, and parameter estimation for mobile robot localization and guidance
CN111611643A (zh) 户型矢量化数据获得方法、装置、电子设备及存储介质
WO2021082168A1 (zh) 一种场景图像中特定目标对象的匹配方法
KR101460313B1 (ko) 시각 특징과 기하 정보를 이용한 로봇의 위치 추정 장치 및 방법
CN112037268B (zh) 一种动态场景下的基于概率传递模型的环境感知方法
CN111161318A (zh) 一种基于yolo算法及gms特征匹配的动态场景slam方法
Iraei et al. Object tracking with occlusion handling using mean shift, Kalman filter and edge histogram
CN109508674B (zh) 基于区域划分的机载下视异构图像匹配方法
CN113689459B (zh) 动态环境下基于gmm结合yolo实时跟踪与建图方法
CN107564008A (zh) 基于关键像素模糊聚类的快速sar图像分割方法
CN110807774A (zh) 一种点云分类与语义分割方法
CN113362358A (zh) 一种动态场景下基于实例分割的鲁棒性位姿估计方法
Min et al. Coeb-slam: A robust vslam in dynamic environments combined object detection, epipolar geometry constraint, and blur filtering
Han et al. Accurate and robust vanishing point detection method in unstructured road scenes
CN116299525A (zh) 一种基于点云区域相关性的动态环境rgb-d视觉slam方法
CN114283199B (zh) 一种面向动态场景的点线融合语义slam方法
CN108694348B (zh) 一种基于自然特征的跟踪注册方法及装置
CN114067240A (zh) 一种融合行人特性的在线更新策略行人单目标跟踪方法
CN112528937A (zh) 一种视频抽油机启停的检测方法
CN112084985A (zh) 一种基于cssd的电力检修行人多目标跟踪算法
CN116363218B (zh) 一种适用于动态环境的轻量化视觉slam方法

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