CN111780781B - 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 - Google Patents
基于滑动窗口优化的模板匹配视觉和惯性组合里程计 Download PDFInfo
- Publication number
- CN111780781B CN111780781B CN202010580511.0A CN202010580511A CN111780781B CN 111780781 B CN111780781 B CN 111780781B CN 202010580511 A CN202010580511 A CN 202010580511A CN 111780781 B CN111780781 B CN 111780781B
- Authority
- CN
- China
- Prior art keywords
- imu
- image
- moment
- window
- optimization
- 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
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 37
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims abstract description 29
- 230000009466 transformation Effects 0.000 claims abstract description 29
- 238000005259 measurement Methods 0.000 claims abstract description 12
- 238000000034 method Methods 0.000 claims description 21
- 238000006073 displacement reaction Methods 0.000 claims description 16
- 238000004364 calculation method Methods 0.000 claims description 15
- 238000013519 translation Methods 0.000 claims description 15
- 230000010354 integration Effects 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000001360 synchronised effect Effects 0.000 claims description 5
- 238000012546 transfer Methods 0.000 claims description 5
- 241000209094 Oryza Species 0.000 claims description 4
- 235000007164 Oryza sativa Nutrition 0.000 claims description 4
- 235000009566 rice Nutrition 0.000 claims description 4
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 2
- 238000013016 damping Methods 0.000 claims description 2
- 238000009434 installation Methods 0.000 claims description 2
- 238000005295 random walk Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 230000000007 visual effect Effects 0.000 abstract description 21
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 5
- 238000005286 illumination Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000000844 transformation 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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/22—Matching criteria, e.g. proximity measures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
- G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
- G06V10/751—Comparing pixel values or logical combinations thereof, or feature values having positional relevance, e.g. template matching
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- Evolutionary Computation (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Evolutionary Biology (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Multimedia (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计,在载体上安装VO和IMU,根据IMU测量到的米制轨迹对VO测量到的非米制轨迹进行校正,得到单目VO的尺度因子;对VO采集到的图像进行仿射变换和模板匹配,得到基于VO观测量的视觉残差;对IMU测量值进行预积分,得到基于VO观测量的惯性残差;建立滑动窗口,判断当前帧是否发生了足够的运动,若是,将当前帧加入滑动窗口进行优化,并将窗口内最早的一帧边缘化,否则,将当前帧直接从滑动窗口中剔除;解算滑动窗口的非线性最小二乘方程,得到载体位置、速度、姿态的最优估计值。本发明有效提高了组合里程计的定位实时性,且具有更好的鲁棒性。
Description
技术领域
本发明属于导航技术领域。
背景技术
由于视觉里程计(VO)和惯性测量单元(IMU)具有良好的互补特性,所以越来越多的导航系统采用视觉和惯性融合的方案进行定位,在大量场景中取得了很好的效果。视觉前端是VO/IMU组合定位系统中最关键的模块,如果视觉前端无法很好的跟踪图像,定位效果会下降很多。目前的视觉前端非常依赖于周围环境,很容易受到光照变化、遮挡物、快速运动等因素影响,因此提高视觉前端的鲁棒性是组合导航系统能够准确定位的关键。
目前常用的视觉前端通常采用特征点法或光流法进行位姿估计。特征点法首先要对每一帧图像进行特征检测,然后计算每一个特征点的描述子,根据描述子信息与上一帧的特征点进行匹配。在得到匹配好的特征点对后,通过PnP(Perspective-n-Point)算法计算得到相机的位姿估计。由于这种方法需要在每一帧图像都提取特征点并计算描述子,耗时较长,而且特征点法对于图像中的特征点数量要求较高,很难在特征点较少的场景下工作。光流法首先假设相邻图像帧间每个点的灰度值保持不变,在这个假设的基础上根据灰度值对特征点进行跟踪。相比于特征点法,光流法由于不需要在每一帧都提取特征点,也不用进行特征点的匹配工作,所有具有更好的实时性。但是光流法由于基于灰度值不变的假设,对系统的光照变化非常敏感,很难在光照不稳定的场景下工作。此外,通常VO/IMU组合定位系统的后端需要处理大量的三维空间点,会导致计算量很大,影响组合定位系统的实时性。
发明内容
发明目的:为解决现有技术存在计算量大以及无法在关照不稳定的场景下工作等问题,本发明提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计。
技术方案:本发明提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计,具体包括如下步骤:
步骤一:将俯视单目相机VO和惯性传感器IMU安装在载体上,并将IMU的轨迹和VO的轨迹进行同步;
步骤二:设置基准时间,VO和IMU根据基准时间在采集的数据上加上时间戳信息,从而得到时间同步的图像和惯性测量值;
步骤五:IMU测得相邻两帧图像之间的原始数据,所述原始数据为相邻两帧图像之间VO的加速度和角度速,分别对原始数据进行预积分,从而推导出IMU的误差状态方程,利用IMU的误差状态方程处理预积分中的噪声,从而建立IMU惯性残差模型;
步骤六:构建滑动窗口,将当前时刻的图像放入滑动窗口中,基于模板匹配和图像仿射变换,得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量将该与VO的尺度因子相乘,得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p;根据上一次优化时建立的先验约束、IMU惯性残差和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p,建立基于窗口的非线性优化函数;判断p是否大于预设的阈值,若是,则对最早的一个关键帧进行边缘化,基于非线性优化函数,对窗口进行优化,得到当前时刻的载体的位置、速度和姿态的最优估计值,并将当前时刻的图像作为用于下次优化计算的一个关键帧,根据边缘化的关键帧和窗口中剩余的关键帧,建立用于下一次优化计算的先验约束,并继续将下一个时刻的图像放入窗口中,计算下一个时刻载体的位置、速度和姿态的最优估计值;否则,在窗口中删除当前时刻的图像,并将下一个时刻的图像放入窗口中,继续计算下一个时刻载体的位置、速度和姿态的最优估计值。
进一步的,所述步骤一中将IMU的轨迹和VO的轨迹进行同步,具体为:根据IMU安装方向计算出VO和IMU之间的旋转外参数,并测量得到VO和IMU之间的平移量;将载体在IMU坐标系下的坐标与旋转外参数相乘,再加上VO和IMU传感器之间的平移量,从而将IMU的轨迹转换到VO坐标系下,实现IMU和VO的轨迹同步。
进一步的,所述步骤三具体为:
步骤3.1:在第k时刻的图像的中心选择一个大小为A×A的矩阵区域作为模板,按照如下公式进行图像仿射变换,得到图像仿射变换后的模板B:
其中,W(·)为图像仿射变换,(x,y)为图像中像素点的坐标值,p1,p2,p3,p4,p5,p6均为在该次仿射变换中的仿射变换参数;在该次仿射变换中p1,p2,p3,p4,p5,p6初始值均为0,该六个参数的确定值通过超定方程法计算得到;
步骤3.2:根据标准平方差匹配计算得到模板B和第k+1时刻的图像的相似度,从而在第k+1时刻的图像中找到与模板B相似度最高的一块区域,将该区域作为第k+1时刻的图像的模板;
T为空间转置。
进一步的,所述VO的尺度因子s为:
进一步的,所述步骤六中的非线性优化函数χ*为:
χ*=argmin(eprior+∑eIMU+∑eVO)
其中,eprior为由边缘化计算得到的先验残差;eIMU为根据IMU的误差状态方程计算得到的IMU惯性残差,eVO为根据模板匹配计算得到的上一个关键帧点与当前时刻图像之间的米制像素位移增量。
进一步的,所述步骤六中采用列文伯格-马夸尔特算法求解非线性优化函数,从而对窗口进行迭代优化。
有益效果:本发明相比于传统视觉前端所采用的特征点法与光流法,模板匹配VO不需要提取特征点,摆脱了视觉前端对特征点的依赖,使组合里程计具有更好的鲁棒性。本发明还在模板匹配算法中加入了图像仿射变换,使模板匹配算法对旋转进一步的提高了鲁棒性,可以适用于快速运动或坡道场景等相机图像可能发生旋转变换的环境中。相比于传统的视觉惯性里程计,本发明提出的组合里程计在后端不用对大量路标点的深度值进行优化,有效提高了组合里程计的定位实时性。
附图说明
图1是本发明的系统框图。
图2是IMU和VO同步时间处理示意图。
图3是加入仿射变换的模板匹配示意图。
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的不当限定。
如图1所示,本实施例提供了一种基于滑动窗口优化的模板匹配视觉和惯性组合里程计,视觉部分通过模板匹配算法计算得到视觉残差,IMU部分通过预积分计算得到惯性残差。后端通过L-M算法求出一个最优的位姿估计值,使视觉残差和惯性残差之和最小。具体为:
步骤一:安装传感器并初始化外参。在载体平台上安装俯视单目相机VO和惯性传感器IMU。根据VO和IMU坐标系之间的旋转约束计算得到单目相机和惯性传感器之间的旋转外参数,并通过人工测量直接得到VO和IMU之间的平移量。将载体在IMU坐标系下的坐标与旋转外参数相乘,再加上VO和IMU传感器之间的平移量,从而将IMU的轨迹转换到VO坐标系下,实现IMU和VO的轨迹同步。
步骤二:采集时间同步的传感器数据。由于IMU通常以一个较高的频率采集数据,而VO采集图像的频率较低,所以在采集传感器数据的时候要做好硬件同步和软件同步。组合里程计的同步时间处理示意图如图2所示。
给视觉传感器和惯性传感器一个基准时间,VO和IMU根据给出的基准时间为它们各自采集到的数据加上时间戳信息,从而得到时间同步的VO数据和IMU数据;
如此就可以将视觉轨迹和IMU轨迹对齐,完成了组合里程计的初始化。
步骤四:确定模板尺寸。假设k时刻,相机采集到的图像大小为K×L,那么以图像最中心的像素点为模板中心,选择一块大小为A×A的矩阵区域作为模板,A的选择方法为:
其中,min(·)表示取最小值。
步骤五:加入图像仿射变换。选择模板匹配并进行仿射变换的示意图如图3。对所选模板中的像素点进行图像仿射变换,所用的仿射变换表示为:
其中,W(·)为图像仿射变换;(x,y)为像素点的坐标值;p1,p2,p3,p4,p5,p6为仿射变换的参数,这六个参数值需要实时在线估计,本实施例采用超定方程法去估算,使图像在发生一定的旋转之后还能准确进行模板匹配。通过调整仿射变换的六个参数,可以得到相邻图像间的旋转和平移从而更好的进行模板匹配,使模板匹配对旋转具有更好的鲁棒性,克服了因为运动过快或者坡道场景导致的模板误匹配问题。
采用超定方程法叠代的终止条件为:
其中,F为模板在仿射变换后的误差函数。当误差函数最小时,通过这6个放射变换的参数可以得到相邻图像间的旋转和平移,从而使图像之间可以更好的进行模板匹配,使模板匹配对旋转具有更好的鲁棒性,克服了因为运动过快或者坡道场景导致的模板误匹配问题。
考虑在k时刻和k+1时刻进行模板匹配。根据标准平方差匹配计算得到k时刻模板和k+1时刻的图像之间的相似度;在k+1时刻的图像中找到和k时刻经图像仿射变换模板相似程度最高的一块图像作为k+1时刻的模板。将k时刻模板的中心点像素坐标设为(uk,vk),k+1时刻模板的中心点像素坐标设为(uk+1,vk+1),则k时刻到k+1时刻的像素坐标增量,即通过相机测量得到的非米制位移增量为:
其中T为空间转置。
将得到的相机位移增量与尺度因子相乘,即可得到相邻时刻两个图像之间米制的像素平移距离。
步骤六:计算IMU预积分和误差传递。对读取到的IMU数据进行预积分,得到相邻两图像帧之间相机的平移距离、速度变化和旋转变化量。
由于IMU会产生累积误差,所以要计算IMU的误差传递。IMU读取到的测量数据受到偏置和高斯白噪声的影响,IMU测量到的载体的角速度ω和加速度a可以表示为:
其中,和分别表示角速度和加速度的真实值;bgyro和bacc分别表示陀螺仪和加速度计偏置的随机游走,会随着运行时间缓慢变化,其导数服从高斯分布;ngyro和nacc分别表示陀螺仪和加速度计受到的高斯白噪声。根据IMU的运动方程,可以推导出IMU的误差状态方程为:
其中,χB为IMU误差状态向量;F为系统传递矩阵;G为噪声观测矩阵。
步骤七:构建滑动窗口,将当前时刻的图像放入滑动窗口中,基于模板匹配和图像仿射变换,得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量将该与VO的尺度因子相乘,得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p;根据视觉里程计的状态变量、IMU的误差状态方程和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p,建立基于窗口的非线性优化函数;判断该p是否大于预设的阈值,若是,则对最早的一个关键帧进行边缘化,基于非线性优化函数和上一次优化时建立的先验约束,对窗口进行优化,得到当前时刻的载体的位置、速度和姿态的最优估计值,并将当前时刻的图像作为用于下次优化计算的一个关键帧,根据边缘化的关键帧和窗口中剩余的关键帧,建立用于下一次优化计算的先验约束,并继续将下一个时刻的图像放入窗口中,计算下一个时刻载体的位置、速度和姿态的最优估计值;当前帧与上一个关键帧间的位置距离没有超过预设阈值,说明相机还没有发生足够大的运动,不必对滑动窗口进行更新,窗口中删除当前时刻的图像,并将下一个时刻的图像放入窗口中,继续计算下一个时刻载体的位置、速度和姿态的最优估计值若。
步骤八:求解系统状态变量的最优估计值。视觉和惯性组合里程计的系统状态变量χ为:
χ=[x0,x1,…,xn]
其中,x0,x1,…,xn为每一个时刻载体的位姿估计值,包括位置、速度、姿态、陀螺仪偏置和加速度计偏置。构建的非线性优化函数表示为:
χ*=argmin(eprior+∑eIMU+∑eVO)
其中,χ*为系统状态变量的最优估计值;eprior为由边缘化计算得到的先验残差;eIMU为根据运动约束计算得到的IMU残差(根据IMU的误差状态方程计算得到的IMU残差);eVO为根据模板匹配计算得到的视觉残差(根据模板匹配计算得到的相邻时刻的两个图像之间的米制的像素位移增量)。
通过列文伯格-马夸尔特(L-M)算法求解构造的非线性最小二乘问题,求解的增量方程表示为:
(JTJ+μI)Δx=-JTf
其中,J为残差对状态变量求导的雅克比矩阵;μ为阻尼因子,需要根据系统误差的真实下降值和按照近似模型得到的下降值之间的比例关系实时调整;Δx为用于状态变量更新的增量;f为系统误差。根据该增量方程迭代对滑动窗口内的状态变量进行更新,直到增量Δx足够小,则输出载体位置、速度和姿态的最优估计值。
本发明基于滑动窗口优化的模板匹配视觉和惯性组合里程计的技术方案为:在载体上安装单目摄像头和IMU并计算测量两个传感器之间的外参。其中,单目相机垂直于地面安装,镜头俯视地面。采集时间同步的传感器测量数据,并根据IMU测量到的米制轨迹对VO测量到的非米制轨迹进行校正,得到单目VO的尺度因子,完成系统初始化。对单目相机采集到的图像进行仿射变换和模板匹配,得到基于VO观测量的视觉残差。对IMU测量值进行预积分,并推导其噪声模型,得到基于VO观测量的惯性残差。建立某个固定大小的滑动窗口,判断当前帧是否发生了足够的运动。若当前帧发生了足够的运动,则将当前帧加入滑动窗口进行非线性优化,同时将窗口内时间最早的一帧边缘化,否则,将当前帧直接从滑动窗口中剔除。通过L-M算法对滑动窗口内的所有状态变量进行迭代优化,得到载体位置、速度、姿态的最优估计值并实时输出。
上面结合附图对本发明的实施方式作了详细说明,但是本发明并不限于上述实施方式,在本领域普通技术人员所具备的知识范围内,还可以在不脱离本发明宗旨的前提下做出各种变化。
Claims (5)
1.基于滑动窗口优化的模板匹配视觉和惯性组合里程计,其特征在于,具体包括如下步骤:
步骤一:将俯视单目相机VO和惯性传感器IMU安装在载体上,并将IMU的轨迹和VO的轨迹进行同步;
步骤二:设置基准时间,VO和IMU根据基准时间在采集的数据上加上时间戳信息,从而得到时间同步的图像和惯性测量值;
步骤五:IMU测得相邻两帧图像之间的原始数据,所述原始数据为相邻两帧图像之间VO的加速度和角度速,分别对原始数据进行预积分,从而推导出IMU的误差状态方程,利用IMU的误差状态方程处理预积分中的噪声,从而建立IMU惯性残差模型;
步骤六:构建滑动窗口,将当前时刻的图像放入滑动窗口中,基于模板匹配和图像仿射变换,得到当前时刻图像与窗口中上一个关键帧之间的非米制像素位移增量将与VO的尺度因子相乘,得到当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p;根据上一次优化时建立的先验约束、根据步骤5中的IMU惯性残差模型计算当前时刻图像与上一个关键帧之间的IMU惯性残差和当前时刻图像与窗口中上一个关键帧之间的米制像素位移增量p,建立基于窗口的非线性优化函数;判断p是否大于预设的阈值,若是,则对最早的一个关键帧进行边缘化,基于非线性优化函数,对窗口进行优化,得到当前时刻的载体的位置、速度和姿态的最优估计值,并将当前时刻的图像作为用于下次优化计算的一个关键帧,根据边缘化的关键帧和窗口中剩余的关键帧,建立用于下一次优化计算的先验约束,并继续将下一个时刻的图像放入窗口中,计算下一个时刻载体的位置、速度和姿态的最优估计值;否则,在窗口中删除当前时刻的图像,并将下一个时刻的图像放入窗口中,继续计算下一个时刻载体的位置、速度和姿态的最优估计值;
所述步骤五具体为:计算IMU预积分和误差传递,对读取到的IMU数据进行预积分,得到相邻两图像帧之间相机的平移距离、速度变化和旋转变化量;
由于IMU会产生累积误差,所以要计算IMU的误差传递,IMU读取到的测量数据受到偏置和高斯白噪声的影响,IMU测量到的载体的角速度ω和加速度a表示为:
其中,和分别表示角速度和加速度的真实值;bgyro和bacc分别表示陀螺仪和加速度计偏置的随机游走,会随着运行时间缓慢变化,其导数服从高斯分布;ngyro和nacc分别表示陀螺仪和加速度计受到的高斯白噪声,根据IMU的运动方程,可以推导出IMU的误差状态方程为:
其中,χB为IMU误差状态向量;F为系统传递矩阵;G为噪声观测矩阵;
所述步骤六中求解得到的最优估计值的表达式为:
χ=[x0,x1,…,xn]
其中,x0,x1,…,xn为每一个时刻载体的位姿估计值,包括位置、速度、姿态、陀螺仪偏置和加速度计偏置;
所述步骤六中的非线性优化函数χ*为:
χ*=arg min(eprior+∑eIMU+∑eVO)
其中,eprior为由边缘化计算得到的先验残差;eIMU为IMU惯性残差,eVO为根据模板匹配计算得到的上一个关键帧点与当前时刻图像之间的米制像素位移增量;
所述步骤六中采用列文伯格-马夸尔特算法求解非线性优化函数,从而对窗口进行迭代优化,求解的增量方程表示为:
(JTJ+μI)Δx=-JTf
其中,J为残差对状态变量求导的雅克比矩阵;μ为阻尼因子;Δx为用于状态变量更新的增量;f为系统误差,根据该增量方程迭代对滑动窗口内的状态变量进行更新,直到增量Δx足够小,则输出载体位置、速度和姿态的最优估计值。
2.根据权利要求1所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计,其特征在于,所述步骤一中将IMU的轨迹和VO的轨迹进行同步,具体为:根据IMU安装方向计算出VO和IMU之间的旋转外参数,并测量得到VO和IMU之间的平移量;将载体在IMU坐标系下的坐标与旋转外参数相乘,再加上VO和IMU传感器之间的平移量,从而将IMU的轨迹转换到VO坐标系下,实现IMU和VO的轨迹同步。
3.根据权利要求1所述的基于滑动窗口优化的模板匹配视觉和惯性组合里程计,其特征在于,所述步骤三具体为:
步骤3.1:在第k时刻的图像的中心选择一个大小为A×A的矩阵区域作为模板,按照如下公式进行图像仿射变换,得到图像仿射变换后的模板B:
其中,W(·)为图像仿射变换,(x,y)为图像中像素点的坐标值,p1,p2,p3,p4,p5,p6均为在该次仿射变换中的仿射变换参数;在该次仿射变换中p1,p2,p3,p4,p5,p6初始值均为0,该六个参数的确定值通过超定方程法计算得到;
步骤3.2:根据标准平方差匹配计算得到模板B和第k+1时刻的图像的相似度,从而在第k+1时刻的图像中找到与模板B相似度最高的一块区域,将该区域作为第k+1时刻的图像的模板;
T为空间转置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010580511.0A CN111780781B (zh) | 2020-06-23 | 2020-06-23 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010580511.0A CN111780781B (zh) | 2020-06-23 | 2020-06-23 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111780781A CN111780781A (zh) | 2020-10-16 |
CN111780781B true CN111780781B (zh) | 2022-05-06 |
Family
ID=72757687
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010580511.0A Active CN111780781B (zh) | 2020-06-23 | 2020-06-23 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111780781B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112464432B (zh) * | 2020-10-27 | 2024-05-14 | 江苏大学 | 一种基于双速数值积分结构的惯性预积分的优化方法 |
CN113793381A (zh) * | 2021-07-27 | 2021-12-14 | 武汉中海庭数据技术有限公司 | 单目视觉信息和轮速信息融合的定位方法及系统 |
CN114915913A (zh) * | 2022-05-17 | 2022-08-16 | 东南大学 | 一种基于滑窗因子图的uwb-imu组合室内定位方法 |
CN114851206B (zh) * | 2022-06-06 | 2024-03-29 | 天津中科智能识别有限公司 | 基于视觉引导机械臂抓取炉具的方法 |
CN116026316B (zh) * | 2023-03-30 | 2023-08-29 | 山东科技大学 | 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法 |
CN117218161B (zh) * | 2023-11-09 | 2024-01-16 | 聊城市敏锐信息科技有限公司 | 一种鱼缸内鱼类轨迹跟踪方法及系统 |
CN117765084B (zh) * | 2024-02-21 | 2024-05-03 | 电子科技大学 | 基于动态分支预测的迭代求解的面向视觉定位方法 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106446815B (zh) * | 2016-09-14 | 2019-08-09 | 浙江大学 | 一种同时定位与地图构建方法 |
US10107627B2 (en) * | 2017-03-08 | 2018-10-23 | Northrop Grumman Systems Corporation | Adaptive navigation for airborne, ground and dismount applications (ANAGDA) |
CN107063246A (zh) * | 2017-04-24 | 2017-08-18 | 齐鲁工业大学 | 一种视觉导航/惯性导航的松散组合导航方法 |
CN110210346A (zh) * | 2019-05-21 | 2019-09-06 | 西安电子科技大学 | 一种视频模板匹配实时处理的优化方法 |
CN110751123B (zh) * | 2019-06-25 | 2022-12-23 | 北京机械设备研究所 | 一种单目视觉惯性里程计系统及方法 |
CN110411475A (zh) * | 2019-07-24 | 2019-11-05 | 南京航空航天大学 | 一种基于模板匹配算法和imu辅助的机器人视觉里程计 |
-
2020
- 2020-06-23 CN CN202010580511.0A patent/CN111780781B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN111780781A (zh) | 2020-10-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111780781B (zh) | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 | |
CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
CN111780754B (zh) | 基于稀疏直接法的视觉惯性里程计位姿估计方法 | |
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
CN107869989B (zh) | 一种基于视觉惯导信息融合的定位方法及系统 | |
JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
CN112598757B (zh) | 一种多传感器时间空间标定方法及装置 | |
CN104732518B (zh) | 一种基于智能机器人地面特征的ptam改进方法 | |
CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
CN112749665B (zh) | 一种基于图像边缘特征的视觉惯性slam方法 | |
CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN111156984A (zh) | 一种面向动态场景的单目视觉惯性slam方法 | |
CN112649016A (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
CN114216454B (zh) | 一种gps拒止环境下基于异源图像匹配的无人机自主导航定位方法 | |
CN109631894A (zh) | 一种基于滑动窗口的单目视觉惯性紧耦合方法 | |
CN115371665B (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
CN111307146A (zh) | 一种基于双目相机和imu的虚拟现实头戴显示设备定位系统 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN114397642A (zh) | 一种基于图优化的三维激光雷达与imu外参标定方法 | |
CN114608554A (zh) | 一种手持slam设备以及机器人即时定位与建图方法 | |
CN115046545A (zh) | 一种深度网络与滤波结合的定位方法 | |
CN114964276A (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 |