CN114119732A - 基于目标检测和K-means聚类的联合优化动态SLAM方法 - Google Patents
基于目标检测和K-means聚类的联合优化动态SLAM方法 Download PDFInfo
- Publication number
- CN114119732A CN114119732A CN202111480233.2A CN202111480233A CN114119732A CN 114119732 A CN114119732 A CN 114119732A CN 202111480233 A CN202111480233 A CN 202111480233A CN 114119732 A CN114119732 A CN 114119732A
- Authority
- CN
- China
- Prior art keywords
- dynamic
- points
- current frame
- frame
- feature points
- 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
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000003064 k means clustering Methods 0.000 title claims abstract description 19
- 238000005457 optimization Methods 0.000 title claims abstract description 15
- 230000003068 static effect Effects 0.000 claims abstract description 49
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 30
- 238000000605 extraction Methods 0.000 claims description 3
- 239000000126 substance Substances 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 6
- 238000002474 experimental method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- QBWCMBCROVPCKQ-UHFFFAOYSA-N chlorous acid Chemical compound OCl=O QBWCMBCROVPCKQ-UHFFFAOYSA-N 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 238000002591 computed tomography Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000013450 outlier detection Methods 0.000 description 1
- 238000004540 process dynamic Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- 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
-
- 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
- G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/06—Topological mapping of higher dimensional structures onto lower dimensional surfaces
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Probability & Statistics with Applications (AREA)
- Image Analysis (AREA)
Abstract
本发明公开基于目标检测和K‑means聚类的联合优化动态SLAM方法，通过Yolov5目标检测算法和K‑means聚类算法可以准确的检测出动态物体在图像中所在的位置，进而可以精准的剔除掉动态物体上的动态特征点，使得求解的相机位姿精度大大提升。而且本发明采用了轻量级的Yolov5目标检测算法，计算成本短，耗时短，可以和SLAM***的跟踪线程并行，跟踪线程无需等待Yolov5的检测结果，所以本算法可以实时运行。最后本发明融入了极线约束和投影条件的方法，可以很好的剔除掉语义先验为静态物体运动产生的动态特征点。
Description
技术领域
本发明涉及定位与地图构建，尤其涉及基于目标检测和K-means聚类的联合优化动态SLAM方法。
背景技术
同时定位与地图构建(Simultaneous localization and mapping,SLAM)技术是实现机器人自主运动的关键技术，在移动机器人、自动驾驶、无人机和增强现实与虚拟现实(Augmented reality,Virtual reality,AR/VR)等领域有广泛的应用。SLAM的任务是通过机器人采集的传感器数据，在运动过程中同时完成对自身运动的估计和对环境地图的建立，即实现定位和建图两个目标。目前，SLAM技术可以分为动态SLAM和静态SLAM，其中ORB-SLAM2,LSD-SLAM,DSO等传统SLAM算法主要应用于静态场景下，而DS-SLAM和Dynamic-SLAM算法主要应用于动态场景下。
ORBSLAM2是最经典的基于静态场景下的SLAM***之一，该***包括三个线程，分别是跟踪、建图和回环检测。跟踪线程在跟踪参考关键帧时，通过随机采样一致性(RANSAC)算法将环境中的动态特征作为外点进行滤除，用剩下的特征匹配点对求解相机初始位姿。建图线程在局部地图中求解优化问题，将局部地图中的地图点重投影到当前帧，获得更多的3D-2D匹配点对，求解更为精准的相机位姿。回环检测线程闭合回环，利用位姿图优化消除累积误差。
ORBSLAM2中所采用滤除误匹配点对的RANSAC算法，是从当前帧中所有的3D-2D匹配点对中随机选取一组3D-2D点对来求解当前帧的位姿，用剩下的3D-2D点对来验证这个求得的位姿，如果满足这个求得的位姿的3D-2D点对数量大于设定的阈值，说明这个位姿是准确的，如果小于设定的阈值，则重新选取一组3D-2D点对来求解位姿，重复上述步骤。这种算法在低动态场景中，动态点数量较少时，可以达到一定效果。但是在高动态场景中，动态点数量很多，甚至场景中动态点数目大于静态点数目，此时求解出的位姿就会出现很大的误差，甚至导致跟踪和定位失败。
DS-SLAM是目前最经典的基于动态场景下的SLAM***之一，该算法处理动态场景下的动态特征主要分为三个部分。第一，首先采用SegNet神经网络对当前帧进行像素级语义分割，并假定人最有可能是动态对象，所以从中分割出人的区域。第二，在语义分割的同时，计算光流金字塔，得到当前帧中匹配的特征点，用RANSAC求解当前帧的位姿和基础矩阵，然后利用基本矩阵计算当前帧中的极线。最后，确定匹配点与其对应的极线之间的距离是否小于某个阈值。如果距离大于阈值，则该匹配点被判定为动态点。第三，如果落在人区域内的动态点数量大于设定阈值，则说明人是运动的，然后剔除掉人所在区域内的所有特征点，用剩下的特征点求解当前帧位姿。
DS-SLAM中也存在着许多缺点，第一，由于此时的场景存在着大量的动态特征，所以用RANSAC算法求解出的当前帧的位姿和基础矩阵存在很大误差，这样进而会导致后面用极线约束判定的动态点不准。第二，极线约束的外点检测方法并不能找到所有外点，当物体沿极线方向运动时这种方法会失效。第三，用物体区域特征点中的外点的比例来判断该物体是否运动，这用方法存在局限性，特征点的数量受物体纹理的影响较大。第四，SegNet语义分割网络运算成本大，耗时长，难以满足SLAM***的实时性。
发明内容
本发明的目的在于提供基于目标检测和K-means聚类的联合优化动态SLAM方法。
本发明采用的技术方案是：
基于目标检测和K-means聚类的联合优化动态SLAM方法，其包括以下步骤：
步骤S1：获取输入图像得到RGB图像及对应的深度图像，
步骤S2：对当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算；同时将当前帧的RGB图像用Yolov5目标检测得到物体的边界框位置和类别；
步骤S3：将检测出的物体边界框进行分类，把人物所在的边界框标记为动态物体框，把非人物的物体所在的边界框标记为静态物体框；
步骤S4：根据深度值当前帧的深度图像中动态物体框内的像素聚成k类；
步骤S5：从K-means聚类分割出的k类中找出像素点数量最大的类，把在最大类但不在静态物体框内的特征点标记为准动态特征点并剔除掉，把在其他类的但不在静态物体框内的特征点标记为疑似动态特征点，把其余剩下的特征点暂时标记为静态特征点；
步骤S6：将当前帧的RGB图像的静态特征点和疑似动态特征点与上一帧的RGB图像的特征点进行描述子匹配，得到Ni个静态特征匹配点对和Nj个疑似动态特征匹配点对；将静态特征匹配点对和疑似动态特征匹配点对进行加权最小化重投影误差，初步求解当前帧的位姿Tcw1，公式如下：
其中：∑1＝n×E,∑2＝0.4×n×E；xi为静态特征点坐标，pi为对应的投影坐标；xj为可疑动态特征点坐标，pj为对应的投影坐标，∑1为静态特征点的信息矩阵，∑2为可疑动态特征点点的信息矩阵，n为当前特征点所在图像金字塔的层数，E为2×2的单位矩阵；
步骤S7：在静态物体框中而实际运动但无法被Yolov5目标检测算法和K-means聚类算法检测到的特征点，用极限约束和投影条件检测出剩下少量动态特征点；
步骤S8：不满足极线约束和投影条件的剩余动态特征点剔除后，把剩下的特征点标记为最终静态特征点，将最终静态特征点对应的地图点投影到当前帧，建立最小重投影误差求解当前帧的最终位姿：
其中xi是准静态特征点对应的地图点坐标，pi为准静态特征点坐标，N为准静态特征点数量，Tcw为当前帧最终位姿。
进一步地，步骤S1中的RGB图像通过RGB-D相机采集得到。
进一步地，步骤S3中非人物的物体包括电脑、电视和椅子。
进一步地，步骤S4具体包括以下步骤：
步骤S41：随机在深度图像动态物体框内选出k个像素深度值作为聚类中心，初始化聚类中心为Z1(1),Z2(1)…Zk(1)；
其中，Zk(1)为第1次迭代(初始化)中的第k个聚类中心。
步骤S42：在m次迭代中，对于i＝1,2,…k，计算动态物体框内每一个像素的深度值Z与每个聚类中心Zi(m)深度值之差‖Z-Zi(m)‖，得到深度值之差最小的聚类中心Zi，并判定对应的像素点属于聚类中心，Z∈Si(m)；
其中Si(m)表示第m次迭代中第i类像素点集合。
步骤S43：计算新的聚类中心：
其中，Ni(m)为第m次迭代中属于第i类的像素点的数量，Zi(m+1)为第m+1次迭代中第i个聚类中心；
步骤S44：对所有的i＝1,2,…k，判定当前迭代的聚类中心和上一次迭代的聚类中心是否相等，即Zi(m+1)＝Zi(m)；是则，终止迭代；否则，迭代次数加1并转到步骤S43继续迭代计算。
进一步地，步骤S7中采用极线约束来检测动态点的具体步骤如下：
步骤S711：首先通过步骤S6得到的Ni对静态特征点求得基础矩阵，基础矩阵的求解可以表述为：
p2 TFp1＝0
其中，F为基础矩阵，p1和p2分别表示参考帧和当前帧匹配点的齐次坐标。
步骤S712：然后通过基础矩阵求解参考帧上的特征点对应到当前帧上的极线方程，极线方程表述为：
ax+by+c＝0
其中：[a b c]T＝FK[u1 v1 1]
其中：K为相机内参矩阵，[u1 v1 1]为参考帧中特征点的齐次坐标。
S713：求当前帧特征点到对应极线的极线距离的平方：
d2服从卡方分布，取95％置信度，则在金字塔第n层:d2的约束条件为：
d2＜3.84×1.22n
把不满足极线约束条件的当前帧的特征点标记为动态点。
进一步地，步骤S7中用投影条件来检测动态点，把局部地图中的地图点通过初始位姿Tcw1投影到当前帧，与当前帧的特征点进行匹配，求投影点和当前帧特征点的投影匹配误差；具体的步骤如下：
S721：求局部地图中的地图点在当前帧上的投影点坐标：
其中，
为局部地图中地图点的3D坐标，
为局部地图中的地图点投影到当前帧上的投影坐标，Tcw1为步骤S6中求得的当前帧初始位姿；
S722：特征点坐标u、v平方和服从2自由度的卡方分布，取95％置信度，求u、v的投影约束条件为：
其中(u2,v2)为当前帧中特征点的坐标，n为当前帧中特征点所在金字塔层级；
S723：设定深度相机最大误差为5％，得到z的投影约束条件为：
把不满足投影约束条件的当前帧的特征点标记为动态点。
本发明采用以上技术方案，在传统的ORBSLAM2算法流程上进行改进，着重解决传统SLAM算法无法处理环境中动态物体的问题，大大减小了动态物体对相机位姿估计的干扰，有效地提高了建图效果和定位精度。针对于动态场景中存在的动态物体，通过Yolov5目标检测算法可以有效的检测出动态物体在图像中的区域位置，而K-measn聚类算法可以通过Yolov5检测出的区域位置准确的检测出动态物体的轮廓位置和可疑动态区域。这样就可以精准的剔除动态物体上的动态特征，同时又较好地保留了静态特征，从而避免特征点过少而导致的SLAM***跟踪失效问题。
附图说明
以下结合附图和具体实施方式对本发明做进一步详细说明；
图1为本发明基于目标检测和K-means聚类的联合优化动态SLAM方法的结构示意图；
图2为在w_xyz数据集下的ORBSLAM2算法相机估计轨迹与真实轨迹图；
图3为w_xyz数据集下本发明算相机估计轨迹与真实轨迹图；
图4为在w_halfs数据集下ORBSLAM2算法相机估计轨迹与真实轨迹图；
图5为w_halfs数据集下本发明算法相机估计轨迹与真实轨迹图；
图6为在w_xyz数据集下的ORBSLAM2算法的误差分布图；
图7为w_xyz数据集下的本发明算法误差分布图；
图8为在w_halfs数据集下的ORBSLAM2算法的误差分布图；
图9为w_halfs数据集下的本发明算法误差分布图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图对本申请实施例中的技术方案进行清楚、完整地描述。
如图1至图9任一所示，本发明公开了基于目标检测和K-means聚类的联合优化动态SLAM方法，其包括以下步骤：
步骤S1：获取输入图像得到RGB图像及对应的深度图像，
步骤S2：对当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算；同时将当前帧的RGB图像用Yolov5目标检测得到物体的边界框位置和类别；
步骤S3：将检测出的物体边界框进行分类，把人物所在的边界框标记为动态物体框，把非人物的物体所在的边界框标记为静态物体框；
步骤S4：根据深度值当前帧的深度图像中动态物体框内的像素聚成k类；
步骤S5：从K-means聚类分割出的k类中找出像素点数量最大的类，把在最大类但不在静态物体框内的特征点标记为准动态特征点并剔除掉，把在其他类的但不在静态物体框内的特征点标记为疑似动态特征点，把其余剩下的特征点暂时标记为静态特征点；
步骤S6：将当前帧的RGB图像的静态特征点和疑似动态特征点与上一帧的RGB图像的特征点进行描述子匹配，得到Ni个静态特征匹配点对和Nj个疑似动态特征匹配点对；将静态特征匹配点对和疑似动态特征匹配点对进行加权最小化重投影误差，初步求解当前帧的位姿Tcw1，公式如下：
其中：∑1＝n×E,∑2＝0.4×n×E；xi为静态特征点坐标，pi为对应的投影坐标；xj为可疑动态特征点坐标，pj为对应的投影坐标，∑1为静态特征点的信息矩阵，∑2为可疑动态特征点点的信息矩阵，n为当前特征点所在图像金字塔的层数，E为2×2的单位矩阵；
步骤S7：在静态物体框中而实际运动但无法被Yolov5目标检测算法和K-means聚类算法检测到的特征点，用极限约束和投影条件检测出剩下少量动态特征点；
步骤S8：不满足极线约束和投影条件的剩余动态特征点剔除后，把剩下的特征点标记为最终静态特征点，将最终静态特征点对应的地图点投影到当前帧，建立最小重投影误差求解当前帧的最终位姿：
其中xi是准静态特征点对应的地图点坐标，pi为准静态特征点坐标，N为准静态特征点数量，Tcw为当前帧最终位姿。
进一步地，步骤S1中的RGB图像通过RGB-D相机采集得到。
进一步地，步骤S3中非人物的物体包括电脑、电视和椅子。
进一步地，步骤S4具体包括以下步骤：
步骤S41：随机在深度图像动态物体框内选出k个像素深度值作为聚类中心，初始化聚类中心为Z1(1),Z2(1)…Zk(1)；
其中，Zk(1)为第1次迭代(初始化)中的第k个聚类中心。
步骤S42：在m次迭代中，对于i＝1,2,…k，计算动态物体框内每一个像素的深度值Z与每个聚类中心Zi(m)深度值之差‖Z-Zi(m)‖，得到深度值之差最小的聚类中心Zi，并判定对应的像素点属于聚类中心，Z∈Si(m)；
其中Si(m)表示第m次迭代中第i类像素点集合。
步骤S43：计算新的聚类中心：
其中，Ni(m)为第m次迭代中属于第i类的像素点的数量，Zi(m+1)为第m+1次迭代中第i个聚类中心；
步骤S44：对所有的i＝1,2,…k，判定当前迭代的聚类中心和上一次迭代的聚类中心是否相等，即Zi(m+1)＝Zi(m)；是则，终止迭代；否则，迭代次数加1并转到步骤S43继续迭代计算。
进一步地，步骤S7中采用极线约束来检测动态点的具体步骤如下：
步骤S711：首先通过步骤S6得到的Ni对静态特征点求得基础矩阵，基础矩阵的求解可以表述为：
p2 TFp1＝0
其中，F为基础矩阵，p1和p2分别表示参考帧和当前帧匹配点的齐次坐标。
步骤S712：然后通过基础矩阵求解参考帧上的特征点对应到当前帧上的极线方程，极线方程表述为：
ax+by+c＝0
其中：[a b c]T＝FK[u1 v1 1]
其中：K为相机内参矩阵，[u1 v1 1]为参考帧中特征点的齐次坐标。
S713：求当前帧特征点到对应极线的极线距离的平方：
d2服从卡方分布，取95％置信度，则在金字塔第n层:d2的约束条件为：
d2＜3.84×1.22n
把不满足极线约束条件的当前帧的特征点标记为动态点。
进一步地，步骤S7中用投影条件来检测动态点，把局部地图中的地图点通过初始位姿Tcw1投影到当前帧，与当前帧的特征点进行匹配，求投影点和当前帧特征点的投影匹配误差；具体的步骤如下：
S721：求局部地图中的地图点在当前帧上的投影点坐标：
其中，
为局部地图中地图点的3D坐标，
为局部地图中的地图点投影到当前帧上的投影坐标，Tcw1为步骤S6中求得的当前帧初始位姿；
S722：特征点坐标u、v平方和服从2自由度的卡方分布，取95％置信度，求u、v的投影约束条件为：
其中(u2,v2)为当前帧中特征点的坐标，n为当前帧中特征点所在金字塔层级；
S723：设定深度相机最大误差为5％，得到z的投影约束条件为：
把不满足投影约束条件的当前帧的特征点标记为动态点。
本发明的具体效果验证实验：
本发明算法测试实验在华硕A456UR7200笔记本上进行，其操作***为Ubuntul8.04，该笔记本电脑配备lntel酷睿5-7200H CPU,显卡为NVIDIA GEFORCE 930MX。Yolov5目标检测部分由python语言编写，SLAM部分由C++语言编写，两个部分之间由Unix域套接字进行通信。
实验使用TUM RGB-D数据集来对算法的性能进行评估，TUM数据集是由德国慕尼黑大学制作，使用Kinect传感器以30HZ的速率采集信息，图像分辨率为640*480,在采集图像数据的同时，使用高精度运动捕捉***VICON与惯性测量***实时获取相机位置与姿态的数据，可近似作为RGB-D相机的真实位姿数据。本发明主要使用TUM RGB-D数据集中的5个动态场景序列进行实验，这4个序列分为walking和sitting两类，Sitting数据集系列包括freiburg3 sitting xyz(s_xyz)，sitting数据集系列场景为：两个人坐在桌子前聊天。
walking数据集系列包括：freiburg3 walking xyz(w_xyz),freiburg3 walkingstatic(w_static)，freiburg3 walking halfsphere(w_halfs)序列，walking数据梊系列的场景为：两个实验人员在桌子前或围绕桌子走动，同时相机也处于不同的运动状态中。其中static表示相机处于静止状态，xyz表示相机沿着空间X-Y-Z轴平移运动，rpy表示相机在翻转角、俯仰角和偏航角上旋转，halfsphere表示相机沿着直径为1m的半球面的轨迹移动。
首先将本发明算与传统的ORBSLAM2算法进行对比，选取绝对位姿误差(ATE)作为算法精度的评判标准。实验结果如表1所示，其中RMSE表示均方根误差，Mean表示均值误差，Std表示方差。
表1 ORB-SLAM2算法与本发明算法的绝对轨迹误差(ATE)对比
图2为在w_xyz数据集下的ORBSLAM2算法相机估计轨迹与真实轨迹图，图3为w_xyz数据集下本发明算相机估计轨迹与真实轨迹图，由图2和3可以看出，在高动态场景下，本发明的运动轨迹能较好地贴合真实轨迹，而ORB-SLAM2存在较大的定位误差。
图4为在w_halfs数据集下ORBSLAM2算法相机估计轨迹与真实轨迹图，图5为w_halfs数据集下本发明算法相机估计轨迹与真实轨迹图。
图6为在w_xyz数据集下的ORBSLAM2算法的误差分布图，图7为w_xyz数据集下的本发明算法误差分布图。
图8为在w_halfs数据集下的ORBSLAM2算法的误差分布图，图9为w_halfs数据集下的本发明算法误差分布图。
除此之外，此外，还将本发明算法和最先进的动态SLAM算法，即DynaSLAM、DS-SLAM等进行对比试验，分别比较相机绝对位姿估计的均方根误差，如表2所示，粗1体表示最优结果。
表2其他动态SLAM算法与本发明算法的绝对轨迹误差(ATE)对比
| DS-SLAM | DynaSLAM | 本发明SLAM算法 | |
| w_xyz | 0.0247 | 0.0150 | 0.0146 |
| w_static | 0.0081 | 0.0090 | 0.0091 |
| w_halfs | 0.0303 | 0.0250 | 0.0277 |
| s_xyz | 0.0065 | 0.0150 | 0.0107 |
本发明采用以上技术方案，通过Yolov5目标检测算法和K-means聚类算法可以准确的检测出动态物体在图像中所在的位置，进而可以精准的剔除掉动态物体上的动态特征点，使得求解的相机位姿精度大大提升。而且本发明采用了轻量级的Yolov5目标检测算法，计算成本短，耗时短，可以和SLAM***的跟踪线程并行，跟踪线程无需等待Yolov5的检测结果，所以本算法可以实时运行。最后本发明融入了极线约束和投影条件的方法，可以很好的剔除掉语义先验为静态物体运动产生的动态特征点。
显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
Claims (6)
1.基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：其包括以下步骤：
步骤S1：获取输入图像得到RGB图像及对应的深度图像，
步骤S2：对当前帧的RGB图像上进行ORB特征点提取和对所提取的ORB特征点进行描述子计算；同时将当前帧的RGB图像用Yolov5目标检测得到物体的边界框位置和类别；
步骤S3：将检测出的物体边界框进行分类，把人物所在的边界框标记为动态物体框，把非人物的物体所在的边界框标记为静态物体框；
步骤S4：根据深度值当前帧的深度图像中动态物体框内的像素聚成k类；
步骤S5：从K-means聚类分割出的k类中找出像素点数量最大的类，把在最大类但不在静态物体框内的特征点标记为准动态特征点并剔除掉，把在其他类的但不在静态物体框内的特征点标记为疑似动态特征点，把其余剩下的特征点暂时标记为静态特征点；
步骤S6：将当前帧的RGB图像的静态特征点和疑似动态特征点与上一帧的RGB图像的特征点进行描述子匹配，得到Ni个静态特征匹配点对和Nj个疑似动态特征匹配点对；将静态特征匹配点对和疑似动态特征匹配点对进行加权最小化重投影误差，初步求解当前帧的位姿Tcw1，公式如下：
其中：∑1＝n×E，∑2＝0.4×n×E；xi为静态特征点坐标，pi为对应的投影坐标；xj为可疑动态特征点坐标，pj为对应的投影坐标，∑1为静态特征点的信息矩阵，∑2为可疑动态特征点点的信息矩阵，n为当前特征点所在图像金字塔的层数，E为2×2的单位矩阵；
步骤S7：在静态物体框中而实际运动但无法被Yolov5目标检测算法和K-means聚类算法检测到的特征点，用极限约束和投影条件检测出剩下少量动态特征点；
步骤S8：不满足极线约束和投影条件的剩余动态特征点剔除后，把剩下的特征点标记为最终静态特征点，将最终静态特征点对应的地图点投影到当前帧，建立最小重投影误差求解当前帧的最终位姿：
其中xi是准静态特征点对应的地图点坐标，pi为准静态特征点坐标，N为准静态特征点数量，Tcw为当前帧最终位姿。
2.根据权利要求1所述的基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：步骤S1中的RGB图像通过RGB-D相机采集得到。
3.根据权利要求1所述的基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：步骤S3中非人物的物体包括电脑、电视和椅子。
4.根据权利要求1所述的基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：步骤S4具体包括以下步骤：
步骤S41：随机在深度图像动态物体框内选出k个像素深度值作为聚类中心，初始化聚类中心为Z1(1)，Z2(1)…Zk(1)；
其中，Zk(1)为第1次迭代中的第k个聚类中心。
步骤S42：在m次迭代中，对于i＝1，2，…k，计算动态物体框内每一个像素的深度值Z与每个聚类中心Zi(m)深度值之差||Z-Zi(m)||，得到深度值之差最小的聚类中心Zi，并判定对应的像素点属于聚类中心，Z∈Si(m)；
其中，Si(m)表示第m次迭代中第i类像素点的集合。
步骤S43：计算新的聚类中心：
其中，Ni(m)为第m次迭代中属于第i类的像素点的数量，Zi(m+1)为第m+1次迭代中第i个聚类中心；
步骤S44：对所有的i＝1，2，…k，判定当前迭代的聚类中心和上一次迭代的聚类中心是否相等，即Zi(m+1)＝Zi(m)；是则，终止迭代；否则，迭代次数加1并转到步骤S43继续迭代计算。
5.根据权利要求1所述的基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：步骤S7中采用极线约束来检测动态点的具体步骤如下：
步骤S711：首先通过步骤S6得到的Ni对静态特征点求得基础矩阵，基础矩阵的求解可以表述为：
p2 TFp1＝0
其中，F为基础矩阵，p1和p2分别表示参考帧和当前帧匹配点的齐次坐标。
步骤S712：然后通过基础矩阵求解参考帧上的特征点对应到当前帧上的极线方程，极线方程表述为：
ax+by+c＝0
其中：[a b c]T＝FK[u1 v1 1]
其中：K为相机内参矩阵，[u1 v1 1]为参考帧中特征点的齐次坐标。
S713：求当前帧特征点到对应极线的极线距离的平方：
d2服从卡方分布，取95％置信度，则在金字塔第n层：d2的约束条件为：
d2＜3.84×1.22n
把不满足极线约束条件的当前帧的特征点标记为动态点。
6.根据权利要求1所述的基于目标检测和K-means聚类的联合优化动态SLAM方法，其特征在于：步骤S7中用投影条件来检测动态点，把局部地图中的地图点通过初始位姿Tcw1投影到当前帧，与当前帧的特征点进行匹配，求投影点和当前帧特征点的投影匹配误差；具体的步骤如下：
S721：求局部地图中的地图点在当前帧上的投影点坐标：
其中，
为局部地图中地图点的3D坐标，
为局部地图中的地图点投影到当前帧上的投影坐标，Tcw1为步骤S6中求得的当前帧初始位姿；
S722：特征点坐标u、v平方和服从2自由度的卡方分布，取95％置信度，求u、v的投影约束条件为：
其中(u2，v2)为当前帧中特征点的坐标，n为当前帧中特征点所在金字塔层级；
S723：设定深度相机最大误差为5％，得到z的投影约束条件为：
把不满足投影约束条件的当前帧的特征点标记为动态点。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111480233.2A CN114119732A (zh) | 2021-12-06 | 2021-12-06 | 基于目标检测和K-means聚类的联合优化动态SLAM方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111480233.2A CN114119732A (zh) | 2021-12-06 | 2021-12-06 | 基于目标检测和K-means聚类的联合优化动态SLAM方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114119732A true CN114119732A (zh) | 2022-03-01 |
Family
ID=80367027
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111480233.2A Pending CN114119732A (zh) | 2021-12-06 | 2021-12-06 | 基于目标检测和K-means聚类的联合优化动态SLAM方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114119732A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114926536A (zh) * | 2022-07-19 | 2022-08-19 | 合肥工业大学 | 一种基于语义的定位与建图方法、***及智能机器人 |
| CN116067360A (zh) * | 2023-01-09 | 2023-05-05 | 安徽工程大学 | 一种基于双重约束的机器人地图构建方法、存储介质及设备 |
| CN116399350A (zh) * | 2023-05-26 | 2023-07-07 | 北京理工大学 | 一种融合YOLOv5的半直接法视觉里程计的确定方法 |
-
2021
- 2021-12-06 CN CN202111480233.2A patent/CN114119732A/zh active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114926536A (zh) * | 2022-07-19 | 2022-08-19 | 合肥工业大学 | 一种基于语义的定位与建图方法、***及智能机器人 |
| CN114926536B (zh) * | 2022-07-19 | 2022-10-14 | 合肥工业大学 | 一种基于语义的定位与建图方法、***及智能机器人 |
| CN116067360A (zh) * | 2023-01-09 | 2023-05-05 | 安徽工程大学 | 一种基于双重约束的机器人地图构建方法、存储介质及设备 |
| CN116067360B (zh) * | 2023-01-09 | 2023-08-22 | 安徽工程大学 | 一种基于双重约束的机器人地图构建方法、存储介质及设备 |
| CN116399350A (zh) * | 2023-05-26 | 2023-07-07 | 北京理工大学 | 一种融合YOLOv5的半直接法视觉里程计的确定方法 |
| CN116399350B (zh) * | 2023-05-26 | 2023-09-01 | 北京理工大学 | 一种融合YOLOv5的半直接法视觉里程计的确定方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Dai et al. | Rgb-d slam in dynamic environments using point correlations | |
| CN111462200B (zh) | 一种跨视频行人定位追踪方法、***及设备 | |
| CN108460779B (zh) | 一种动态环境下的移动机器人图像视觉定位方法 | |
| CN110555901B (zh) | 动静态场景的定位和建图方法、装置、设备和存储介质 | |
| CN114119732A (zh) | 基于目标检测和K-means聚类的联合优化动态SLAM方法 | |
| Boniardi et al. | Robot localization in floor plans using a room layout edge extraction network | |
| Raposo et al. | Theory and practice of structure-from-motion using affine correspondences | |
| WO2008070206A2 (en) | A seamless tracking framework using hierarchical tracklet association | |
| CN110827321B (zh) | 一种基于三维信息的多相机协同的主动目标跟踪方法 | |
| CN106780567B (zh) | 一种融合颜色和梯度直方图的免疫粒子滤波扩展目标跟踪方法 | |
| CN111914832B (zh) | 一种rgb-d相机在动态场景下的slam方法 | |
| Shalnov et al. | Convolutional neural network for camera pose estimation from object detections | |
| Wang et al. | Effective multiple pedestrian tracking system in video surveillance with monocular stationary camera | |
| Yu et al. | Drso-slam: A dynamic rgb-d slam algorithm for indoor dynamic scenes | |
| Savitha et al. | Motion detection in video surviellance: A systematic survey | |
| CN114549629A (zh) | 水下单目视觉估计目标三维位姿的方法 | |
| Min et al. | Coeb-slam: A robust vslam in dynamic environments combined object detection, epipolar geometry constraint, and blur filtering | |
| Zhuang et al. | Amos-SLAM: An Anti-Dynamics Two-stage SLAM Approach | |
| CN111160362B (zh) | 一种fast特征均匀化提取与帧间特征误匹配去除方法 | |
| CN110163112B (zh) | 一种考生姿态分割与平滑方法 | |
| WO2023178951A1 (zh) | 图像分析方法、模型的训练方法、装置、设备、介质及程序 | |
| CN116429087A (zh) | 一种适应于动态环境的视觉slam方法 | |
| Li et al. | A Target Tracking Model for Enterprise Production Monitoring System Based on Spatial Information and Appearance Model. | |
| Wang et al. | Pmds-slam: Probability mesh enhanced semantic slam in dynamic environments | |
| Zhao et al. | Real-time human segmentation by bowtienet and a slam-based human ar system |
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 |