1.一种适用于动态环境的视觉SLAM方法,其特征在于:包括:对输入图像提取特征点,并将输入图像分为框内区域和框外区域;
对框外区域的特征点通过几何约束滤除框外动点以获取框外静点;
对框内区域的特征点进行速度计算以划分为框内动点和框内静点;
将框内静点和框外静点一起纳入位姿解算以获取最优位姿。
2.根据权利要求1所述的适用于动态环境的视觉SLAM方法,其特征在于:所述对输入图像提取特征点通过ORB‑SLAM2实现,所述将输入图像分为框内区域和框外区域通过YOLOv4实现。
3.根据权利要求2所述的适用于动态环境的视觉SLAM方法,其特征在于:所述对框外区域的特征点通过几何约束滤除框外动点以获取框外静点包括:设I1、I2分别为相机运动中相邻的两帧图像,分别对应两个相异时刻,O1、O2分别是两个相异时刻的相机光心,点P为一个空间点,它在I1、I2上的投影分别为P1、P2;以线段O1O2为基线,基线与I1、I2的交点分别为e1、e2;面O1O2P与I1和I2的交线l1及l2记为极线;
建立像素坐标系,设[u1,v1]和[u2,v2]分别是投影点在I1和I2时刻的像素坐标系坐标,极线l2在uv直角坐标系中表示为au2+bv2+c=0,则投影点到极线l2的距离为T
其中[a b c]=FK[u1v1 1],F表示I1到I2的基础矩阵,K表示相机参数矩阵;
设置阈值Dn,若d
4.根据权利要求3所述的适用于动态环境的视觉SLAM方法,其特征在于:所述对框外区域的特征点通过几何约束滤除框外动点以获取框外静点还包括:对于满足d
其中,Tcw_1和Tcw_2分别是I1和I2时刻世界坐标系到相机坐标系的变换矩阵,z为对应的深T T度坐标值,[u1 v1 z1]为I1时刻的投影坐标;实际投影坐标表示为P=[u2 v2 z2]。
若特征点满足约束条件: 则特征点定义为静
点,若不满足,则将该特征点作为动点剔除;其中,n表示图像金字塔的层数。
5.根据权利要求4所述的适用于动态环境的视觉SLAM方法,其特征在于:所述对框内区域的特征点进行速度计算以划分为框内动点和框内静点包括:T
获取t时刻框内区域的特征点在世界坐标系的坐标[Xt Yt Zt] =Twc_tKPt,其中,Twc_t表示t时刻相机坐标系到世界坐标系的变换矩阵,Pt表示t时刻的投影坐标;
计算框内区域的特征点在t时刻的速度Vt,Vt的表达式为:T T T T
Vt=([Xt+1 Yt+1 Zt+1]‑[Xt Yt Zt])/dt=[Vx_t Vy_t Vz_t] ;其中,[Xt+1 Yt+1 Zt+1] 表示相邻t+1时刻框内区域的特征点在世界坐标系的坐标,dt表示时间间隔;
将满足约束条件的特征点作为框内动点剔除,剩余的特征点作为框内静点;其中约束条件为:||Vt+1‑Vt||1<<||Vt+1+Vt||1。
6.根据权利要求1所述的适用于动态环境的视觉SLAM方法,其特征在于:采用最小化误差函数来求解所述最优位姿;
其中,所述最小化误差函数为:
Σ2=ε×Σ1=ε×n×E;
其中,Tcw_t表示世界坐标系到相机坐标系的变换矩阵,Si和Sj分别表示框外静点和框内静点的坐标,Pi和Pj分别表示框外静点和框内静点的投影坐标,Σ1和Σ2表示相应点的三阶权重矩阵也即信息矩阵,E表示三阶单位阵,ε表示权重系数。
7.根据权利要求6所述的适用于动态环境的视觉SLAM方法,其特征在于:所述将框内静点和框外静点一起纳入位姿解算以求解最优位姿还包括:求解框内静点和框外静点的重投影误差,并采用信息矩阵分配权重。
8.根据权利要求7所述的适用于动态环境的视觉SLAM方法,其特征在于:所述求解框内静点和框外静点的重投影误差,并采用信息矩阵分配权重包括:在ORB‑SLAM2的Bundle Adjustment中设置信息矩阵,这里选择为单位矩阵E,默认各个坐标值误差均摊;
ORB‑SLAM2使用了图像金字塔,然后在不同尺度上提取特征点,对于上层图像提取特征点,需要将特征点的坐标乘以层数n以使其坐标恢复为第一层上的尺度;
对于框内静点的坐标,在原信息矩阵的基础上再乘以ε,其中ε的取值范围为0.6至0.7;
T T
回归到高斯‑牛顿迭代法求解重投影误差,将投影误差项记为e ,其误差平方项为e Σe,其中Σ为相应的信息矩阵。
9.根据权利要求1所述的适用于动态环境的视觉SLAM方法,其特征在于:所述适用于动态环境的视觉SLAM方法还包括:在ORB‑SLAM2上增加稠密点云建图线程。
10.根据权利要求9所述的适用于动态环境的视觉SLAM方法,其特征在于:所述在ORB‑SLAM2上增加稠密点云建图线程包括:对跟踪线程tracking进行修改,将跟踪线程的关键帧插入到点云地图中,获取关键帧图像关联的点云id以及本帧相对于世界坐标系的变换;
联合RGB‑D图像的信息,找到对应点的空间位置,构造空间点云;
在ORB‑SLAM2的显示器线程viewer中对每一个关键帧中的点云通过刚体变换转到世界坐标系下;
通过PCL库函数统计滤波器和体素滤波器对点云作去外点和降采样的处理,将通过滤波的点云输出到全局地图;
ORB‑SLAM2的显示器线程viewer中增加PCL点云显示,在PCL自带的显示器窗口中显示点云地图。