CN113720323A - 基于点线特征融合的单目视觉贯导slam方法及装置 - Google Patents
基于点线特征融合的单目视觉贯导slam方法及装置 Download PDFInfo
- Publication number
- CN113720323A CN113720323A CN202110873290.0A CN202110873290A CN113720323A CN 113720323 A CN113720323 A CN 113720323A CN 202110873290 A CN202110873290 A CN 202110873290A CN 113720323 A CN113720323 A CN 113720323A
- Authority
- CN
- China
- Prior art keywords
- camera
- imu
- representing
- point
- frame
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 89
- 230000004927 fusion Effects 0.000 title claims abstract description 26
- 238000001514 detection method Methods 0.000 claims abstract description 55
- 230000008569 process Effects 0.000 claims abstract description 54
- 125000004122 cyclic group Chemical group 0.000 claims abstract description 50
- 238000012937 correction Methods 0.000 claims abstract description 49
- 238000005457 optimization Methods 0.000 claims abstract description 25
- 230000033001 locomotion Effects 0.000 claims abstract description 13
- 230000007613 environmental effect Effects 0.000 claims abstract description 7
- 238000005259 measurement Methods 0.000 claims description 41
- 230000005484 gravity Effects 0.000 claims description 30
- 238000000605 extraction Methods 0.000 claims description 22
- 230000000007 visual effect Effects 0.000 claims description 16
- 238000012935 Averaging Methods 0.000 claims description 8
- 230000010354 integration Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 description 10
- 230000009466 transformation Effects 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010606 normalization Methods 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000008570 general process Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了基于点线特征融合的单目视觉贯导SLAM方法及装置，所述方法包括以下步骤：将单目相机获取的图像和IMU获取的环境信息作为输入信息；相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；重复相机轨迹进行循环检测和循环修正，得到新的局部地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图；本发明的优点在于：具有更高的精度和更小的轨迹误差。
Description
技术领域
本发明涉及地图构建领域，更具体涉及基于点线特征融合的单目视觉贯导SLAM方法及装置。
背景技术
随着SLAM(同步定位与建图)技术的逐渐发展，基于视觉的SLAM算法逐渐成为当今研究的热点。基于点特征的单目视觉SLAM方法是一种以单目相机为传感器，来检测提取周围环境角点的方法。利用此种算法使用提取的特征点来估计相机位姿和3D地图，具有较好的表现的同时，计算速度也很快。现有的比较成熟的算法有：PTAM、ORB-SLAM、ORB-SLAM2等等。但是基于点特征的单目视觉SLAM方法要保证周围环境的纹理程度，其在低纹理场景中，表现不好。基于线特征的视觉SLAM算法是针对在低纹理场景中点特征算法提取不足或者失败而提出的。此算法是利用对周围环境的线特征提取以及匹配来完成对机器人的定位和对环境的构图。线特征的好处在于能够描述环境的边缘信息，丰富环境中的几何信息，能够很好的描述低纹理的环境信息。此种算法不依赖环境的点特征，而完全依赖以对环境的线特征提取与匹配。现有的比较成熟的技术有：LSD、LCD等。但是基于线特征的视觉SLAM算法仅使用线特征对计算资源要求极大，会使计算速度变慢，在纹理环境中就会丧失优势。基于深度学习的动态视觉SLAM算法，是基于神经网络技术对动态物体进行识别，然后剔除动态物体，最后对动态物体以外的环境匹配与构图。主要技术点在于使用神经网络(比如CNN)识别出已经提取出特征点的动态物体，把它剔除不用，利用剩余的静态物体进行计算，由于其加入深度学习技术，增加操作的复杂性且动态物体的判别比较繁杂。基于上述几种方法在低纹理场景中不适用以及操作复杂的问题，现有技术涌现了点线特征融合的视觉贯导SLAM方法，通过点线融合的方式提高定位准确性的同时适用于低纹理场景。
中国专利申请公开号CN109579840A，公开了一种点线特征融合的紧相合双目视觉惯性SLAM方法，包括以下步骤：确定相机坐标系与惯性传感器坐标系的变换关系：建立点钱特征加IMU跟踪线手里，求解初始的立体点线三维坐标，在IMU初始化后，使用IMU预测特征点线的位置，建立正确特征数据关联，结合IMU和点线特征重投影误差项，求解连续帧的位姿变换；建立点线特征加IMU局部光束法平差线程，在局部关键帧窗口中优化点线三维坐标，关键帧位姿以及IMU的状态量；建立点线特征回环检测线程，使用点线特征加权计算词袋模型的得分来检测回环，并进行全局状态量的优化。该专利申请能够在特征点数目较少以及相机快速运动情况下保证稳定和高精度，适用于低纹理场景，但是其对词带模型中的点线进行加权，前期由于点线提取不足而引起的误差就会积累下来，引起后期的较大的轨迹误差。
发明内容
本发明所要解决的技术问题在于现有技术基于点线融合的视觉惯性SLAM方法的轨迹误差较大，导致最终生成的地图精度不高的问题。
本发明通过以下技术手段实现解决上述技术问题的：基于点线特征融合的单目视觉贯导SLAM方法，所述方法包括以下步骤：
S1：将单目相机获取的图像和IMU获取的环境信息作为输入信息；
S2：基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
S3：对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；
S4：***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；
S5：所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
本发明将单目相机和IMU分别输入先验信息，来确定相机移动过程，剔除误差较大的数据后进行BA优化，然后重复相机轨迹经过循环检测和循环修正，轨迹平均得出最终轨迹也即最终局部地图，此过程具有更高的精度和更小的轨迹误差。
进一步地，所述步骤S1之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s,Rwg,b,v0:k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0,0,G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0:k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0:k)∝p(I0:k|yk)p(yk)，其中，I0:k表示首帧图像到末帧图像的测量信息，
表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0:k)最大化的后验分布，∝表示相当于，p(I0:k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度；
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1,i表示相邻两帧的测量信息。
进一步地，所述步骤S2包括：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。
进一步地，所述步骤S2中所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj,i、j＝1,2,3…，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
进一步地，所述步骤S2中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。
进一步地，所述步骤S4中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。
本发明还提供基于点线特征融合的单目视觉贯导SLAM装置，所述装置包括：
信息获取模块，用于将单目相机获取的图像和IMU获取的环境信息作为输入信息；
关键帧获取模块，用于基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
BA优化模块，用于对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；
检测修正模块，用于***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；
全局优化模块，用于所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
进一步地，所述信息获取模块之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s,Rwg,b,v0:k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0,0,G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0:k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0:k)∝p(I0:k|yk)p(yk)，其中，I0:k表示首帧图像到末帧图像的测量信息，
表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0:k)表示最大化的后验分布，∝表示相当于，p(I0:k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度；
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1,i表示相邻两帧的测量信息。
进一步地，所述关键帧获取模块还用于：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。
进一步地，所述关键帧获取模块中所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj,i、j＝1,2,3…，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
进一步地，所述关键帧获取模块中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。
进一步地，所述检测修正模块中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。
本发明的优点在于：本发明将单目相机和IMU分别输入先验信息，来确定相机移动过程，剔除误差较大的数据后进行BA优化，然后重复相机轨迹经过循环检测和循环修正，轨迹平均得出最终轨迹也即最终局部地图，此过程具有更高的精度和更小的轨迹误差。
附图说明
图1为本发明实施例所公开的基于点线特征融合的单目视觉贯导SLAM方法的流程图；
图2为本发明实施例所公开的基于点线特征融合的单目视觉贯导SLAM方法中投影线与匹配线段的关系示意图；
图3为本发明实施例所公开的基于点线特征融合的单目视觉贯导SLAM方法中匹配线段端点到投影线的距离的示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
如图1所示，基于点线特征融合的单目视觉贯导SLAM方法，所述方法包括以下步骤：
S1：将单目相机获取的图像和IMU获取的环境信息作为输入信息；相机和IMU是并行运行的，分别提取出图像信息和环境测量信息，然后通过两者紧密耦合的方式对两者的环境信息进行初始化。所谓紧密耦合就是指两个者采集的信息综合在一起作为输入信息，而不是单独作为输入信息。需要说明的是，所述步骤S1之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s,Rwg,b,v0:k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0,0,G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0:k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0:k)∝p(I0:k|yk)p(yk)，其中，I0:k表示首帧图像到末帧图像的测量信息，
表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0:k)表示最大化的后验分布，∝表示相当于，p(I0:k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度。
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1,i表示相邻两帧的测量信息。
S2：基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
本实施例中，通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。FAST角点提取算法属于现有技术，其大致过程如下：
(1)在图像中选取像素p，假设它的亮度为Ip。
(2)设置一个阈值T(比如，Ip的10％)。
(3)以像素p为中心，选取半径为3的圆上的16个像素点。
(4)假设选取的圆上有连续的N个点的亮度大于Ip+T或小于Ip-T，那么像素p可以被认为是点特征。
(5)循环以上四部，对每一个像素执行相同的操作。
在提取点、线特征时，考虑到本发明是在低纹理下进行的，点特征不易被提取这一根本前提。对点、线特征进行加权处理。本发明基于低纹理场景下进行的，线特征提取数量比点特征多，并且因此在加权处理时以线特征为主，点特征为副进行加权，此种加权方式是对环境提取到的有效的点线特征进行加权，用此种方式加权可以在点线特征提取时，连续几帧没有提取到点特征时，依旧可以依照线特征初始化地图，不影响***的有序进行。因此本发明根据线特征的个数来划分点、线特征的比重，避免了复杂的权值分配计算，保证***的实时性。设置线特征个数的阈值为N。
点线特征权重wp和wl分布如下：
Nl>N：wp＝0.2，wl＝0.8
Nl<N：
其中，Nl表示线匹配成功的个数。
所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj,i、j＝1,2,3…，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
在进行线特征匹配前应该对空间中的特征线进行数学化表示，便于计算机描述。设空间直线L两个端点的齐次坐标为P1＝[x1,y1,z1,w1]T和P2＝[x2,y2,z2,w2]T，其普吕克矩阵定义为
则其非齐次坐标为
和
则直线的普吕克坐标为：
其中，L是个6×1的向量，包含n和v，n是直线的矩向量，v是直线的方向向量，n垂直于由原点和直线决定的平面。w1和w2表示齐次因子。
普吕克矩阵T和坐标L的转换关系如下：
其中，n∧表示直线的矩向量n的反对称矩阵。
设直线变换矩阵Hcw，它由旋转矩阵
和平移向量
构成。Lc是空间中直线Lw由空间坐标系转换到相机坐标系的坐标。把Lc用普吕克坐标表示为：
其中，
表示平移向量的反对称矩阵。
设直线Lc投影到图像平面上为lc，其普吕克坐标表示为：
其中Kl表示直线的投影矩阵，fx、fy、cx、cy表示相机内参数里边的焦距和图像中心(每个相机都有固定的焦距和图像中心)，nc表示普吕克表达中的法向量，l1、l2、l3表示直线lc的系数，lc＝l1x+l2y+l3z。
如图2及图3所示，单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
所述步骤S2中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。
S3：对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；BA优化的过程为现有技术，在CSDN博客上Jinterest2020年1月14日上传的文献《SLAM十四讲_后端1》公开了BA优化的具体过程如下：
把世界坐标转换到相机坐标，用到相机的旋转矩阵R和平移矩阵t。
P′＝Rp+t＝[X′,Y′,Z′]
将P′投至归一化平面，得到归一化坐标：
考虑归一化坐标的畸变情况，得到去畸变前的原始像素坐标
根据内参模型，计算像素坐标：
把相机在移动过程中的观测数据记为：
z＝h(x,y)
x指代相机的位姿，即对应的是变换矩阵T，y指代三维点p，则观测误差为：
e＝z-h(T,p)
把其他时刻的观测量考虑进来。设zij为在位姿Ti处观察路标pj产生的数据，那么整体的代价函数为：
对这个最小二乘求解，相当于对位姿和路标同时做了调整，也就是所谓的BA。
S4：***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；其中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。
S5：所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
通过以上技术方案，本发明将单目相机和IMU分别输入先验信息，来确定相机移动过程，剔除误差较大的数据后进行BA优化，然后重复相机轨迹经过循环检测和循环修正，轨迹平均得出最终轨迹也即最终局部地图，此过程具有更高的精度和更小的轨迹误差。
实施例2
基于本发明实施例1，本发明实施例2还提供基于点线特征融合的单目视觉贯导SLAM装置，所述装置包括：
信息获取模块，用于将单目相机获取的图像和IMU获取的环境信息作为输入信息；
关键帧获取模块，用于基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
BA优化模块，用于对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；
检测修正模块，用于***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；
全局优化模块，用于所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
具体的，所述信息获取模块之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s,Rwg,b,v0:k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0,0,G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0:k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0:k)∝p(I0:k|yk)p(yk)，其中，I0:k表示首帧图像到末帧图像的测量信息，
表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0:k)表示最大化的后验分布，∝表示相当于，p(I0:k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度；
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1,i表示相邻两帧的测量信息。
具体的，所述关键帧获取模块还用于：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。
具体的，所述关键帧获取模块中所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj,i、j＝1,2,3…，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
具体的，所述关键帧获取模块中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。
具体的，所述检测修正模块中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述方法包括以下步骤：
S1：将单目相机获取的图像和IMU获取的环境信息作为输入信息；
S2：基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
S3：对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；
S4：***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；
S5：所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
2.根据权利要求1所述的基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述步骤S1之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s，Rwg，b，v0：k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0，0，G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0：k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0：k)∝p(I0：k|yk)p(yk)，其中，I0：k表示首帧图像到末帧图像的测量信息，i0：k表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0：k)表示最大化的后验分布，∝表示相当于，p(I0：k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度；
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1，i表示相邻两帧的测量信息。
3.根据权利要求1所述的基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述步骤S2包括：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。
4.根据权利要求1所述的基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述步骤S2中所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj，i、j＝1，2，3…，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
5.根据权利要求1所述的基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述步骤S2中对两种方式得到的相机的位姿融合得到相机移动过程包括：根据三角化测量获取相机的位姿形成相机的第一移动轨迹，根据IMU获取相机的位姿形成相机的第二移动轨迹，第一移动轨迹与第二移动轨迹按照时间轴重叠后，剔除同一时刻两者的位姿偏差超过第三预设值的位姿，然后对第一移动轨迹和第二移动轨迹求平均值，得到相机移动过程。
6.根据权利要求1所述的基于点线特征融合的单目视觉贯导SLAM方法，其特征在于，所述步骤S4中重复相机轨迹进行循环检测和循环修正包括：在重复相机轨迹过程中，循环检测主要是在关键帧数据库中查找与当前检测的关键帧是否为相似的关键帧，如果是，则当作闭环候选帧，使用闭环候选帧再次定位相机轨迹，得到新的局部地图，循环修正是在每次循环检测的过程中，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图。
7.基于点线特征融合的单目视觉贯导SLAM装置，其特征在于，所述装置包括：
信息获取模块，用于将单目相机获取的图像和IMU获取的环境信息作为输入信息；
关键帧获取模块，用于基于输入信息进行点线特征提取、点线特征加权及点线特征匹配，点线特征提取的同时进行三角化测量实时获取相机的位姿，IMU安装在单目相机的重心处，IMU也实时获取相机的位姿，对两种方式得到的相机的位姿融合得到相机移动过程，相机移动过程中获得很多帧图像，将所有帧图像中点线特征匹配成功的图像帧作为关键帧；
BA优化模块，用于对所有关键帧中提取特征点低于第一预设值的关键帧剔除，剩余的关键帧形成的相机运动轨迹构成多个局部地图，所有局部地图中点线特征个数低于第二预设值的剔除，对剩下的局部地图进行BA优化；
检测修正模块，用于***初始化，重复相机轨迹进行循环检测和循环修正，得到新的局部地图，循环检测和循环修正结束后，将同一个局部地图的经多次检测和修正后的结果进行平均值操作，得到最终局部地图；
全局优化模块，用于所有的最终局部地图形成全局地图，对全局地图进行全局BA优化，再经过循环检测和循环修正更新全局地图，构建出环境中的完整地图。
8.根据权利要求7所述的基于点线特征融合的单目视觉贯导SLAM装置，其特征在于，所述信息获取模块之前还包括IMU初始化，IMU初始化的过程为：
将IMU采集的参数放在一起构成状态向量yk＝{s，Rwg，b，v0：k}，其中，s表示尺度因子，Rwg表示重力向量，重力向量在世界坐标系下的表示为g＝RwggI，其中gI＝(0，0，G)T，G是重力大小模值；
表示加速度计以及陀螺仪的偏置，在初始化阶段该偏置为常数；v0：k表示从首帧图像到末帧图像的无尺度机体速度；
IMU初始化中被最大化的后验分布为p(yk|I0：k)∝p(I0：k|yk)p(yk)，其中，I0：k表示首帧图像到末帧图像的测量信息，
表示已经测量到的首帧图像到末帧图像的测量信息且
p(yk|I0：k)表示最大化的后验分布，∝表示相当于，p(I0：k|yk)表示似然，p(yk)表示先验分布；
IMU的最大后验估计表示为
其中，Ii-1表示第i-1帧的测量信息，gdir表示重力向量，vi-1表示第i-1帧图像的无尺度机体速度，vi表示第i帧图像的无尺度机体速度；
对IMU的最大后验估计取负对数并且假设IMU预积分以及先验分布服从高斯分布，于是IMU最终的最大后验估计表示为
其中，rp表示先验残差项，Ii-1，i表示相邻两帧的测量信息。
9.根据权利要求7所述的基于点线特征融合的单目视觉贯导SLAM装置，其特征在于，所述关键帧获取模块还用于：通过FAST角点提取算法进行点特征提取，通过线段检测器LSD算法进行线特征提取，在低纹理场景中，对点线特征加权且线特征的权重高于点特征的权重。
10.根据权利要求7所述的基于点线特征融合的单目视觉贯导SLAM装置，其特征在于，所述关键帧获取模块中所述点线特征匹配的过程为：
单目相机获得的图像的相邻帧中的点特征分别记为Ki和Qj，i、j＝1，2，3...，把K1与Qj中距离最近的点特征特征匹配，把K2与Qj中距离最近的点特征特征匹配，依此类推完成点特征匹配；
单目相机获得的图像的相邻帧中的线特征分别记为lc和l′c，通过公式
获取重投影误差，当图像当前帧的当前线特征与其相邻帧中的某个线特征的重投影误差最小时，线特征匹配成功，其中，l1、l2分别表示直线lc的x方向系数以及y方向系数，
表示l′c的两个端点分别到lc的距离。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110873290.0A CN113720323B (zh) | 2021-07-30 | 2021-07-30 | 基于点线特征融合的单目视觉惯导slam方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110873290.0A CN113720323B (zh) | 2021-07-30 | 2021-07-30 | 基于点线特征融合的单目视觉惯导slam方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113720323A true CN113720323A (zh) | 2021-11-30 |
| CN113720323B CN113720323B (zh) | 2024-01-23 |
Family
ID=78674538
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110873290.0A Active CN113720323B (zh) | 2021-07-30 | 2021-07-30 | 基于点线特征融合的单目视觉惯导slam方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113720323B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115752489A (zh) * | 2022-09-20 | 2023-03-07 | 湖南云箭集团有限公司 | 可移动设备的定位方法、装置和电子设备 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108665540A (zh) * | 2018-03-16 | 2018-10-16 | 浙江工业大学 | 基于双目视觉特征和imu信息的机器人定位与地图构建*** |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN112419497A (zh) * | 2020-11-13 | 2021-02-26 | 天津大学 | 基于单目视觉的特征法与直接法相融合的slam方法 |
-
2021
- 2021-07-30 CN CN202110873290.0A patent/CN113720323B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108665540A (zh) * | 2018-03-16 | 2018-10-16 | 浙江工业大学 | 基于双目视觉特征和imu信息的机器人定位与地图构建*** |
| WO2020155616A1 (zh) * | 2019-01-29 | 2020-08-06 | 浙江省北大信息技术高等研究院 | 一种基于数字视网膜的拍摄装置的定位方法 |
| CN110782494A (zh) * | 2019-10-16 | 2020-02-11 | 北京工业大学 | 一种基于点线融合的视觉slam方法 |
| CN112419497A (zh) * | 2020-11-13 | 2021-02-26 | 天津大学 | 基于单目视觉的特征法与直接法相融合的slam方法 |
Non-Patent Citations (1)
| Title |
|---|
| 姚二亮;张合新;张国良;徐慧;赵欣;: "基于Vision-IMU的机器人同时定位与地图创建算法", 仪器仪表学报, no. 04 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115752489A (zh) * | 2022-09-20 | 2023-03-07 | 湖南云箭集团有限公司 | 可移动设备的定位方法、装置和电子设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113720323B (zh) | 2024-01-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
| CN111739063B (zh) | 一种基于多传感器融合的电力巡检机器人定位方法 | |
| CN111462135B (zh) | 基于视觉slam与二维语义分割的语义建图方法 | |
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与*** | |
| CN110335316B (zh) | 基于深度信息的位姿确定方法、装置、介质与电子设备 | |
| CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
| CN110782494A (zh) | 一种基于点线融合的视觉slam方法 | |
| CN110717927A (zh) | 基于深度学习和视惯融合的室内机器人运动估计方法 | |
| CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
| CN114018236B (zh) | 一种基于自适应因子图的激光视觉强耦合slam方法 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
| CN111899345B (zh) | 一种基于2d视觉图像的三维重建方法 | |
| CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
| CN114648584B (zh) | 一种用于多源融合定位的鲁棒性控制方法和*** | |
| CN114255323A (zh) | 机器人、地图构建方法、装置和可读存储介质 | |
| CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
| CN116772844A (zh) | 一种基于动态环境下的视觉惯性室内机器人的导航方法 | |
| CN115147344A (zh) | 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法 | |
| CN109544632B (zh) | 一种基于层次主题模型的语义slam对象关联方法 | |
| CN112731503B (zh) | 一种基于前端紧耦合的位姿估计方法及*** | |
| CN113720323B (zh) | 基于点线特征融合的单目视觉惯导slam方法及装置 | |
| CN112907633A (zh) | 动态特征点识别方法及其应用 | |
| CN116147618B (zh) | 一种适用动态环境的实时状态感知方法及*** | |
| CN117152228A (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 |