CN112393721A - 相机位姿估计方法 - Google Patents
相机位姿估计方法 Download PDFInfo
- Publication number
- CN112393721A CN112393721A CN202011054180.3A CN202011054180A CN112393721A CN 112393721 A CN112393721 A CN 112393721A CN 202011054180 A CN202011054180 A CN 202011054180A CN 112393721 A CN112393721 A CN 112393721A
- Authority
- CN
- China
- Prior art keywords
- fast corner
- fast
- brief
- matching
- brightness
- 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 43
- 239000013598 vector Substances 0.000 claims abstract description 51
- 238000005259 measurement Methods 0.000 claims abstract description 40
- 230000001133 acceleration Effects 0.000 claims abstract description 16
- 230000004927 fusion Effects 0.000 claims abstract description 8
- 238000005070 sampling Methods 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 12
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000000717 retained effect Effects 0.000 claims description 4
- 239000013589 supplement Substances 0.000 claims description 4
- 238000013461 design Methods 0.000 abstract description 8
- 238000010586 diagram Methods 0.000 description 5
- 230000005484 gravity Effects 0.000 description 3
- 238000005286 illumination Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 230000004393 visual impairment Effects 0.000 description 2
- 238000007476 Maximum Likelihood Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000009466 transformation Effects 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/20—Instruments for performing navigational calculations
-
- 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
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开一种相机位姿估计方法,其包括步骤:采集图像帧数据流,以第一分辨率获取包含RGB图像和深度图像的图像数据帧;提取ORB特征,获得具有方向向量的FAST角点和包含若干维二进制向量的BRIEF描述子;搭建加速度与陀螺仪的角速度测量模型,获得惯性测量单元数据的方向向量;保留与惯性测量单元数据方向向量方向一致的FAST角点,对BRIEF描述子进行匹配,匹配成功,从深度相机中获得FAST角点的深度数据;采用李代数表示位姿,获得目标函数,使用李代数扰动模型对目标函数求导,找到局部极小值作为位姿预估值。本发明通过深度相机与惯性测量融合协同进行数据采集和匹配来对机器人本体的姿态进行估计,提升设计精度。
Description
技术领域
本发明涉及机器人导航与定位技术领域,更具体地说,本发明涉及一种 相机位姿估计方法。
背景技术
机器人的导航定位一般由雷达、视觉、惯导等单独或融合技术完成,视 觉也分为单目,双目及深度相机等多种图像获取方式,
惯性相对位姿测量方法将惯性传感器如加速度计和陀螺仪直接固连在被 测物体和运动参考系上,也可以将多种惯性传感器组合成惯性测量单元后再 进行安装。视觉位姿测量方法通常是从拍摄的图像中提取特征点信息,之后 计算得到目标物相对于参考坐标系的位姿信息。
但是视觉测量量容易受遮挡物干扰、环境光线干扰,目标物和参考系同 时运动时靶标更容易移出摄像机视场,导致拍摄画面无明显变化,例如受强 烈光照影响,或面对一片纯色区域时,没有特征点;运动速度过快时,相邻 图像里边搜索的范围可能会很大,在固定较短时间间隔下,两帧图像可能差 别过大,这些都无法满足对机器人本体的快速、准确、实时定位。为了保证 机器人速度里程计的精确性,迫切需要一种高精度的相机位姿估计方法。
发明内容
针对上述技术中存在的不足之处,本发明提供一种相机位姿估计方法, 通过深度相机与惯性测量融合协同进行数据采集和匹配来对机器人本体的姿 态进行估计,解决了机器人速度里程计的设计精度不高的问题,用于提升机 器人速度里程计的设计精度。
为了实现根据本发明的这些目的和其它优点,本发明通过以下技术方案 实现:
本发明所述的相机位姿估计方法,其包括以下步骤:
数据采集:深度相机采集图像帧数据流,以第一分辨率获取包含RGB图 像和深度图像的图像数据帧;
数据处理:从所述图像数据帧中提取ORB特征,获得具有方向向量的 FAST角点和包含若干维二进制向量的BRIEF描述子;搭建加速度与陀螺仪 的角速度测量模型,获得惯性测量单元数据的方向向量;
数据融合:保留与所述惯性测量单元数据方向向量方向一致的FAST角 点,对两幅图像的BRIEF描述子进行匹配,匹配成功,从深度相机中获得所 述FAST角点的深度数据;
位姿预估:根据匹配成功的所述FAST角点的深度数据,采用李代数表 示位姿,获得目标函数,使用李代数扰动模型对所述目标函数求导,找到局 部极小值作为位姿预估值。
优选的是,获得具有方向向量的FAST角点,包括以下步骤:
一次降采样:对第一分辨率的图像数据帧进行一次降采样处理,获得第 二分辨率的图像数据帧;
提取FAST角点:通过像素亮度比较,从第二分辨率的图像数据帧中提 取出FAST角点,加入特征点数组;
获得FAST角点的方向向量:判断获取的FAST角点是否在特征点数组 中:若在,保留该FAST角点的特征值,对该FAST角点进行旋转获得其方 向向量;若不在,删除该FAST角点的特征值;
二次降采样:对保留特征值的FAST角点的图像数据帧进行二次降采样 处理,并循环至提取FAST角点的步骤。
优选的是,提取FAST角点,包括以下步骤:
从第二分辨率的图像数据帧中提取亮度为Ai的目标像素,并设置亮度阈 值为T;
以目标像素为原点取一半径圆周上的若干个像素点并顺时针编号;
检测顺时针编号的若干个像素点的亮度来判断所述目标像素是否为 FAST角点:若满足3个及以上亮度同时大于Ai+T或大于Ai-T,并且连续 12个像素点亮度同时大于Ai+T或小于Ai-T,则将该目标像素确定为FAST 角点,加入特征点数组;
若不满足三个及以上亮度同时大于Ai+T或Ai-T,或者不满足连续12个 像素点亮度同时大于Ai+T或小于Ai-T,或者确定FAST角点并加入特征数 组后,将下一像素设为目标像素。
优选的是,获得包含若干维二进制向量的BRIEF描述子,包括以下步骤:
对一FAST角点附近的一对像素进行大小比较,若大则取1,否则取0, 形成由0和1组成的一维二进制向量的BRIEF描述子;
对一所述FAST角点附近的其他若干对像素进行大小比较,形成包含若 干维二进制向量的BRIEF描述子。
优选的是,对两幅图像的BRIEF描述子进行匹配,采用快速近似最近邻 算法,包括以下步骤:
检测FAST角点位置,保留与惯性测量单元运动方向一致的FAST角点 位置;
计算BRIEF描述子;
对两幅图像的BRIEF描述子进行汉明距离匹配;
找出最大距离和最小距离,判断两幅图像的BRIEF描述子之间的距离与 最大距离和最小距离之间的关系:若两幅图像的BRIEF描述子之间的距离小 于0.3倍最大距离,则匹配成功,若两幅图像的BRIEF描述子之间的距离大 于2倍最小距离且大于0.3倍最大距离,匹配有误。
优选的是,还包括匹配有误导致视觉缺失的处理步骤:
若对两幅图像的BRIEF描述子的匹配有误,切换惯性测量单元运动方程 中的传感器相对惯性系的速度的导数与从传感器到惯性系的四元数的导数 项,以一步长为单位,计算FAST角点的方向向量与惯性测量单元的运动加 速度向量,作为视觉估计缺失时的补充代替。
优选的是,还包括采用最小二乘法对位姿预估值进行优化的步骤:
获取估计初值,初始化半径μ;
求解最小二乘问题;
计算近似模型与实际函数的差异ρ,通过一次判断ρ大小获取μ值:若 ρ>3/4,则μ=18μ;若ρ<1/4,则μ=0.5μ;
获得μ值后,二次判断ρ大小:若ρ>0.95,令xk+1=xk+△xk,判断是否 收敛:若收敛,计算结束;若不收敛,返回至求解最小二乘问题。
本发明至少包括以下有益效果:
1.本发明提供的相机位姿估计方法,通过深度相机与惯性测量融合协同 进行数据采集和匹配来对机器人本体的姿态进行估计,可以减小图像匹配的 搜索范围,缩短匹配时间,解决了机器人速度里程计的设计精度不高的问题, 用于提升机器人速度里程计的设计精度。
2.本发明切换惯性测量单元运动方程中的传感器相对惯性系的速度的导 数与从传感器到惯性系的四元数的导数项,以一步长为单位,计算FAST角 点的方向向量与惯性测量单元的运动加速度向量,作为视觉估计缺失时的补 充代替,可以在很少或没有特征点时持续跟踪,在没有特征点,比如受强烈 光照影响,或面对一片纯色区域时,也能够基本正确地跟踪数十秒的时间;
3.本发明采用最小二乘法对位姿预估值进行优化,提高预估精度。
本发明的其它优点、目标和特征将部分通过下面的说明体现,部分还将 通过对本发明的研究和实践而为本领域的技术人员所理解。
附图说明
图1-1为本发明所述的相机位姿估计方法的流程图;
图1-2为本发明所述的相机成像模型示意图;
图2-1为本发明所述的深度相机采集的RGB图像示例;
图2-2为本发明所述的深度相机采集的深度图像示例;
图3-1为本发明所述的获得具有方向向量的FAST角点的流程图;
图3-2为本发明所述的获得具有方向向量的FAST角点的示例图;
图3-3为本发明所述的提取的FAST角点的示意图;
图4-1为本发明所述的对两幅图像的BRIEF描述子进行匹配的流程图;
图4-2为本发明所述的特征点图;
图4-3为本发明所述的特征点匹配示意图;
图4-4为本发明所述的融合了深度数据的匹配图片;
图5为本发明所述的采用最小二乘法对位姿预估值进行优化的示例。
具体实施方式
下面结合附图对本发明做进一步的详细说明,以令本领域技术人员参照 说明书文字能够据以实施。
应当理解,本文所使用的诸如“具有”、“包含”以及“包括”术语并不排除一 个或多个其它元件或其组合的存在或添加。
<实施例1>
如图1-1所示,本发明提供一种相机位姿估计方法,其运用在机器人导 航与定位技术领域。相机位姿估计方法包括以下步骤,如图1所示:
S10.数据采集:深度相机采集图像帧数据流,以第一分辨率获取包含RGB 图像和深度图像的图像数据帧;
S20.数据处理:从图像数据帧中提取ORB特征,获得具有方向向量的 FAST角点和包含若干维二进制向量的BRIEF描述子;搭建加速度与陀螺仪 的角速度测量模型,获得惯性测量单元数据的方向向量;
S30.数据融合:保留与惯性测量单元数据方向向量方向一致的FAST角 点,对两幅图像的BRIEF描述子进行匹配,匹配成功,从深度相机中获得FAST 角点的深度数据;
S40.位姿预估:根据匹配成功的FAST角点的深度数据,采用李代数表示 位姿,获得目标函数,使用李代数扰动模型对目标函数求导,找到局部极小 值作为位姿预估值。
上述实施方式中,通过提取图像的ORB(Oriented FAST and Rotated BRIEF)特征改进了传统的FAST检测子不具有方向性的问题,且采用速度较 快的二进制描述子BRIEF,使得图像的ORB特征提取速度加快。运动速度过 快的时候,相邻图像里边搜索的范围可能会很大,在固定较短时间间隔下, 两帧图像可能差别过大,设置惯性测量单元,保留与惯性测量单元数据方向 向量方向一致的FAST角点,对两幅图像的BRIEF描述子进行匹配,筛选出 大致位置,进而缩小匹配范围,缩短匹配时间,提高效率。具体地,图像帧 数据流可采用RGB-D相机进行采集,在图像帧数据流中以1280*720的第一 分辨率获取同时包含RGB图像和深度图像的图像数据帧,RGB图像示例如 图2-1所示,深度图像示例如图2-2所示。因为相机摄像头的采集速率一般比 较低,而惯性测量单元可以达到每秒1000帧或者更高,加到图像帧间预测, 就可以以每秒1000帧的速率输出位置,而不是只在有图像的时间输出位置,因此,深度相机采集图像帧数据流与惯性测量单元的运动数据融合,可以在 图像帧间预测位置,提高位姿的估计精度。
本发明实施例提供的相机位姿估计方法,通过深度相机与惯性测量融合 协同进行数据采集和匹配来对机器人本体的姿态进行估计,可以减小图像匹 配的搜索范围,缩短匹配时间,解决了机器人速度里程计的设计精度不高的 问题,用于提升机器人速度里程计的设计精度。
作为上述实施方式的具体说明,步骤S40中位姿预估具体如下:
通用的相机运动方程为:Xk=f(xk-1,uk)+wk;其中,Xk为相机位姿;uk为 惯性测量单元输入运动参数;wk为噪声;
结合通用相机的运动方程,惯性测量单元的运动方程如下:
其中,距离对时间的导数是速度,速度对时间的导数是加速度。Pt表示 传感器相对惯性系的位置,默认是三维的,Vt表示传感器相对惯性系的速度, 同样是在惯性系下表示的,默认是三维的,Qt表示从传感器到惯性系的四元 数,表示从传感器到惯性系的旋转变换;gt代表惯性系下的重力加速度,也 就是初始的惯性测量单元坐标系下表示的重力加速度。
通用的相机观测方程为:Zk,j=h(yj,xk)+vk
其中,Zk,j为在xk位置所观测到的数据,yj为路标点,即在数据处理阶段 匹配的特征点,vk为观测噪声。
根据如图1-2所示的相机成像的空间关系,可将问题表述为:
szk,j=Kexp(ξ*)yj
其中,K是相机内参,S为相机在K处,对j点观测到的像素距离,exp() 为李代数表示的相机位姿,y为j点的世界坐标。
如图1-2中,假设相机在空间中从A点运动到B点,空间中的P点在两 个位置所获取的图像中均被捕获,出现在图像的不同位置。
用李代数表示位姿,目标函数可以写成:
进一步使用李代数扰动模型求导:
在唯一解的情况下,找到局部极小值,即是全局最优值,作为初步估计 结果。
<实施例2>
在实施例1的基础上,本发明实施例主要对获取FAST角点进行说明。 获得具有方向向量的FAST角点,如图3-1所示,包括以下步骤:
S21.一次降采样:对第一分辨率的图像数据帧进行一次降采样处理,获 得第二分辨率的图像数据帧;
S22.提取FAST角点:通过像素亮度比较,从第二分辨率的图像数据帧中 提取出FAST角点,加入特征点数组;
S23.获得FAST角点的方向向量:判断获取的FAST角点是否在特征点数 组中:若在,保留该FAST角点的特征值,对该FAST角点进行旋转获得其 方向向量;若不在,删除该FAST角点的特征值;
S24.二次降采样:对保留特征值的FAST角点的图像数据帧进行二次降采 样处理,并循环至提取FAST角点的步骤。
上述事实方式中,对图像数据帧依次进行步骤S21的一次降采样和步骤 S24的二次降采样的多次采样,是为了不管距离目标远近,都能保持尺度上 的一致,有些图片放大之后原来的角点可能就不再是角点。在第一分辨率优 选为1280*720的基础上,一次降采样获得的第二分辨率优选为480*360,二 次降采样获得分辨率优选为240*180,可有效短时间内快速获取精确的搜索 范围。步骤S22提供了一种通过像素亮度比较来获取FAST角点的方法。步 骤S23中保留FAST角点特征值的同时通过旋转获得特征点的方向向量,作 为优选,可通过灰度质心法对FAST角点进行旋转以获得所有特征点的方向 向量。步骤S23中,FAST角点的特征值提取,使用高成本的GPU作为上位 机计算资源时,可用SIFT算法进行。
<实施例3>
在实施例2的基础上,本发明实施例主要给出步骤S22中提取FAST角 点的一种示例。如图3-2所示,提取FAST角点,包括以下步骤:
从第二分辨率的图像数据帧中提取亮度为Ai的目标像素,并设置亮度阈 值为T;
以目标像素为原点取一半径圆周上的若干个像素点并顺时针编号;
检测顺时针编号的若干个像素点的亮度来判断目标像素是否为FAST角 点:若满足3个及以上亮度同时大于Ai+T或大于Ai-T,并且连续12个像素 点亮度同时大于Ai+T或小于Ai-T,则将该目标像素确定为FAST角点,加 入特征点数组;
若不满足三个及以上亮度同时大于Ai+T或Ai-T,或者不满足连续12个 像素点亮度同时大于Ai+T或小于Ai-T,或者确定FAST角点并加入特征数 组后,将下一像素设为目标像素。
该实施方式中,作为优选,为了有效提取FAST角点,亮度阈值T大小 设置为0.3*Ai,取半径为3的圆周上的16个像素点并顺时针编号,并在16 个像素点中,初始重点筛选圆周上1.5.9.13四个像素的亮度进行比较,若3 个及以上亮度同时大于Ai+T或大于Ai-T,进一步地,再连续12个像素点的 亮度同时大于Ai+T或大于Ai-T,即可确定像素A为FAST角点,加入数组 A[]中,提取的FAST角点如图3-3所示;否则,将下一个像素设为A。通过 本实施方式,可快速、精确提取FAST角点。
<实施例4>
在实施例3的基础上,本发明实施例主要给出步骤S20中BRIEF描述子 的获取方法。获得包含若干维二进制向量的BRIEF描述子,包括以下步骤:
对一FAST角点附近的一对像素进行大小比较,若大则取1,否则取0, 形成由0和1组成的一维二进制向量的BRIEF描述子;
对一FAST角点附近的其他若干对像素进行大小比较,形成包含若干维 二进制向量的BRIEF描述子。
该实施方式中,BRIEF描述子全部是由0和1组成的二进制向量,0,1 表示FAST角点附近两个像素的大小关系,大则取1,否则取0。为了获得多 维向量,本发明实施方式优选对FAST角点附近的512对像素进行大小比较, 获得512维的二进制向量,形成本发明所需的BRIEF描述子。至于FAST角 点附近的像素选取,本发明优选采用泊松分布来挑选位置,因为FAST角点 的提取过程保留了方向与旋转,因此BRIEF描述子在图像旋转时也不会丢失。
<实施例5>
在实施例4的基础上,本发明实施例主要给出步骤S30中对两幅图像的 BRIEF描述子进行匹配的方法。
惯性测量单元作为运动方程的输入,加速度与陀螺仪角速度测量模型如 下:
am=Rt T(at-gt)+abt+an,wm=wt+wbt+wn
其中,am表示加速度模型;wm表示角速度模型;Rt T表示从惯性系到IMU 坐标系的旋转矩阵;abt、wbt代表随机噪声,可理解为加速度计和陀螺仪的零 点漂移;an、wn为零均值高斯白噪声;gt为重力加速度;at表示传感器测量 得到的加速度数据;wt表示传感器测量得到的角速度数据。
本发明实施方式的匹配方法选择快速近似最近邻算法,即根据惯性测量 单元给出的运动数据(包括加速度与陀螺仪角速度)所表示的方向向量,采 用快速近似最近邻算法使得ORB特征点在匹配时与惯性测量单元方向结合。 具体地,如图4-1所示,包括以下步骤:
检测FAST角点位置,保留与惯性测量单元运动方向一致的FAST角点 位置;
计算BRIEF描述子;
对两幅图像的BRIEF描述子进行汉明距离匹配;
找出最大距离和最小距离,判断两幅图像的BRIEF描述子之间的距离与 最大距离和最小距离之间的关系:若两幅图像的BRIEF描述子之间的距离小 于0.3倍最大距离,则匹配成功,若两幅图像的BRIEF描述子之间的距离大 于2倍最小距离且大于0.3倍最大距离,匹配有误。
以图4-2所示的本发明获取的特征点图为例,通过上述实施方式提供的 快速近似最近邻算法对两幅图像的BRIEF描述子进行匹配,匹配成功后如图 4-3所示。匹配成功后,再从RGB-D相机读取该点的深度数据,融合了深度 数据的匹配图像如图4-4所示。
<实施例6>
在实施例5的基础上,若对两幅图像的BRIEF描述子的匹配有误,例如 匹配不到深度数据,甚至极端情况下无关键点匹配时,本实施方式提供一种 匹配有误导致视觉缺失的处理方法。
即切换惯性测量单元运动方程中的传感器相对惯性系的速度Qt的导数 与从传感器到惯性系的四元数Vt导数项,以一步长为单位,计算FAST角点 的方向向量与惯性测量单元的运动加速度向量,作为视觉估计缺失时的补充 代替。具体地,可以以t=300ms为步长单位进行计算。
上述实施方式提供的匹配有误导致视觉缺失的处理方法,可以在很少或 没有特征点时持续跟踪。在没有特征点,比如受强烈光照影响,或面对一片 纯色区域时,也能够基本正确地跟踪数十秒的时间。
<实施例7>
在实施例6的基础上,本发明实施例主要给出采用最小二乘法对位姿预 估值进行优化的示例。
根据通用相机的运动方程和观测方程可知,对姿态估计问题转化为在什 么样的情况下最有可能产生现在的观测数据,条件概率表示如下:
设噪声满足高斯分布,那么:P(zk,j|xj,yj)=N(h(yj,xk),Qk,j)
对其求负对数就相当于求最小化噪声项的平方,也就是最小二乘问题。
最优解即是状态的最大似然估计。
接下来需要确定一个可信区域,并在区域内寻找最优值,因此可选用列 文伯格-马夸尔特方法。
第k次迭代的优化半径为μ,最小二乘解法表示为:
具体的优化步骤如图5所示:
获取估计初值X(),初始化半径μ;
求解最小二乘问题;
计算ρ,通过一次判断ρ大小获取μ值:若ρ>3/4,则μ=18μ;若ρ<1/4, 则μ=0.5μ;
获得μ值后,二次判断ρ大小:若ρ>0.95,令xk+1=xk+△xk,判断是否 收敛:若收敛,计算结束;若不收敛,返回至求解最小二乘问题。
通过本实施方式中最小二乘法对位姿预估值的优化,可以进一步提升机 器人速度里程计的设计精度。
尽管本发明的实施方案已公开如上,但其并不仅仅限于说明书和实施方 式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领 域的人员而言可容易地实现另外的修改。因此在不背离权利要求及等同范围 所限定的一般概念下,本发明并不限于特定的细节和这里示出与描述的图例。
Claims (7)
1.一种相机位姿估计方法,其特征在于,包括以下步骤:
数据采集:深度相机采集图像帧数据流,以第一分辨率获取包含RGB图像和深度图像的图像数据帧;
数据处理:从所述图像数据帧中提取ORB特征,获得具有方向向量的FAST角点和包含若干维二进制向量的BRIEF描述子;搭建加速度与陀螺仪的角速度测量模型,获得惯性测量单元数据的方向向量;
数据融合:保留与所述惯性测量单元数据方向向量方向一致的FAST角点,对两幅图像的BRIEF描述子进行匹配,匹配成功,从深度相机中获得所述FAST角点的深度数据;
位姿预估:根据匹配成功的所述FAST角点的深度数据,采用李代数表示位姿,获得目标函数,使用李代数扰动模型对所述目标函数求导,找到局部极小值作为位姿预估值。
2.如权利要求1所述的相机位姿估计方法,其特征在于,获得具有方向向量的FAST角点,包括以下步骤:
一次降采样:对第一分辨率的图像数据帧进行一次降采样处理,获得第二分辨率的图像数据帧;
提取FAST角点:通过像素亮度比较,从第二分辨率的图像数据帧中提取出FAST角点,加入特征点数组;
获得FAST角点的方向向量:判断获取的FAST角点是否在特征点数组中:若在,保留该FAST角点的特征值,对该FAST角点进行旋转获得其方向向量;若不在,删除该FAST角点的特征值;
二次降采样:对保留特征值的FAST角点的图像数据帧进行二次降采样处理,并循环至提取FAST角点的步骤。
3.如权利要求2所述的相机位姿估计方法,其特征在于,提取FAST角点,包括以下步骤:
从第二分辨率的图像数据帧中提取亮度为Ai的目标像素,并设置亮度阈值为T;
以目标像素为原点取一半径圆周上的若干个像素点并顺时针编号;
检测顺时针编号的若干个像素点的亮度来判断所述目标像素是否为FAST角点:若满足3个及以上亮度同时大于Ai+T或大于Ai-T,并且连续12个像素点亮度同时大于Ai+T或小于Ai-T,则将该目标像素确定为FAST角点,加入特征点数组;
若不满足三个及以上亮度同时大于Ai+T或Ai-T,或者不满足连续12个像素点亮度同时大于Ai+T或小于Ai-T,或者确定FAST角点并加入特征数组后,将下一像素设为目标像素。
4.如权利要求1所述的相机位姿估计方法,其特征在于,获得包含若干维二进制向量的BRIEF描述子,包括以下步骤:
对一FAST角点附近的一对像素进行大小比较,若大则取1,否则取0,形成由0和1组成的一维二进制向量的BRIEF描述子;
对一所述FAST角点附近的其他若干对像素进行大小比较,形成包含若干维二进制向量的BRIEF描述子。
5.如权利要求1所述的相机位姿估计方法,其特征在于,对两幅图像的BRIEF描述子进行匹配,采用快速近似最近邻算法,包括以下步骤:
检测FAST角点位置,保留与惯性测量单元运动方向一致的FAST角点位置;
计算BRIEF描述子;
对两幅图像的BRIEF描述子进行汉明距离匹配;
找出最大距离和最小距离,判断两幅图像的BRIEF描述子之间的距离与最大距离和最小距离之间的关系:若两幅图像的BRIEF描述子之间的距离小于0.3倍最大距离,则匹配成功,若两幅图像的BRIEF描述子之间的距离大于2倍最小距离且大于0.3倍最大距离,匹配有误。
6.如权利要求1所述的相机位姿估计方法,其特征在于,还包括匹配有误导致视觉缺失的处理步骤:
若对两幅图像的BRIEF描述子的匹配有误,切换惯性测量单元运动方程中的传感器相对惯性系的速度的导数与从传感器到惯性系的四元数的导数项,以一步长为单位,计算FAST角点的方向向量与惯性测量单元的运动加速度向量,作为视觉估计缺失时的补充代替。
7.如权利要求1所述的相机位姿估计方法,其特征在于,还包括采用最小二乘法对位姿预估值进行优化的步骤:
获取估计初值,初始化半径μ;
求解最小二乘问题;
计算近似模型与实际函数的差异ρ,通过一次判断ρ大小获取μ值:若ρ>3/4,则μ=18μ;若ρ<1/4,则μ=0.5μ;
获得μ值后,二次判断ρ大小:若ρ>0.95,令xk+1=xk+△xk,判断是否收敛:若收敛,计算结束;若不收敛,返回至求解最小二乘问题。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011054180.3A CN112393721B (zh) | 2020-09-30 | 2020-09-30 | 相机位姿估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011054180.3A CN112393721B (zh) | 2020-09-30 | 2020-09-30 | 相机位姿估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112393721A true CN112393721A (zh) | 2021-02-23 |
CN112393721B CN112393721B (zh) | 2024-04-09 |
Family
ID=74596257
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011054180.3A Active CN112393721B (zh) | 2020-09-30 | 2020-09-30 | 相机位姿估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112393721B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114459507A (zh) * | 2022-03-03 | 2022-05-10 | 湖南大学无锡智能控制研究院 | 一种dvl安装误差标定方法、设备及*** |
CN117109638A (zh) * | 2023-10-24 | 2023-11-24 | 山东大学 | 一种在线单舵轮agv参数标定方法、***、设备及介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105953796A (zh) * | 2016-05-23 | 2016-09-21 | 北京暴风魔镜科技有限公司 | 智能手机单目和imu融合的稳定运动跟踪方法和装置 |
CN109493384A (zh) * | 2018-09-20 | 2019-03-19 | 顺丰科技有限公司 | 相机位姿估计方法、***、设备及存储介质 |
CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
-
2020
- 2020-09-30 CN CN202011054180.3A patent/CN112393721B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105953796A (zh) * | 2016-05-23 | 2016-09-21 | 北京暴风魔镜科技有限公司 | 智能手机单目和imu融合的稳定运动跟踪方法和装置 |
CN109493384A (zh) * | 2018-09-20 | 2019-03-19 | 顺丰科技有限公司 | 相机位姿估计方法、***、设备及存储介质 |
CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
Non-Patent Citations (2)
Title |
---|
徐子锋;石超;王永锋;陈龙;: "基于ORB+PROSAC误匹配剔除算法的视觉SLAM研究", 软件工程, no. 05, 5 May 2019 (2019-05-05) * |
洪亮;冯常;: "基于RGB-D相机数据的SLAM算法", 电子设计工程, no. 09, 5 May 2018 (2018-05-05) * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114459507A (zh) * | 2022-03-03 | 2022-05-10 | 湖南大学无锡智能控制研究院 | 一种dvl安装误差标定方法、设备及*** |
CN114459507B (zh) * | 2022-03-03 | 2024-02-09 | 湖南大学无锡智能控制研究院 | 一种dvl安装误差标定方法、设备及*** |
CN117109638A (zh) * | 2023-10-24 | 2023-11-24 | 山东大学 | 一种在线单舵轮agv参数标定方法、***、设备及介质 |
CN117109638B (zh) * | 2023-10-24 | 2024-03-15 | 山东大学 | 一种在线单舵轮agv参数标定方法、***、设备及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112393721B (zh) | 2024-04-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
CN110221603B (zh) | 一种基于激光雷达多帧点云融合的远距离障碍物检测方法 | |
CN112985416B (zh) | 激光与视觉信息融合的鲁棒定位和建图方法及*** | |
US11830216B2 (en) | Information processing apparatus, information processing method, and storage medium | |
US7467061B2 (en) | Information processing method and apparatus for finding position and orientation of targeted object | |
Rambach et al. | Learning to fuse: A deep learning approach to visual-inertial camera pose estimation | |
CN110570453B (zh) | 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法 | |
US9679384B2 (en) | Method of detecting and describing features from an intensity image | |
CN111024066A (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
US11082633B2 (en) | Method of estimating the speed of displacement of a camera | |
JP6782903B2 (ja) | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム | |
JP7147753B2 (ja) | 情報処理装置、情報処理方法、及びプログラム | |
US10607350B2 (en) | Method of detecting and describing features from an intensity image | |
JP6229041B2 (ja) | 基準方向に対する移動要素の角度偏差を推定する方法 | |
CN114623817A (zh) | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 | |
CN112393721B (zh) | 相机位姿估计方法 | |
JP6410231B2 (ja) | 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム | |
CN110490903B (zh) | 一种双目视觉测量中多目标快速捕获与跟踪方法 | |
CN114723811A (zh) | 一种非结构化环境四足机器人立体视觉定位建图方法 | |
Li et al. | RD-VIO: Robust visual-inertial odometry for mobile augmented reality in dynamic environments | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及*** | |
CN116147618B (zh) | 一种适用动态环境的实时状态感知方法及*** | |
US10977810B2 (en) | Camera motion estimation | |
JP2010009236A (ja) | 平面領域推定装置及びプログラム | |
KR102407802B1 (ko) | 인공신경망 학습 기반의 실내외 3차원 좌표 및 방위 추정 장치 |
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 |