CN106991705B - 一种基于p3p算法的位置参数估计方法 - Google Patents
一种基于p3p算法的位置参数估计方法 Download PDFInfo
- Publication number
- CN106991705B CN106991705B CN201710220035.XA CN201710220035A CN106991705B CN 106991705 B CN106991705 B CN 106991705B CN 201710220035 A CN201710220035 A CN 201710220035A CN 106991705 B CN106991705 B CN 106991705B
- Authority
- CN
- China
- Prior art keywords
- characteristic points
- solutions
- camera
- feature points
- points
- 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.)
- Expired - Fee Related
Links
Images
Landscapes
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于P3P算法的位置参数估计方法,包括步骤:对使用的相机进行标定得到相机参数;在相机的当前使用位姿下,对空间中的4个特征点进行成像并获取4个特征点的世界坐标及其图像坐标;将4个特征点分成两组,以对角位置的两个特征点作为公共特征点,将剩余的2个特征点分别与公共特征点组成两组P3P特征点;根据每组P3P特征点中特征点的世界坐标和图像坐标,利用P3P算法求解出公共特征点位置参数的一组实数解,获取两组P3P特征点对应的两组实数解;从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数。通过确定唯一解并对唯一解进行校验,以提高位置参数解算的效率和准确性。
Description
技术领域
本发明涉及一种位置参数估计方法,特别是涉及一种基于P3P算法的位置参数估计方法,属于计算机视觉技术领域。
背景技术
基于视觉的位姿估计技术是计算机视觉领域中的研究热点之一,是视觉定位及导航技术的重要环节。基于视觉的位姿估计技术,通过相机捕获空间合作目标图像,通过图像处理和位姿估计算法求解相机位姿参数,具有精度适中、成本低、使用独立灵活及抗电磁干扰的特点。
PnP算法作为常用的位姿测量方法,以空间中相对位置已知的n(n≥3)个点作为特征点,由摄像机采集一幅图像,计算摄像机相对于特征点的位姿信息。在实际应用中,特征点数越多,在相机与目标之间发生相对运动时,特征点容易超出成像图像范围,以及特征点匹配出现错误,最终导致解算失败。因此使用的特征点数越少,解算越灵活,一般使用P3P算法和P4P算法。
虽然,P3P算法精度与P4P算法精度相差不大;但是,P3P算法根据3个特征点最多可以解算出四组解,P4P算法根据4个特征点可以解得唯一解。然而,在图像处理阶段如果出现某个特征点提取错误,导致位姿解算错误,则该算法无法进行自我校验。因此,需要找到一种所需特征点数少、可以确定唯一解并且能够对唯一解进行校验的位姿解算算法。
发明内容
本发明的主要目的在于,克服现有技术中的不足,提供一种基于P3P算法的位置参数估计方法,不仅计算效率高,而且可提高位置参数解算的准确性,可应用于采用分离式合作目标引导无人机视觉着陆领域。
为了达到上述目的,本发明所采用的技术方案是:
一种基于P3P算法的位置参数估计方法,包括以下步骤:
1)对使用的相机进行标定,得到用于计算相机内参数模型的相机内参数,相机内参数包括相机的焦距和光心坐标;
2)在相机的当前使用位姿下,对空间中的4个特征点进行成像获得成像图像,以4个特征点中的任意一个特征点为原点建立世界坐标系,并获取4个特征点的世界坐标及其在成像图像中的图像坐标;
3)将4个特征点分成两组,以4个特征点中位于对角位置的两个特征点作为公共特征点,将4个特征点中剩余的2个特征点分别与公共特征点组成均具有3个特征点的两个子集,两个子集即为两组P3P特征点;
4)根据每组P3P特征点中特征点的世界坐标和图像坐标,利用P3P算法求解出公共特征点位置参数的一组实数解,获取两组P3P特征点对应的两组实数解;
5)从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数。
本发明进一步设置为:所述步骤1)中的标定采用张正友标定法。
本发明进一步设置为:所述步骤2)中的4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求。
本发明进一步设置为:所述步骤2)中的获取4个特征点的世界坐标及其在成像图像中的图像坐标,具体为,
2-1)在空间中布置分离式的4个合作目标,每个合作目标的中心表示一个特征点,则共有4个特征点;
2-2)以4个特征点中的任意一个特征点为原点建立世界坐标系,通过工具测量特征点的世界坐标;
2-3)使用已标定好焦距的相机,在相机的当前使用位姿下,对合作目标进行成像获得成像图像;
2-4)利用图像处理方法,从相机采集的合作目标的成像图像中提取特征点的图像坐标。
本发明进一步设置为:所述工具包括卷尺和全站仪。
本发明进一步设置为:所述步骤4)中的获取两组P3P特征点对应的两组实数解,具体为,
4-1)选取一组P3P特征点,根据相机小孔成像模型、相机内参数模型和相机外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下,
(C2-C1)T(C2-C1)=(P2-P1)T(P2-P1)
(C3-C1)T(C3-C1)=(P3-P1)T(P3-P1)
(C3-C1)T(C2-C1)=(P3-P1)T(P2-P1)
其中,C1、C2、C3分别为该组P3P特征点中3个特征点的相机坐标,P1、P2、P3分别为该组P3P特征点中3个特征点的世界坐标;
4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中,得到P3P模型的三个约束方程为,
其中,λ1、λ2、d1均为待解未知数,θ1、θ2、θ3分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角;
4-3)利用三角函数换元法,引入中间变量x、y,通过λ1=x+cosθ1和λ2=y+cosθ2来简化约束方程,转化成关于x的一元四次方程,求解出x所有的实数解;
对应的x的多解,求解的相机坐标系下的公共特征点坐标即位置参数存在多解,每一组P3P特征点求解出的公共特征点的位置参数为4个解以下;
4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m,m≤4,其第i个解记为Ai,i∈[1,m];将另一组P3P特征点的公共特征点的位置参数的解的个数记为n,n≤4,其第j个解记为Bj,j∈[1,n]。
本发明进一步设置为:所述步骤5)从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数,具体为,
5-1)计算两组实数解中的位置参数相似度ρ(i,j),计算公式为,
5-2)设置位置参数相似度阈值,根据公共特征点在相机坐标系下的位置参数具有唯一性的特点,选取两组实数解中位置参数相似度最大的两个解,并将该两个解与位置参数相似度阈值进行校验;
若其位置参数相似度大于位置参数相似度阈值,则认定位置参数解算正确,选取两个解的平均值作为公共特征点的位置参数,直接将公共特征点的位置参数用于导航;否则,认定位置参数解算错误,采集下一帧图像重复步骤2)到步骤5)。
与现有技术相比,本发明具有的有益效果是:
本发明提高的一种基于P3P算法的位置参数估计方法,通过设置公共特征点组成两组P3P特征点来确定真值,并通过有效地简化约束方程来提高位置参数估计的实时性和稳定性,克服了P3P算法的多解和计算效率低的问题;同时,在确定唯一解的基础上,通过计算不同解的位置参数相似度对位置参数求解结果加入一个校验的环节,以提高位置参数解算的准确性,使之更具有工程实用性。
上述内容仅是本发明技术方案的概述,为了更清楚的了解本发明的技术手段,下面结合附图对本发明作进一步的描述。
附图说明
图1为本发明实施例的4个特征点的位置示意图;
图2为本发明实施例的P3P算法模型的示意图。
具体实施方式
下面结合说明书附图,对本发明作进一步的说明。
本发明提供一种基于P3P算法的位置参数估计方法,包括以下步骤:
1)对使用的相机进行标定,得到用于计算相机内参数模型的相机内参数,相机内参数包括相机的焦距和光心坐标;其中的标定采用张正友标定法,即张氏标定法。
2)在相机的当前使用位姿下,对空间中的4个特征点进行成像获得成像图像,以4个特征点中的任意一个特征点为原点建立世界坐标系,并获取4个特征点的世界坐标及其在成像图像中的图像坐标;其中,4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求,如图1所示。
具体为,
2-1)在空间中布置分离式的4个合作目标,每个合作目标的中心表示一个特征点,则共有4个特征点;
2-2)以4个特征点中的任意一个特征点为原点O建立世界坐标系,通过卷尺和全站仪等工具测量特征点的世界坐标;
2-3)使用已标定好焦距的相机,在相机的当前使用位姿下,对合作目标进行成像获得成像图像;
2-4)利用图像处理方法,从相机采集的合作目标的成像图像中提取特征点的图像坐标。
3)将4个特征点分成两组,以4个特征点中位于对角位置的两个特征点作为公共特征点,将4个特征点中剩余的2个特征点分别与公共特征点组成均具有3个特征点的两个子集,两个子集即为两组P3P特征点。
4)根据每组P3P特征点中特征点的世界坐标和图像坐标,利用P3P算法求解出公共特征点位置参数的一组实数解,获取两组P3P特征点对应的两组实数解;
具体为,
4-1)选取一组P3P特征点,根据相机小孔成像模型、相机内参数模型和外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下,
(C2-C1)T(C2-C1)=(P2-P1)T(P2-P1)
(C3-C1)T(C3-C1)=(P3-P1)T(P3-P1)
(C3-C1)T(C2-C1)=(P3-P1)T(P2-P1)
其中,C1、C2、C3分别为该组P3P特征点中3个特征点的相机坐标,相机坐标根据步骤2)中获得的特征点图像坐标与步骤1)标定得到的相机参数计算得到,P1、P2、P3分别为该组P3P特征点中3个特征点的世界坐标;
4-2)将步骤1)得到的相机参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中,得到P3P模型的三个约束方程为,
4-3)利用三角函数换元法,引入中间变量x、y,通过λ1=x+cosθ1和λ2=y+cosθ2来简化约束方程,转化成关于x的一元四次方程,求解出x所有的实数解;
对应的x的多解,求解的相机坐标系下的公共特征点坐标即位置参数存在多解,每一组P3P特征点求解出的公共特征点的位置参数为4个解以下;
4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m,m≤4,其第i个解记为Ai,i∈[1,m];将另一组P3P特征点的公共特征点的位置参数的解的个数记为n,n≤4,其第j个解记为Bj,j∈[1,n]。
5)从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数;
具体为,
5-1)计算两组实数解中的位置参数相似度ρ(i,j),计算公式为,
5-2)设置位置参数相似度阈值,根据公共特征点在相机坐标系下的位置参数具有唯一性的特点,选取两组实数解中位置参数相似度最大的两个解,并将该两个解与位置参数相似度阈值进行校验;
若其位置参数相似度大于位置参数相似度阈值,则认定位置参数解算正确,选取两个解的平均值作为公共特征点的位置参数,直接将公共特征点的位置参数用于导航;否则,认定位置参数解算错误,采集下一帧图像重复步骤2)到步骤5)。
本发明的创新点在于,基于P3P算法求解位姿出现多解的情况下,通过确定唯一解并对唯一解进行校验,以提高位置参数解算的效率和准确性。
以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (7)
1.一种基于P3P算法的位置参数估计方法,其特征在于,包括以下步骤:
1)对使用的相机进行标定,得到用于计算相机内参数模型的相机内参数,相机内参数包括相机的焦距和光心坐标;
2)在相机的当前使用位姿下,对空间中的4个特征点进行成像获得成像图像,以4个特征点中的任意一个特征点为原点建立世界坐标系,并获取4个特征点的世界坐标及其在成像图像中的图像坐标;
3)将4个特征点分成两组,以4个特征点中位于对角位置的两个特征点作为公共特征点,将4个特征点中剩余的2个特征点分别与公共特征点组成均具有3个特征点的两个子集,两个子集即为两组P3P特征点;
4)根据每组P3P特征点中特征点的世界坐标和图像坐标,利用P3P算法求解出公共特征点位置参数的一组实数解,获取两组P3P特征点对应的两组实数解;
5)从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数。
2.根据权利要求1所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述步骤1)中的标定采用张正友标定法。
3.根据权利要求1所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述步骤2)中的4个特征点满足任意3个特征点不共线、且4个特征点连线呈菱形的要求。
4.根据权利要求1所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述步骤2)中的获取4个特征点的世界坐标及其在成像图像中的图像坐标,具体为,
2-1)在空间中布置分离式的4个合作目标,每个合作目标的中心表示一个特征点,则共有4个特征点;
2-2)以4个特征点中的任意一个特征点为原点建立世界坐标系,通过工具测量特征点的世界坐标;
2-3)使用已标定好焦距的相机,在相机的当前使用位姿下,对合作目标进行成像获得成像图像;
2-4)利用图像处理方法,从相机采集的合作目标的成像图像中提取特征点的图像坐标。
5.根据权利要求4所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述工具包括卷尺和全站仪。
6.根据权利要求1所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述步骤4)中的获取两组P3P特征点对应的两组实数解,具体为,
4-1)选取一组P3P特征点,根据相机小孔成像模型、相机内参数模型和相机外参数模型构建该组P3P特征点中3个特征点关于世界坐标和相机坐标的关系式如下,
(C2-C1)T(C2-C1)=(P2-P1)T(P2-P1)
(C3-C1)T(C3-C1)=(P3-P1)T(P3-P1)
(C3-C1)T(C2-C1)=(P3-P1)T(P2-P1)
其中,C1、C2、C3分别为该组P3P特征点中3个特征点的相机坐标,P1、P2、P3分别为该组P3P特征点中3个特征点的世界坐标;
4-2)将步骤1)得到的相机内参数和步骤2)获取的特征点的世界坐标及其图像坐标代入到世界坐标和相机坐标的关系式中,得到P3P模型的三个约束方程为,
其中,λ1、λ2、d1均为待解未知数,θ1、θ2、θ3分别为相机光心指向该组P3P特征点中3个特征点所构成的3个向量之间的夹角;
4-3)利用三角函数换元法,引入中间变量x、y,通过λ1=x+cosθ1和λ2=y+cosθ2来简化约束方程,转化成关于x的一元四次方程,求解出x所有的实数解;
对应的x的多解,求解的相机坐标系下的公共特征点坐标即位置参数存在多解,每一组P3P特征点求解出的公共特征点的位置参数为4个解以下;
4-4)将一组P3P特征点的公共特征点的位置参数的解的个数记为m,m≤4,其第i个解记为Ai,i∈[1,m];将另一组P3P特征点的公共特征点的位置参数的解的个数记为n,n≤4,其第j个解记为Bj,j∈[1,n]。
7.根据权利要求6所述的一种基于P3P算法的位置参数估计方法,其特征在于:所述步骤5)从两组实数解中选取位置参数相似度最大的两个解并进行校验,最终确定出公共特征点的位置参数,具体为,
5-1)计算两组实数解中的位置参数相似度ρ(i,j),计算公式为,
5-2)设置位置参数相似度阈值,根据公共特征点在相机坐标系下的位置参数具有唯一性的特点,选取两组实数解中位置参数相似度最大的两个解,并将该两个解与位置参数相似度阈值进行校验;
若其位置参数相似度大于位置参数相似度阈值,则认定位置参数解算正确,选取两个解的平均值作为公共特征点的位置参数,直接将公共特征点的位置参数用于导航;否则,认定位置参数解算错误,采集下一帧图像重复步骤2)到步骤5)。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710220035.XA CN106991705B (zh) | 2017-04-06 | 2017-04-06 | 一种基于p3p算法的位置参数估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710220035.XA CN106991705B (zh) | 2017-04-06 | 2017-04-06 | 一种基于p3p算法的位置参数估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106991705A CN106991705A (zh) | 2017-07-28 |
CN106991705B true CN106991705B (zh) | 2020-03-10 |
Family
ID=59414740
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710220035.XA Expired - Fee Related CN106991705B (zh) | 2017-04-06 | 2017-04-06 | 一种基于p3p算法的位置参数估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106991705B (zh) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110500954A (zh) * | 2019-07-30 | 2019-11-26 | 中国地质大学(武汉) | 一种基于圆特征及p3p算法的飞行器位姿测量方法 |
CN110689575B (zh) * | 2019-09-29 | 2022-04-22 | 阿波罗智联(北京)科技有限公司 | 图像采集器标定方法、装置、设备和介质 |
CN111932611B (zh) * | 2020-05-26 | 2024-05-10 | 阿波罗智联(北京)科技有限公司 | 物***置获取方法和装置 |
CN111667539B (zh) * | 2020-06-08 | 2023-08-29 | 武汉唯理科技有限公司 | 一种相机标定及平面测量方法 |
CN112150546B (zh) * | 2020-10-23 | 2023-11-21 | 西华大学 | 一种基于辅助点几何约束的单目视觉位姿估计方法 |
CN116310126B (zh) * | 2023-03-23 | 2023-11-03 | 南京航空航天大学 | 基于合作目标的飞机进气道三维重建方法及*** |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104656097A (zh) * | 2015-01-28 | 2015-05-27 | 武汉理工大学 | 基于旋转式二维激光三维重构***的标定装置及方法 |
CN104677340A (zh) * | 2013-11-30 | 2015-06-03 | 中国科学院沈阳自动化研究所 | 一种基于点特征的单目视觉位姿测量方法 |
CN105809702A (zh) * | 2016-03-29 | 2016-07-27 | 南京航空航天大学 | 一种基于Tsai算法的改进位姿估计方法 |
-
2017
- 2017-04-06 CN CN201710220035.XA patent/CN106991705B/zh not_active Expired - Fee Related
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104677340A (zh) * | 2013-11-30 | 2015-06-03 | 中国科学院沈阳自动化研究所 | 一种基于点特征的单目视觉位姿测量方法 |
CN104656097A (zh) * | 2015-01-28 | 2015-05-27 | 武汉理工大学 | 基于旋转式二维激光三维重构***的标定装置及方法 |
CN105809702A (zh) * | 2016-03-29 | 2016-07-27 | 南京航空航天大学 | 一种基于Tsai算法的改进位姿估计方法 |
Non-Patent Citations (2)
Title |
---|
Robust analysis of P3P pose estimation;Yingming Hao et al.;《2007 IEEE International Conference on Robotics and Biomimetics (ROBIO)》;20080516;全文 * |
Some Discussion on the Conditions of the Unique Solution of P3P Problem;Wang Ting et al.;《2006 International Conference on Mechatronics and Automation》;20061111;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN106991705A (zh) | 2017-07-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106991705B (zh) | 一种基于p3p算法的位置参数估计方法 | |
CN108734744B (zh) | 一种基于全站仪的远距离大视场双目标定方法 | |
CN107341814B (zh) | 基于稀疏直接法的四旋翼无人机单目视觉测程方法 | |
CN106919944B (zh) | 一种基于orb算法的大视角图像快速识别方法 | |
Li et al. | A 4-point algorithm for relative pose estimation of a calibrated camera with a known relative rotation angle | |
CN105809702A (zh) | 一种基于Tsai算法的改进位姿估计方法 | |
CN107300382B (zh) | 一种用于水下机器人的单目视觉定位方法 | |
CN111210478B (zh) | 一种无共同视野多相机***外参标定方法、介质及*** | |
CN105354841B (zh) | 一种快速遥感影像匹配方法及*** | |
CN108109169B (zh) | 一种基于矩形标识的位姿估计方法、装置及机器人 | |
CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
CN111598172B (zh) | 基于异构深度网络融合的动态目标抓取姿态快速检测方法 | |
CN110595479B (zh) | 一种基于icp算法的slam轨迹评估方法 | |
CN104777469B (zh) | 一种基于量测误差协方差矩阵范数的雷达节点选择方法 | |
CN113393524B (zh) | 一种结合深度学习和轮廓点云重建的目标位姿估计方法 | |
CN106056121A (zh) | 基于sift图像特征匹配的卫星装配工件快速识别方法 | |
CN103632338A (zh) | 一种基于匹配曲线特征的图像配准评估法 | |
CN104077769A (zh) | 一种图像配准中的误匹配点对剔除算法 | |
CN110033492A (zh) | 摄像机标定方法及终端 | |
CN113259837A (zh) | 基于角度估计和指纹定位算法的室内定位方法 | |
CN111583342A (zh) | 一种基于双目视觉的目标快速定位方法及装置 | |
CN103714550A (zh) | 一种基于匹配曲线特征评估的图像配准自动优化算法 | |
WO2020007323A1 (zh) | 商超购物车定位方法、商超购物车定位***及商超购物车 | |
CN101894369A (zh) | 从图像序列中计算摄像机焦距的实时方法 | |
CN106651950B (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 | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20200310 |
|
CF01 | Termination of patent right due to non-payment of annual fee |