CN109448055B - 单目视觉姿态确定方法及系统 - Google Patents
单目视觉姿态确定方法及系统 Download PDFInfo
- Publication number
- CN109448055B CN109448055B CN201811102499.1A CN201811102499A CN109448055B CN 109448055 B CN109448055 B CN 109448055B CN 201811102499 A CN201811102499 A CN 201811102499A CN 109448055 B CN109448055 B CN 109448055B
- Authority
- CN
- China
- Prior art keywords
- cooperative
- target
- pose information
- feature unit
- initial
- 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
Images
Classifications
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明实施例提供了一种单目视觉姿态确定方法及系统,采用每个特征单元的世界坐标以及像素坐标作为初始信息,基于EPNP算法确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确,能够有效避免SoftPOSIT算法发散或者收敛到错误的姿态,提高了姿态解算的收敛性能和精度,且能够实现实时姿态测量,具有良好的应用前景。
Description
技术领域
本发明实施例涉及计算机视觉测量技术领域,更具体地,涉及单目视觉姿态确定方法及系统。
背景技术
目前,单目视觉姿态测量方法只使用一个摄像机,相较于多目视觉测量方法,测量范围大、结构简单,且避免了立体匹配的困难,具有广泛适用性,因此广泛应用于机器人跟踪导航、飞行器对接、视觉导航等多个领域。但是受限于每个空间点有且仅有一条透视投影直线与之对应,缺乏空间点深度信息,因此需要额外的约束条件实现从二维图像坐标到三维空间坐标的转换,最常用的方法是利用特征点空间位置约束,即PNP问题,该问题的研究受到了国内外学者的广泛关注和研究。
特征点的物点和像点之间对应关系的确定(或称为物点和像点的匹配)是各种姿态求解算法运行的前提条件,无论是对于单目视觉系统还是对于多目视觉系统而言都是。目前,关于PNP问题的研究前提是默认已知特征点的物点和像点之间对应关系,由操作人员根据观测图像手动进行匹配,但是随着特征点个数的增加,以及物体在运动过程出现特征点遮挡等问题时,匹配难度加大,且无法满足对实时性有要求的场合。SoftPOSIT算法可以同时进行被测目标的姿态确定以及物点和像点之间对应关系的确定,即将姿态确定以及物点和像点之间对应关系的确定这两个问题结合起来一起解决,不仅解决了物点和像点的匹配问题,还解决了姿态求解的问题。
SoftPOSIT算法被认为是解决姿态确定问题以及物点和像点之间对应关系的确定问题的最好算法,具有良好的应用前景。但是现有技术中在使用SoftPOSIT算法进行姿态确定时,通常利用给定的被测目标的初始位姿或者利用多次随机初始化得到的被测目标的初始位姿作为迭代初值进行迭代求解,这种情况下,如果被测目标的初始位姿与真实位姿偏差过大,则极有可能导致最终确定的被测目标的姿态发散或者收敛到错误的姿态,限制了该算法的应用。因此,如何得到准确的被测目标的初始姿态并将其作为SoftPOSIT算法的迭代初值,提高SoftPOSIT算法进行姿态确定时的收敛性能和确定的姿态精度,并解决实时性问题是单目视觉姿态测量能够广泛应用于工业测量等各个领域的前提和亟待解决的问题。
发明内容
为克服上述问题或者至少部分地解决上述问题,本发明实施例提供了一种单目视觉姿态确定方法及系统。
第一方面,本发明实施例提供了一种单目视觉姿态确定方法,包括:
获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的每个特征单元均与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上。
第二方面,本发明实施例提供了一种单目视觉姿态确定系统,包括:
坐标获取模块,用于获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
初始位姿信息获取模块,用于根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
实时姿态信息获取模块,用于将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的特征单元与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上。
第三方面,本发明实施例提供了一种电子设备,包括:
至少一个处理器、至少一个存储器、通信接口和总线;其中,
所述处理器、存储器、通信接口通过所述总线完成相互间的通信;
所述存储器存储有可被所述处理器执行的程序指令,所述处理器调用所述程序指令,以执行第一方面提供的单目视觉姿态确定方法。
第四方面,本发明实施例提供了一种非暂态计算机可读存储介质,所述非暂态计算机可读存储介质存储计算机指令,所述计算机指令使所述计算机执行第一方面提供的单目视觉姿态确定方法。
本发明实施例提供的一种单目视觉姿态确定方法及系统,采用每个特征单元的世界坐标以及像素坐标作为初始信息,基于EPNP算法确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确,能够有效避免SoftPOSIT算法发散或者收敛到错误的姿态,提高了姿态解算的收敛性能和精度,且能够实现实时姿态测量,具有良好的应用前景。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种单目视觉姿态确定方法的流程示意图;
图2为本发明实施例提供的一种单目视觉姿态确定方法中采用的合作立体靶标的结构示意图;
图3为本发明实施例提供的一种单目视觉姿态确定方法中采用的位置测量装置的结构示意图;
图4为本发明实施例提供的一种单目视觉姿态确定方法中采用的合作立体靶标上安装孔的结构示意图;
图5为本发明实施例提供的一种单目视觉姿态确定方法中采用的合作立体靶标上设置有12个特征单元时的结构示意图;
图6为本发明实施例提供的一种单目视觉姿态确定系统的结构示意图;
图7为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
在本发明实施例的描述中,需要说明的是,术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明实施例和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明实施例的限制。此外,术语“第一”、“第二”、“第三”仅用于描述目的,而不能理解为指示或暗示相对重要性。
在本发明实施例的描述中,需要说明的是,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以具体情况理解上述术语在本发明实施例中的具体含义。
如图1所示,本发明一实施例提供了一种单目视觉姿态确定方法,包括:
S1,获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
S2,根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
S3,将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的特征单元与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上。
具体地,本发明实施例中提供的单目视觉姿态确定方法,首先需要提供一种合作立体靶标,通过这种合作立体靶标与相机结合构成位置测量装置。其中的合作立体靶标1的结构示意图如图2所示,合作立体靶标1设置在被测目标上,被测目标是一个需要测量姿态的目标物体。位置测量装置的结构示意图如图3所示。图3中,合作立体靶标1上的每个特征单元11均与预设相机2相对设置,且合作立体靶标1在预设相机2的光轴上。合作立体靶标1与预设相机2之间的距离与合作立体靶标1上每两个特征单元之间的距离成正比例,才能保证特征单元在预设相机上的成像(即对应的像素单元)清晰可辨。合作立体靶标1与预设相机2之间的距离、合作立体靶标1上每两个特征单元之间的距离可根据实际需要进行设置,本发明实施例中在此不作具体限定。本发明实施例中提供的位置测量装置结构简单、成本低、实用性强。图3中在合作立体靶标1上具有世界坐标系,也即靶标坐标系Ob,坐标轴分别为X’、Y’和Z’;在预设相机2上具有相机坐标系Oc,坐标轴分别为x、y和z。
需要说明的是,合作立体靶标上可设置多个特征单元,特征单元的具体数量和在合作立体靶标上的位置可根据需要进行设置。但需要满足合作立体靶标上的所有特征单元组合起来形成的图形体积最大,此时测量的精度最高。同时需要保证当出现某个或某些特征单元被遮挡等情况时能够有足够的其他特征单元进行姿态解算。
首先获取每个特征单元的世界坐标以及每个特征单元在预设相机上成像的像素坐标。其中,世界坐标是一种三维坐标,是世界坐标系中的坐标,世界坐标系是一种绝对坐标系,在没有建立用户坐标系之前所有点的坐标都是以该坐标系的原点来确定各自的位置的。像素坐标是一种二维坐标,是合作立体靶标在预设相机上所成的二维图像中特征单元所处的像素位置。合作立体靶标1在预设相机上成像,合作立体靶标1上的每个特征单元11在预设相机上成像上均有一个对应的像素坐标。
得到每个特征单元的世界坐标和在预设相机上成像的像素坐标后,将二者代入至EPNP算法中,通过EPNP算法确定合作立体靶标的初始位姿信息。将得到的合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息。由于采用每个特征单元的世界坐标以及像素坐标作为初始信息确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确。
本发明实施例中提供的单目视觉姿态确定方法,采用每个特征单元的世界坐标以及像素坐标作为初始信息,基于EPNP算法确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确,能够有效避免SoftPOSIT算法发散或者收敛到错误的姿态,提高了姿态解算的收敛性能和精度,且能够实现实时姿态测量,具有良好的应用前景。
在上述实施例的基础上,本发明实施例中提供的单目视觉姿态确定方法中采用的合作立体靶标上设置有预设数量个安装孔,在若干个安装孔中每个安装孔内均固定有一圆柱体,在每个圆柱体的顶端均设置有一特征单元。
具体地,本发明实施例中合作立体靶标上设置的安装孔的作用是用于固定特征单元,为保证立体性,先确定需要的特征单元的数量,然后在合作立体靶标上选择出与特征单元数量相同的安装孔,在这些安装孔中的每个安装孔内固定一个圆柱体,在这些圆柱体的顶端均设置一个特征单元。
作为优选方案,安装孔内设置有螺纹,圆柱体的表面也设置有螺纹,圆柱体可通过螺纹旋进安装孔内。也可以直接采用胶将圆柱体固定在安装孔内。如图4所示,图4中仅示出了预设数量为14的情况,合作立体靶标1为两级台阶的棱台,两个第一级台阶12位于第二级台阶13的两侧,且两个第一级台阶12位于同一平面内,第二级台阶13位于另一平面内,这两个平面相互平行。在每个第一级台阶12上设置有3个安装孔,在第二级台阶13上设置有8个安装孔,第一级台阶12和第二级台阶13上的安装孔均按行排列,且排列的行相互平行。如图5所示,假设需要12个特征单元,则从14个安装孔内选出12个安装孔,在12个安装孔中每个安装孔中均设置有一个圆柱体14,在每个圆柱体14的顶端均设置有一特征单元11。
在上述实施例的基础上,为保证合作立体靶标上的所有特征单元组合起来形成的图形体积最大,使每一圆柱体在安装孔外的高度可调,以适应各种应用场合。
在上述实施例的基础上,在实际采用图3示出的位置测量装置时,预设相机2保持不动,运动的被测目标携带合作立体靶标运动,在被测目标运动过程中,需要保证被测目标上的合作立体靶标在预设相机的光轴附近。
在上述实施例的基础上,本发明实施例中提供的单目视觉姿态确定方法中,所述根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息,具体包括:
将每一特征单元通过四个虚拟控制点表示,并分别获取每个虚拟控制点的世界坐标;
基于每个虚拟控制点的世界坐标以及每一特征单元在预设相机上成像的像素坐标,确定每一特征单元在相机坐标系下的三维坐标;
基于每一特征单元中四个虚拟控制点的世界坐标,以及每一特征单元在相机坐标系下的三维坐标,确定所述合作立体靶标的初始位姿信息。
具体地,本发明实施例中分别定义相机坐标系为Oc和靶标坐标系为Ob,被测目标的位姿即相机坐标系与靶标坐标系之间的相对位置和相对姿态。其中靶标坐标系与世界坐标系重合,即靶标坐标系中各点的坐标与世界坐标是一致的。
将每一特征单元通过四个虚拟控制点表示,并分别获取每个虚拟控制点的世界坐标,设合作立体靶标中每个特征单元的世界坐标表示为Ki B=(Xi B,Yi B,Zi B),i=1,2,...,n,右上标B用来标注靶标坐标系(即世界坐标系)下的坐标,n为合作立体靶标中特征单元的数量,i表示合作立体靶标中的第i个特征单元。对任一个特征单元来说,设4个虚拟控制点在靶标坐标系下的非齐次坐标表示为Vj B=(xj B,yj B,zj B)T,j=1,2,3,4,特征单元的世界坐标与表示该特征单元的四个虚拟控制点的世界坐标之间的关系满足公式(1)。合作立体靶标中每个特征单元在相机坐标系下的坐标表示为Ki c=(Xi c,Yi c,Zi c),i=1,2,...,n,右上标c用来标注相机坐标系下的坐标。对任一个特征单元来说,设4个虚拟控制点在相机坐标系下的非齐次坐标表示为Vj c=(xj c,yj c,zj c)T,j=1,2,3,4,由线性关系在欧式变换下的不变性可知,特征单元在相机坐标系下的坐标与表示该特征单元的四个虚拟控制点在相机坐标系下的坐标之间的关系满足公式(2)。其中[ai1ai2ai3ai4]T中各项分别表示第i个特征单元关于V1 B、V2 B、V3 B、V4 B的权重,即特征单元i在以虚拟控制点j为基的欧式空间中的坐标。
设特征单元i在预设相机上所成的像对应的像素坐标为(ui,vi),采用张正友平面标定方法对相机进行标定,得到相机内参矩阵为A,则有公式(3)存在
其中,si为特征单元i的投影深度,A为预设相机的内参矩阵,fx,fy,ux,vy为预设相机的内部参数,fx为预设相机在x方向上的焦距,fy为预设相机在y方向上的焦距,(ux,vy)为预设相机上的图像坐标系的中心点坐标。公式(3)可变形为:
每个特征单元i以及在预设相机上所成的像点之间的对应关系可由公式(5)的两个方程表示。
当有n个特征单元和像点对应时,可得到含2n个方程的线性方程组,记为矩阵形式Mx=0,其中M是2n×12的矩阵,向量x=[V1 cT V1 cT V1 cT V1 cT]T为12×1的向量,包含了4个虚拟控制点在相机坐标系下的非齐次坐标。根据欧式变换的保距性,n个特征单元之间的距离已知,即可求出n个特征单元在相机坐标系下的三维坐标Ki c,从而将求解三维到二维的PNP问题就转化为求解经典的三维到三维的刚体运动问题,如公式(6)所示,其中R为旋转矩阵,T为平移矩阵,R和T即为所求的初始姿态信息,即通过合作立体靶标的旋转量与平移量表示合作立体靶标的初始姿态信息。初始姿态信息和初始位置信息(即合作立体靶标的世界坐标)共同构成合作立体靶标的初始位姿信息。
在上述实施例的基础上,本发明实施例中提供的单目视觉姿态确定方法中,所述将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息,具体包括:
基于SoftAssign算法和所述迭代初值,确定所述合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系;
基于POSIT算法、所述对应关系以及所述被测目标运动后所述合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标,更新所述对应关系以及所述合作立体靶标的位姿信息;
重复执行上述更新过程,直至更新后的所述对应关系和更新后的所述合作立体靶标的位姿信息使全局目标函数取值最小,则将更新后的所述合作立体靶标的位姿信息中的姿态信息作为所述实时姿态信息。
具体地,将通过EPNP算法确定的合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,即确定了被测目标的实时姿态信息。SoftPOSIT算法由两部分组成,一部分是SoftAssign算法,通过已知的迭代初值,迭代确定合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系;另一部分是POSIT算法,通过已知的合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系,以及已知的特征单元的世界坐标和图像处理新得到的特征单元在预设相机上成像的像素坐标,循环迭代求解被测目标的位姿参数。
设旋转矩阵R=[R1 R2 R3]T,其中R1 T,R2 T,R3 T分别为旋转矩阵R的行向量,平移矩阵T=(Tx Ty Tz),f为预设相机的焦距,w为比例系数,为常数,P0为靶标坐标系的中心,空间点P的齐次坐标为P0P=(Xi,Yi,Zi,1),空间点P在预设相机上的图像坐标为(xi,yi),则有公式(7):
已知对应关系,再利用式(7)即可得到更新后的姿态信息。由更新后的姿态再确定新的对应关系,如此循环反复,直到迭代使得全局目标函数达到最小化即完成姿态解算和对应关系确定。
全局目标函数的表达式如公式(8)所示:
其中,M1=s(R1,Tx),M2=s(R2,Ty),Si=(Xi,Yi,Zi,1),mij是一个矩阵中的元素,表示特征单元和特征单元在预设相机上成像的像素单元的匹配情况,mij表示第i个特征单元与第j个像素单元的匹配情况,当mij取值为1时,表示第i个特征单元与第j个像素单元匹配,即像素单元p和特征单元P匹配。当mij取值为0时,表示第i个特征单元与第j个像素单元不匹配,即像素单元p和特征单元P不匹配。需要说明的是,预设相机上每个像素单元只能和一个特征单元匹配,反之亦然。像素单元即为一个特征单元在预设相机上所成的像。I为合作立体靶标在预设相机上成像中每行中像素单元的个数,J为合作立体靶标在预设相机上成像中每列中像素单元的个数。
由此可见,本发明实施例中提供的方法是将EPNP算法解算得到的合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值代入至SoftPOSIT算法,能够避免SoftPOSIT算法发散或者收敛到错误的姿态,提高了实时姿态解算时的收敛性能和精度,且能够实现实时姿态测量,具有良好的应用前景。
在上述实施例的基础上,本发明实施例中提供的单目视觉姿态确定方法中,采用的特征单元具体为LED灯或反光片。
具体地,为使特征单元在预设相机上所成的图像中更容易被识别,本发明实施例中采用可以主动发光的LED灯作为特征单元,也可以采用被动发光的反光片作为特征单元。
在上述实施例的基础上,本发明实施例中提供的单目视觉姿态确定方法,若特征单元为反光片,则在所述预设相机周围还设置有LED环形光源,所述预设相机设置在所述LED环形光源的中心位置,所述LED环形光源用于为所述反光片提供光照。
具体地,本发明实施例中,当特征单元为反光片时,需要在位置测量装置中加入额外的光源,因此在预设相机周围设置LED环形光源,将预设相机设置在LED环形光源的中心位置,LED环形光源发出的光照射在反光片上,反光片在预设相机所成的图像中呈现亮斑,便于发光片的识别与图像处理。此时,在被测目标运动时,预设相机和周围的LED环形光源保持不动。
如图6所示,在上述实施例的基础上,本发明实施例中还提供了一种单目视觉姿态确定系统,包括:坐标获取模块61、初始位姿信息获取模块62和实时姿态信息获取模块63。其中,
坐标获取模块61用于获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
初始位姿信息获取模块62用于根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
实时姿态信息获取模块63用于将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的特征单元与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上。
具体地,本发明实施例中,首先通过坐标获取模块61获取每个特征单元的世界坐标以及每个特征单元在预设相机上成像的像素坐标。其中,世界坐标是一种三维坐标,是世界坐标系中的坐标,世界坐标系是一种绝对坐标系,在没有建立用户坐标系之前所有点的坐标都是以该坐标系的原点来确定各自的位置的。像素坐标是一种二维坐标,是合作立体靶标在预设相机上所成的二维图像中特征单元所处的像素位置。合作立体靶标1在预设相机上成像,合作立体靶标1上的每个特征单元11在预设相机上成像上均有一个对应的像素坐标。
得到每个特征单元的世界坐标和在预设相机上成像的像素坐标后,通过初始位姿信息获取模块62将二者代入至EPNP算法中,通过EPNP算法确定合作立体靶标的初始位姿信息。最后通过实时姿态信息获取模块63将得到的合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息。由于采用每个特征单元的世界坐标以及像素坐标作为初始信息确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确。
本发明实施例中提供的单目视觉姿态确定系统,采用每个特征单元的世界坐标以及像素坐标作为初始信息,基于EPNP算法确定合作立体靶标的初始位姿信息,可以使得到的初始位姿信息更加接近得到的实时位姿信息,此时再将初始位姿信息作为迭代初值,通过SoftPOSIT算法确定合作立体靶标的实时姿态信息,可以使得到的实时位姿信息更加准确,能够有效避免SoftPOSIT算法发散或者收敛到错误的姿态,提高了姿态解算的收敛性能和精度,且能够实现实时姿态测量,具有良好的应用前景。
在上述实施例的基础上,本发明实施例中还提供了一种单目视觉姿态确定系统中,初始位姿信息获取模块62具体包括:虚拟控制点世界坐标获取子模块、特征单元三维坐标获取子模块和初始位姿信息确定子模块。其中,虚拟控制点世界坐标获取子模块具体用于:将每一特征单元通过四个虚拟控制点表示,并分别获取每个虚拟控制点的世界坐标;特征单元三维坐标获取子模块具体用于:基于每个虚拟控制点的世界坐标以及每一特征单元在预设相机上成像的像素坐标,确定每一特征单元在相机坐标系下的三维坐标;初始位姿信息确定子模块具体用于:基于每一特征单元中四个虚拟控制点的世界坐标,以及每一特征单元在相机坐标系下的三维坐标,确定所述合作立体靶标的初始位姿信息。
在上述实施例的基础上,本发明实施例中还提供了一种单目视觉姿态确定系统中,实时姿态信息获取模块63具体包括:对应关系确定子模块、更新子模块和重复执行子模块。其中,对应关系确定子模块具体用于:基于SoftAssign算法和所述迭代初值,确定所述合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系;更新子模块具体用于:基于POSIT算法、所述对应关系以及所述被测目标运动后所述合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标,更新所述对应关系以及所述合作立体靶标的位姿信息;重复执行子模块具体用于:使更新子模块重复执行更新过程,直至更新后的所述对应关系和更新后的所述合作立体靶标的位姿信息使全局目标函数取值最小,则将更新后的所述合作立体靶标的位姿信息中的姿态信息作为所述实时姿态信息。
如图7所示,在上述实施例的基础上,本发明实施例中还提供了一种电子设备,包括:处理器(processor)701、存储器(memory)702、通信接口(Communications Interface)703和总线704;其中,
所述处理器701、存储器702、通信接口703通过总线704完成相互间的通信。所述存储器702存储有可被所述处理器701执行的程序指令,处理器701用于调用存储器702中的程序指令,以执行上述各方法实施例所提供的方法,例如包括:S1,获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;S2,根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;S3,将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息。
存储器702中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(Random Access Memory,RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
在上述实施例的基础上,本发明实施例中还提供了一种非暂态计算机可读存储介质,所述非暂态计算机可读存储介质存储计算机指令,所述计算机指令使所述计算机执行上述各方法实施例所提供的方法,例如包括:S1,获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;S2,根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;S3,将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息。
以上所描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下,即可以理解并实施。
通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现,当然也可以通过硬件。基于这样的理解,上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品可以存储在计算机可读存储介质中,如ROM/RAM、磁碟、光盘等,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是:以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (9)
1.一种单目视觉姿态确定方法,其特征在于,包括:
获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的每个特征单元均与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上;
所述合作立体靶标为两级台阶的棱台,两个第一级台阶位于第二级台阶的两侧,且两个第一级台阶位于同一平面内,第二级台阶位于另一平面内,两个平面相互平行;
所述将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息,具体包括:
基于SoftAssign算法和所述迭代初值,确定所述合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系;
基于POSIT算法、所述对应关系以及所述被测目标运动后所述合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标,更新所述对应关系以及所述合作立体靶标的位姿信息;
重复执行上述更新过程,直至更新后的所述对应关系和更新后的所述合作立体靶标的位姿信息使全局目标函数取值最小,则将更新后的所述合作立体靶标的位姿信息中的姿态信息作为所述实时姿态信息;
所述合作立体靶标的初始位姿信息包括:初始姿态信息和初始位置信息,所述初始姿态信息包括旋转矩阵和平移矩阵;
所述基于POSIT算法、所述对应关系以及所述被测目标运动后所述合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标,更新所述对应关系以及所述合作立体靶标的位姿信息,具体包括:
基于所述对应关系,利用如下公式确定更新后的所述合作立体靶标的位姿信息,并基于更新后的所述合作立体靶标的位姿信息确定更新后的对应关系:
其中,R1 T,R2 T,R3 T分别为旋转矩阵R的行向量,T=(Tx Ty Tz)为平移矩阵,f为预设相机的焦距,w为常数,P0为靶标坐标系的中心,P0P=(Xi,Yi,Zi,1),(xi,yi)为空间点P在预设相机上的图像坐标。
2.根据权利要求1所述的单目视觉姿态确定方法,其特征在于,所述合作立体靶标上设置有预设数量个安装孔,在若干个安装孔中每个安装孔内均固定有一圆柱体,在每个圆柱体的顶端均设置有一特征单元。
3.根据权利要求2所述的单目视觉姿态确定方法,其特征在于,每个圆柱体在安装孔外的高度可调。
4.根据权利要求1所述的单目视觉姿态确定方法,其特征在于,所述根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息,具体包括:
将每一特征单元通过四个虚拟控制点表示,并分别获取每个虚拟控制点的世界坐标;
基于每个虚拟控制点的世界坐标以及每一特征单元在预设相机上成像的像素坐标,确定每一特征单元在相机坐标系下的三维坐标;
基于每一特征单元中四个虚拟控制点的世界坐标,以及每一特征单元在相机坐标系下的三维坐标,确定所述合作立体靶标的初始位姿信息。
5.根据权利要求1-4中任一项所述的单目视觉姿态确定方法,其特征在于,所述特征单元具体为LED灯或反光片。
6.根据权利要求5所述的单目视觉姿态确定方法,其特征在于,若所述特征单元为反光片,则在所述预设相机周围还设置有LED环形光源,所述预设相机设置在所述LED环形光源的中心位置,所述LED环形光源用于为所述反光片提供光照。
7.一种单目视觉姿态确定系统,其特征在于,包括:
坐标获取模块,用于获取设置在被测目标上的合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标;
初始位姿信息获取模块,用于根据每个特征单元的世界坐标和每个特征单元在预设相机上成像的像素坐标,通过EPNP算法确定所述合作立体靶标的初始位姿信息;
实时姿态信息获取模块,用于将所述合作立体靶标的初始位姿信息作为SoftPOSIT算法的迭代初值,通过SoftPOSIT算法确定所述合作立体靶标的实时姿态信息;
其中,所述合作立体靶标上的特征单元与所述预设相机相对设置,且所述合作立体靶标在所述预设相机的光轴上;
所述合作立体靶标为两级台阶的棱台,两个第一级台阶位于第二级台阶的两侧,且两个第一级台阶位于同一平面内,第二级台阶位于另一平面内,两个平面相互平行;
所述实时姿态信息获取模块具体用于:
基于SoftAssign算法和所述迭代初值,确定所述合作立体靶标上每个特征单元在预设相机上成像中的像素单元的对应关系;
基于POSIT算法、所述对应关系以及所述被测目标运动后所述合作立体靶标上每个特征单元的世界坐标,以及每个特征单元在预设相机上成像的像素坐标,更新所述对应关系以及所述合作立体靶标的位姿信息;
重复执行上述更新过程,直至更新后的所述对应关系和更新后的所述合作立体靶标的位姿信息使全局目标函数取值最小,则将更新后的所述合作立体靶标的位姿信息中的姿态信息作为所述实时姿态信息;
所述合作立体靶标的初始位姿信息包括:初始姿态信息和初始位置信息,所述初始姿态信息包括旋转矩阵和平移矩阵;
所述实时姿态信息获取模块还具体用于:
基于所述对应关系,利用如下公式确定更新后的所述合作立体靶标的位姿信息,并基于更新后的所述合作立体靶标的位姿信息确定更新后的对应关系:
其中,R1 T,R2 T,R3 T分别为旋转矩阵R的行向量,T=(Tx Ty Tz)为平移矩阵,f为预设相机的焦距,w为常数,P0为靶标坐标系的中心,P0P=(Xi,Yi,Zi,1),(xi,yi)为空间点P在预设相机上的图像坐标。
8.一种电子设备,其特征在于,包括:
至少一个处理器、至少一个存储器、通信接口和总线;其中,
所述处理器、存储器、通信接口通过所述总线完成相互间的通信;
所述存储器存储有可被所述处理器执行的程序指令,所述处理器调用所述程序指令,以执行如权利要求1至6中任一项所述的单目视觉姿态确定方法。
9.一种非暂态计算机可读存储介质,其特征在于,所述非暂态计算机可读存储介质存储计算机指令,所述计算机指令使所述计算机执行如权利要求1至6中任一项所述的单目视觉姿态确定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811102499.1A CN109448055B (zh) | 2018-09-20 | 2018-09-20 | 单目视觉姿态确定方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811102499.1A CN109448055B (zh) | 2018-09-20 | 2018-09-20 | 单目视觉姿态确定方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109448055A CN109448055A (zh) | 2019-03-08 |
CN109448055B true CN109448055B (zh) | 2021-04-16 |
Family
ID=65530621
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811102499.1A Active CN109448055B (zh) | 2018-09-20 | 2018-09-20 | 单目视觉姿态确定方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109448055B (zh) |
Families Citing this family (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111982291B (zh) * | 2019-05-23 | 2022-11-04 | 杭州海康机器人技术有限公司 | 一种基于无人机的火点定位方法、装置及系统 |
CN110555902B (zh) * | 2019-09-10 | 2021-03-16 | 中国科学院长春光学精密机械与物理研究所 | 单目视觉测量合作靶标视景仿真系统 |
CN111220126A (zh) * | 2019-11-19 | 2020-06-02 | 中国科学院光电技术研究所 | 一种基于点特征和单目相机的空间物体位姿测量方法 |
CN110716579B (zh) * | 2019-11-20 | 2022-07-29 | 深圳市道通智能航空技术股份有限公司 | 目标跟踪方法及无人飞行器 |
CN111932611B (zh) * | 2020-05-26 | 2024-05-10 | 阿波罗智联(北京)科技有限公司 | 物体位置获取方法和装置 |
CN111692920B (zh) * | 2020-06-12 | 2022-02-22 | 中山大学 | 一种基于反射镜的空间定向能反射对抗方法 |
CN112985411A (zh) * | 2021-03-02 | 2021-06-18 | 南京航空航天大学 | 一种气浮台靶标布局以及姿态解算方法 |
CN113048938B (zh) * | 2021-03-04 | 2023-03-07 | 湖北工业大学 | 一种合作目标设计及姿态角测量系统及方法 |
CN113028990B (zh) * | 2021-03-08 | 2022-11-18 | 湖北工业大学 | 一种基于加权最小二乘的激光跟踪姿态测量系统及方法 |
CN113324538B (zh) * | 2021-05-08 | 2022-10-21 | 中国科学院光电技术研究所 | 一种合作目标远距离高精度六自由度位姿测量方法 |
CN113566778A (zh) * | 2021-07-27 | 2021-10-29 | 同济大学 | 一种多点透视成像的无人飞行器地面飞行位姿测量方法 |
CN114882110B (zh) * | 2022-05-10 | 2024-04-12 | 中国人民解放军63921部队 | 适用于微纳卫星自组装的相对位姿测量及靶标设计方法 |
CN116152357B (zh) * | 2023-04-04 | 2023-07-28 | 国科天成科技股份有限公司 | 一种无限远对焦相机的参数标定系统和方法 |
CN117893610B (zh) * | 2024-03-14 | 2024-05-28 | 四川大学 | 基于变焦单目视觉的航空装配机器人姿态测量系统 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102980528B (zh) * | 2012-11-21 | 2015-07-08 | 上海交通大学 | 无位姿约束线激光单目视觉三维测量传感器参数标定方法 |
CN107742295A (zh) * | 2016-12-14 | 2018-02-27 | 中国科学院沈阳自动化研究所 | 一种基于视觉的立方星对接重构方法 |
-
2018
- 2018-09-20 CN CN201811102499.1A patent/CN109448055B/zh active Active
Non-Patent Citations (3)
Title |
---|
"基于单目视觉的像机位姿估计技术";陈鹏;《中国博士学位论文全文数据库》;20150516(第6期);参见第8页第3段4-6行,第9页第2段,第17页倒数1-2行,第18页第2段1-2行,第34页最后一段,35页,36页第一段,第63页第3段4-6行,第64页倒数1-2段,表4-2,4-4 * |
"复杂场景下合作靶标的准确快速识别与定位";温卓漫;《中国博士学位论文全文数据库》;20170716(第8期);参见第21页第3段第2-3行,第34页图2.1 * |
"空间目标的单目视觉位姿测量方法研究";夏军营;《中国博士学位论文全文数据库》;20141215;I138-68第35页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109448055A (zh) | 2019-03-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109448055B (zh) | 单目视觉姿态确定方法及系统 | |
US11911914B2 (en) | System and method for automatic hand-eye calibration of vision system for robot motion | |
CN110689584B (zh) | 多相机环境中主动式刚体的位姿定位方法及相关设备 | |
CN109344882B (zh) | 基于卷积神经网络的机器人控制目标位姿识别方法 | |
Pintaric et al. | Affordable infrared-optical pose-tracking for virtual and augmented reality | |
JP6594129B2 (ja) | 情報処理装置、情報処理方法、プログラム | |
US8879801B2 (en) | Image-based head position tracking method and system | |
KR20210107015A (ko) | 교정 타겟을 가진 휴대용 도킹 스테이션을 사용한 헤드 장착 디스플레이 교정 | |
CN110780285A (zh) | 激光雷达与组合惯导的位姿标定方法、系统及介质 | |
CN113442169B (zh) | 机器人的手眼标定方法、装置、计算机设备和可读存储介质 | |
WO2022012337A1 (zh) | 运动臂系统以及控制方法 | |
US20060173357A1 (en) | Patient registration with video image assistance | |
US20150029322A1 (en) | Method and computations for calculating an optical axis vector of an imaged eye | |
EP3430596B1 (en) | Determining the relative position between a thermal camera and a 3d camera using a hybrid phantom | |
JP2018189637A (ja) | カメラパラメータ算出方法、カメラパラメータ算出プログラム、カメラパラメータ算出装置、及びカメラパラメータ算出システム | |
CN110096152B (zh) | 身体部位的空间定位方法、装置、设备及存储介质 | |
CN112816949A (zh) | 传感器的标定方法及装置、存储介质、标定系统 | |
Moser et al. | Evaluation of user-centric optical see-through head-mounted display calibration using a leap motion controller | |
KR102152217B1 (ko) | Vr 장비와 ar 장비간의 좌표계 일치용 지그 및 이를 이용한 물리 공간 공유 방법 | |
TW202124917A (zh) | 擴增實境裝置與定位方法 | |
JP2009175012A (ja) | 計測装置および計測方法 | |
KR20180126475A (ko) | 다중 측량 모드의 3차원 측량 시스템 및 측량방법 | |
CN113034565A (zh) | 一种单目结构光的深度计算方法及系统 | |
CN112070844A (zh) | 结构光系统的校准方法及装置、校准工具图、设备及介质 | |
CN113405532B (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 |