CN112393721B - 相机位姿估计方法 - Google Patents

相机位姿估计方法 Download PDF

Info

Publication number
CN112393721B
CN112393721B CN202011054180.3A CN202011054180A CN112393721B CN 112393721 B CN112393721 B CN 112393721B CN 202011054180 A CN202011054180 A CN 202011054180A CN 112393721 B CN112393721 B CN 112393721B
Authority
CN
China
Prior art keywords
fast corner
fast
brightness
matching
data
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
Application number
CN202011054180.3A
Other languages
English (en)
Other versions
CN112393721A (zh
Inventor
史梦安
程巍
任艳
任勇
徐云龙
胥薇
杨艳红
陈阳子
陈年飞
杨敬晶
马壮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN202011054180.3A priority Critical patent/CN112393721B/zh
Publication of CN112393721A publication Critical patent/CN112393721A/zh
Application granted granted Critical
Publication of CN112393721B publication Critical patent/CN112393721B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

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-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，判断是否收敛：若收敛，计算结束；若不收敛，返回至求解最小二乘问题。
CN202011054180.3A 2020-09-30 2020-09-30 相机位姿估计方法 Active CN112393721B (zh)

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 CN112393721A (zh) 2021-02-23
CN112393721B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459507B (zh) * 2022-03-03 2024-02-09 湖南大学无锡智能控制研究院 一种dvl安装误差标定方法、设备及系统
CN117109638B (zh) * 2023-10-24 2024-03-15 山东大学 一种在线单舵轮agv参数标定方法、系统、设备及介质

Citations (4)

* Cited by examiner, † Cited by third party
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信息融合的位姿估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
基于ORB+PROSAC误匹配剔除算法的视觉SLAM研究;徐子锋;石超;王永锋;陈龙;;软件工程;20190505(05);全文 *
基于RGB-D相机数据的SLAM算法;洪亮;冯常;;电子设计工程;20180505(09);全文 *

Also Published As

Publication number Publication date
CN112393721A (zh) 2021-02-23

Similar Documents

Publication Publication Date Title
CN110763251B (zh) 视觉惯性里程计优化的方法及系统
US9940725B2 (en) Method for estimating the speed of movement of a camera
CN110570453B (zh) 一种基于双目视觉的闭环式跟踪特征的视觉里程计方法
US11830216B2 (en) Information processing apparatus, information processing method, and storage medium
CN111862201B (zh) 一种基于深度学习的空间非合作目标相对位姿估计方法
CN112233177B (zh) 一种无人机位姿估计方法及系统
CN107077137A (zh) 用于估计运动的方法、移动代理以及编码有用于使得处理器执行用于估计运动的方法的计算机程序代码的非暂时性计算机可读介质
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
US20190182433A1 (en) Method of estimating the speed of displacement of a camera
CN112393721B (zh) 相机位姿估计方法
CN105324792B (zh) 用于估计移动元件相对于参考方向的角偏差的方法
EP3182370B1 (en) Method and device for generating binary descriptors in video frames
CN111369495B (zh) 一种基于视频的全景图像的变化检测方法
US10607350B2 (en) Method of detecting and describing features from an intensity image
CN114708293A (zh) 基于深度学习点线特征和imu紧耦合的机器人运动估计方法
El Bouazzaoui et al. Enhancing rgb-d slam performances considering sensor specifications for indoor localization
CN110490903B (zh) 一种双目视觉测量中多目标快速捕获与跟踪方法
Zhang et al. A visual-inertial dynamic object tracking SLAM tightly coupled system
JP4935769B2 (ja) 平面領域推定装置及びプログラム
CN111160362B (zh) 一种fast特征均匀化提取与帧间特征误匹配去除方法
CN116147618B (zh) 一种适用动态环境的实时状态感知方法及系统
US10977810B2 (en) Camera motion estimation
CN115170621A (zh) 一种基于相关滤波框架的动态背景下目标跟踪方法及系统
CN114723811A (zh) 一种非结构化环境四足机器人立体视觉定位建图方法
JP2010038753A (ja) 運動推定装置及びプログラム

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