CN113436249B - 一种快速稳健的单目相机位姿估计算法 - Google Patents

一种快速稳健的单目相机位姿估计算法 Download PDF

Info

Publication number
CN113436249B
CN113436249B CN202110607849.5A CN202110607849A CN113436249B CN 113436249 B CN113436249 B CN 113436249B CN 202110607849 A CN202110607849 A CN 202110607849A CN 113436249 B CN113436249 B CN 113436249B
Authority
CN
China
Prior art keywords
matrix
rotation matrix
algorithm
iteration
weight
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
CN202110607849.5A
Other languages
English (en)
Other versions
CN113436249A (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.)
Chinese People's Liberation Army No63628 Troop
Original Assignee
Chinese People's Liberation Army No63628 Troop
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 Chinese People's Liberation Army No63628 Troop filed Critical Chinese People's Liberation Army No63628 Troop
Priority to CN202110607849.5A priority Critical patent/CN113436249B/zh
Publication of CN113436249A publication Critical patent/CN113436249A/zh
Application granted granted Critical
Publication of CN113436249B publication Critical patent/CN113436249B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及一种快速稳健的单目相机位姿估计算法，所述方法包括使用S估计方法迭代计算标志点权值，通过加权正交迭代算法计算世界坐标系相对于相机的旋转矩阵和平移向量得到位姿，如此不断循环往复，直至收敛，得到优化后的位姿参数。本发明不仅可以自动识别粗差点、提高算法的稳健性，而且能实时、精确地求得旋转矩阵和平移向量，获得准确、可靠的单目相机位姿参数。

Description

一种快速稳健的单目相机位姿估计算法
技术领域
本发明涉及位姿估计技术领域，具体说是一种快速稳健的单目相机位姿估计算法。
背景技术
在已知单目相机内参数的情况下，根据给定的n个标志点在世界坐标系W中的三维坐标和对应的二维图像像素坐标，求解坐标系W相对于相机坐标系的旋转矩阵R和平移向量t的问题，称为2D-3D位姿估计问题，也称为n点透视(PnP)问题(文献[1])。
高效n点透视算法(EPnP)是第一个复杂度为O(n)的快速单目位姿估计算法(文献[2])，EPnP方法通过最小化代数误差，可以在线性复杂度的时间内解算旋转和平移参数。直接最小二乘位姿估计算法(DLS)(文献[3])和最优PnP算法(OPnP)(文献[4])是可以快速、精确、能够处理多解情况的算法，这两种算法均采用多项式结式的方法得到了像方残差的解析解，基于多项式结式的方法往往提供了很多可能的位姿估计解，其中的绝大多数可能解都是不符合要求的。统一PnP算法(UPnP)(文献[5])是一种能够同时处理中心投影模型和非中心投影模型的、复杂度为O(n)的几何最优算法。正交迭代算法(OI方法)(文献[6])是一种经典的广泛使用的迭代算法，该方法提出了一种基于物方残差的位姿估计算法，使用奇异值分解方法得到位姿参数，OI算法具有精度高和计算速度快的优点，是一种被广泛的应用的单目位姿估计方法。近年来，人们对OI方法进行了多方面的改进和推广应用。文献[7]将正交迭代算法应用于多目相机的位姿估计中，文献[8]对比了正交迭代算法与其他位姿估计算法的计算效率，文献[9]提出了提高正交迭代算法速度的方法，文献[10]和文献[11]基于加权形式的物方残差模型，分别提出了加权正交迭代算法(W-OI)和基于尺度估计的加权正交迭代算法(S-OI)。稳健PnP位姿估计算法(RPnP)是一种复杂度为O(n)的位姿估计算法(文献[12])，RPnP的基本思路是随机选择n条以标志点为端点的线段，从中选择一条长度最长的线段，以该线段中心为坐标原点，该线段为Z轴建立新的坐标系，利用两点间距离等式平方作为约束条件，通过求解驻点方法得到位姿参数结果，该方法的稳健性来源主要是在线段越长，端点的误差对线段的影响越小。然而当遇到大范围的粗差时，通过挑选最长线段的方法并不能增强算法的稳健性。
在实际应用过程中，受测量精度限制，世界坐标系W中的三维坐标和对应的二维图像像素坐标等测量值存在测量误差，同时，世界点坐标记录错误、世界点与图像点匹配错误、提取标志点像点坐标时发生较大偏差等情况会不同程度带来测量粗差，导致解算结果不可靠。在测量误差与粗差不可避免的情况下，如何得到实时的、精确的旋转矩阵和平移向量参数，是单目位姿估计算法在应用过程中需解决的重要问题。
其中，
文献[1]:Haralick R M,Joo H,Lee C N,et al.Pose estimation fromcorresponding point data[M]//Freeman H.Machine vision for inspection andmeasurement.Salt Lake City:Academic Press,1989:1-84.
文献[2]:Lepetit V,Moreno-Noguer F,Fua P.EPnP:an accurate O(N)solutionto the PnP problem[J].International Journal of Computer Vision,2009,81(2):155-166.
文献[3]:Hesch J A,Roumeliotis S I.A direct least-squares(DLS)methodfor PnP[C]//2011International Conference on Computer Vision,November 6-13,2011,Barcelona,Spain.New York:IEEE,2011:383-390.
文献[4]:Zheng Y,Kuang Y,Sugimoto S,et al.Revisiting the PnP Problem:AFast,General and Optimal Solution:IEEE International Conference on ComputerVision,2013[C].
文献[5]:Kneip L,Li H D,Seo Y.UPnP:an optimal O(N)solution to theabsolute Pose problem with universal applicability[M]//Fleet D,Pajdla T,Schiele B,et al.Computer vision–ECCV 2014.Lecture notes in computerscience.Cham:Springer,2014,8689:127-142.
文献[6]:Lu C P,Hager G D,Mjolsness E.Fast and globally convergentPose estimation from video images[J].IEEE Transactions on Pattern Analysisand Machine Intelligence,2000,22(6):610-622.
文献[7]:Xu Y X,Jiang Y L,Chen F.Generalized orthogonal iterativealgorithm for Pose estimation of multiple camera systems[J].Acta OpticaSinica,2009,29(1):72-77.
许允喜,蒋云良,陈方.多摄像机系统位姿估计的广义正交迭代算法[J].光学学报,2009,29(1):72-77.
文献[8]:Liu J B,Guo P Y,Li X,et al.Evaluation strategy for cameraPose estimation algorithm based on point correspondences[J].Acta OpticaSinica,2016,36(5):0515002.
刘进博,郭鹏宇,李鑫,等.基于点对应的相机姿态估计算法性能评价[J].光学学报,2016,36(5):0515002.
文献[9]:Li X,Long G C,Liu J B,et al.Accelerative orthogonal iterationalgorithm for camera Pose estimation[J].Acta Optica Sinica,2015,35(1):0115004.
李鑫,龙古灿,刘进博,等.相机位姿估计的加速正交迭代算法[J].光学学报,2015,35(1):0115004.
文献[10]:Zhou R,Zhang Z Y,Huang X H.Weighted orthogonal iterationalgorithm for camera Pose estimation[J].Acta Optica Sinica,2018,38(5):0515002.
周润,张征宇,黄叙辉.相机位姿估计的加权正交迭代算法[J].光学学报,2018,38(5):0515002.
文献[11]:张雄锋，刘海波，尚洋.单目相机位姿估计的稳健正交迭代方法[J].光学学报,2019.39(09):0915004.
文献[12]:Li S,Xu C,Xie M.A Robust O(n)Solution to the Perspective-n-Point Problem[J].IEEE Trans Pattern Anal Mach Intell,2012,34(7):1444-1450.
文献[13]:Susanti Y,Pratiwi H,Sulistijowati H S,et al.Mestimation,sestimation,and MM estimation in robust regression[J].International Journalof Pure and Apllied Mathematics,2014,91(3):349-360.
发明内容
针对上述问题，本发明提供一种自动识别粗差点，提高算法稳健性的快速稳健的单目相机位姿估计算法，也就是FR-OI算法。
本发明解决上述技术问题所采用的技术方案为：一种快速稳健的单目相机位姿估计算法，包括如下步骤：步骤1，将每个标志点的权值设置为
Figure BDA0003094697080000061
记第i个标志点在W坐标系中的坐标为pi，在归一化坐标平面中的坐标为vi(i＝1,2,…,n)，理想情况下相机成像的共线方程为
Rpi+t＝Vi(Rpi+t)，                          (1)
Figure BDA0003094697080000062
称为投影矩阵，采用迭代方法求解旋转矩阵和平移向量，当第i个标志点的权值固定为ωi时，其中ωi≥0且
Figure BDA0003094697080000063
使用加权物方残差和函数
Figure BDA0003094697080000064
作为优化目标函数求解位姿参数；
步骤2，将旋转矩阵初始化为三阶单位矩阵，通过公式(6)不断正交迭代计算旋转矩阵R(k)，直到物方残差小于阈值或正交迭代次数超过设定的最大迭代次数，输出当前的旋转矩阵，通过公式(3)计算对应的平移向量t(k)，具体如下，
假设第k次迭代时的旋转矩阵为R(k)，当R(k)固定时，使得加权物方残差和优化目标函数(2)最小的平移向量为
Figure BDA0003094697080000065
第k+1次迭代时的旋转矩阵为
Figure BDA0003094697080000071
其中
Figure BDA0003094697080000072
记
Figure BDA0003094697080000073
其中
Figure BDA0003094697080000074
表示矩阵的张量积，用r(k)＝ve(cR(k))表示将旋转矩阵R(k)的每一列依次排列形成的一个列向量，通过矩阵G，可将公式(2)重新写成t(k)＝Gr(k)，同理可将
Figure BDA0003094697080000075
改写成
Figure BDA0003094697080000076
其中
Figure BDA0003094697080000077
使用
Figure BDA0003094697080000078
表示加权重心坐标，记
Figure BDA0003094697080000079
由于
Figure BDA00030946970800000710
且
Figure BDA00030946970800000711
与旋转R无关，因此最小化式(4)等价于最大化目标函数
Figure BDA00030946970800000712
其中
Figure BDA00030946970800000713
tr()为矩阵的迹运算，则
Figure BDA00030946970800000714
这里
Figure BDA00030946970800000715
将矩阵M(k)进行奇异值分解得到
Figure BDA0003094697080000081
其中U(k)和V(k)为正交矩阵，
Figure BDA0003094697080000082
(j＝1,2,3)为M(k)的奇异值，则使得(5)式最小的旋转矩阵为
R(k+1)＝V(k)TU(k)，                            (6)
并作为下一次迭代的旋转矩阵。
当权值固定时，通过公式(6)可不断迭代计算R(k)，直至物方残差小于阙值或正交迭代次数超过设定的最大迭代次数时停止迭代，得到误差
Figure BDA0003094697080000083
计算当前权值对应的旋转矩阵，使用旋转矩阵R(k)，通过公式(3)计算对应的平移向量t(k)，得到当前权值对应加权位姿估计参数；
步骤3，使用S估计方法更新每个标志点的权值，再重复步骤2计算当前权值对应的旋转矩阵和平移向量，进一步筛选更新每个标志点的权值；
步骤4,不断循环往复步骤2和步骤3，直至物方残差小于阈值或使用S估计方法的次数超过最大迭代次数时停止迭代；
步骤5，输出旋转矩阵和平移向量，获得位姿参数。
作为优选，所述S估计方法更新权值的方法如下，
当循环次数为1时，对残差进行归一化得到
Figure BDA0003094697080000084
其中
Figure BDA0003094697080000085
表示|ei|的中位数，使用
Figure BDA0003094697080000091
更新权值；
当循环次数大于1时，使用
Figure BDA0003094697080000092
作为归一化残差，使用
Figure BDA0003094697080000093
更新权值。
与现有技术相比，本发明具有以下有益效果：
本发明使用基于加权系数的物方残差作为最小化目标函数，建立基于加权系数的单目相机位姿求解模型；推导得到加权单目相机位姿估计模型的快速迭代求解算法；通过使用S估计方法自动识别粗差点、自动计算每个标志点的权值，更新每个标志点的权值，提高算法的稳健性，并实时的、精确的求得旋转矩阵和平移向量，获得准确、可靠的单目相机位姿参数。
附图说明
图1是六种算法位姿解算中旋转矩阵误差随粗差点个数变化的情况图；
图2是六种算法位姿解算中平移向量误差随粗差点个数变化的情况图；
图3是六种算法位姿解算时间随标志点个数变化图。
具体实施方式
下面将结合图1-3详细说明本发明，在此本发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。
一种快速稳健的单目相机位姿估计算法，包括如下步骤：步骤1，将每个标志点的权值设置为
Figure BDA0003094697080000101
记第i个标志点在W坐标系中的坐标为pi，在归一化坐标平面中的坐标为vi(i＝1,2,…,n)，理想情况下相机成像的共线方程为
Rpi+t＝Vi(Rpi+t)，                          (1)
Figure BDA0003094697080000102
称为投影矩阵。为提高稳健性，本发明采用加权思想改进传统正交迭代算法的优化目标函数，使用S估计(文献[13])方法评估每个标志点的粗差程度，自动计算每个标志点的权值。权值的计算原则是粗差程度越高，权值越小，反之权值越大，通过选择合适的权值削弱粗差点对于位姿估计结果的影响，得到准确、可靠的位姿估计结果。
与传统OI算法类似，本发明采用迭代方法求解旋转矩阵和平移向量，当第i个标志点的权值固定为ωi时，其中ωi≥0且
Figure BDA0003094697080000103
使用加权物方残差和函数
Figure BDA0003094697080000104
作为优化目标函数求解位姿参数；
步骤2，将旋转矩阵初始化为三阶单位矩阵，通过公式(6)不断正交迭代计算旋转矩阵R(k)，直到物方残差小于阈值或正交迭代次数超过设定的最大迭代次数，输出当前的旋转矩阵，通过公式(3)计算对应的平移向量t(k)，具体如下
假设第k次迭代时的旋转矩阵为R(k)，当R(k)固定时，使得加权物方残差和优化目标函数(2)最小的平移向量为
Figure BDA0003094697080000111
第k+1次迭代时的旋转矩阵为
Figure BDA0003094697080000112
其中
Figure BDA0003094697080000113
记
Figure BDA0003094697080000114
其中
Figure BDA0003094697080000115
表示矩阵的张量积，用r(k)＝ve(cR(k))表示将旋转矩阵R(k)的每一列依次排列形成的一个列向量。通过矩阵G，可将公式(2)重新写成t(k)＝Gr(k)，同理可将
Figure BDA0003094697080000116
改写成
Figure BDA0003094697080000117
其中
Figure BDA0003094697080000118
使用
Figure BDA0003094697080000121
表示加权重心坐标，记
Figure BDA0003094697080000122
由于
Figure BDA0003094697080000123
且
Figure BDA0003094697080000124
与旋转R无关，因此最小化式(4)等价于最大化目标函数
Figure BDA0003094697080000125
其中
Figure BDA0003094697080000126
tr()为矩阵的迹运算，则
Figure BDA0003094697080000127
这里
Figure BDA0003094697080000128
将矩阵M(k)进行奇异值分解得到
Figure BDA0003094697080000129
其中U(k)和V(k)为正交矩阵，
Figure BDA00030946970800001210
(j＝1,2,3)为M(k)的奇异值，根据文献[11]可知(5)式最小的旋转矩阵为
R(k+1)＝V(k)TU(k).                              (6)
并作为下一次迭代的旋转矩阵，
因此，当权值固定时，通过公式(6)可不断迭代计算R(k)，直至物方残差小于阙值或正交迭代次数超过设定的最大迭代次数时停止迭代，得到误差
Figure BDA00030946970800001211
计算当前权值对应的旋转矩阵，使用旋转矩阵R(k)，通过公式(3)计算对应的平移向量t(k)，得到当前权值对应加权位姿估计参数；
步骤3，使用S估计方法更新每个标志点的权值，再重复步骤2计算当前权值对应的旋转矩阵和平移向量，进一步筛选更新每个标志点的权值；
S估计方法更新权值的方法如下，
当循环次数为1时，对残差进行归一化得到
Figure BDA0003094697080000131
其中
Figure BDA0003094697080000132
表示|ei|的中位数，使用
Figure BDA0003094697080000133
更新权值；
当循环次数大于1时，使用
Figure BDA0003094697080000134
作为归一化残差，使用
Figure BDA0003094697080000135
更新权值；
步骤4,权值更新后，重新使用加权正交迭代算法解算得到旋转矩阵，不断循环往复步骤2和步骤3，直至物方残差小于阈值或使用S估计方法的迭代次数超过最大迭代次数时停止迭代；
步骤5，输出旋转矩阵和平移向量，获得位姿参数。
为验证本发明所提出的快速稳健的单目相机位姿估计算法的有效性，通过图1-3在稳健性方面与经典位姿估计算法进行对比。
在以下的仿真实验中，标志点坐标、平移向量坐标单位均为厘米。仿真测试中相机的等效焦距为1200，相片分辨率为1000pixels×1000pixels，旋转矩阵为随机生成的行列式为1的正交矩阵，平移向量t的三个分量为在[500,1000]中选取的随机数。W坐标系中的100个理想标志点坐标均匀分布在[-60,60]×[-60,60]×[-60,60]中。首先将标志点相片坐标系中的理想点坐标加入均值为0，标准差为0.1的高斯噪声，用于模拟提点误差。旋转矩阵的误差定义为
Figure BDA0003094697080000141
其中R和Rtrue分别表示旋转矩阵解算值和真值，平移向量误差定义为
Figure BDA0003094697080000142
其中ttrue和t分别表示平移向量真值和解算值。为测试本发明算法的稳健性，在100个标志点随机选择m(m＝0,10,…,60个标志点，加入[-60,60]中均匀分布的粗差并使用含粗差的像点估计位姿参数，实验独立进行1000次。将DLS算法、RPnP算法、OI算法、W-OI算法、S-OI算法解算得到的位姿参数与本发明提出的算法(FR-OI)的结果进行对比。
从图1中可看出，在不存在粗差点的情况下，各算法的解算精度相当。随着粗差点个数的增加，RPnP算法、DLS算法和OI算法的姿态求解精度明显下降，W-OI算法对粗差点个数的敏感程度低于以上三种方法，本发明提出的FR-OI算法的求解精度最高。在存在60个粗差点的情况下，RPnP算法、DLS算法、OI算法、W-OI算法的姿态解算误差分别为2.29°，2.28°、1.11°和0.75°，而本发明算法的解算误差仅为0.17°，比其余四种算法的解算精度高1个数量级，与S-OI算法的解算精度相当。在平移向量误差方面，从图2中可看出，FR-OI算法与S-OI的平移向量求解精度最高，在存在60个粗差点的情况下，本发明算法与S-OI的解算误差仅为0.17％，RPnP算法、DLS算法、OI算法、W-OI算法的姿态解算误差分别为2.32％，2.65％、2.12％和0.93％，本发明提出的算法比其余四种算法高1到2个数量级。因此，与其余四种算法相比，本发明提出的算法对于粗差点个数不敏感，能够很好地抑制粗差点个数增加对位姿参数解算精度带来的影响。
为测试本发明算法的计算时间，当标志点个数依次为50,100，150,200,250,300,350,400时，对各算法的位姿解算时间进行了仿真测试，取500次仿真测试的平均时间作为各算法的计算时间。各算法的计算时间随标志点个数增加的变化情况如图3所示。从图3中可看出，W-OI算法和S-OI算法的位姿解算时间较长，实时性较差。本发明提出的FR-OI算法和与OI算法、DLS算法的位姿解算时间为同一数量级。平均位姿解算时间最短的是DLS算法。当标志点个数为400时，RPnP算法平均解算时间为0.0012秒，OI方法、DLS算法和FR-OI算法的平均位姿解算时间分别为0.0198秒、0.0541秒和0.1422秒，耗时较多的算法为W-OI方法和S-OI算法，所需时间分别为4.3453秒和4.1905秒。因此，本发明算法在保持稳健性的同时，具备较好的实时性，能够适应位姿估计实时性需求。
以上对本发明实施例所提供的技术方案进行了详细介绍，本文中应用了具体个例对本发明实施例的原理以及实施方式进行了阐述，以上实施例的说明只适用于帮助理解本发明实施例的原理；同时，对于本领域的一般技术人员，依据本发明实施例，在具体实施方式以及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (2)

1.一种快速稳健的单目相机位姿估计算法，其特征在于，包括以下步骤：
步骤1，将每个标志点的权值设置为
Figure QLYQS_1
记第i个标志点在W坐标系中的坐标为pi，在归一化坐标平面中的坐标为vi(i＝1,2,…,n)，理想情况下相机成像的共线方程为
Rpi+t＝Vi(Rpi+t)，                          (1)
Figure QLYQS_2
称为投影矩阵，采用迭代方法求解旋转矩阵和平移向量，当第i个标志点的权值固定为ωi时，其中ωi≥0且
Figure QLYQS_3
使用加权物方残差和函数
Figure QLYQS_4
作为优化目标函数求解位姿参数；
步骤2，将旋转矩阵初始化为三阶单位矩阵，通过公式(6)不断正交迭代计算旋转矩阵R(k)，直到物方残差小于阙值或正交迭代次数超过设定的最大迭代次数，输出当前的旋转矩阵，通过公式(3)计算对应的平移向量t(k)，具体如下，
假设第k次迭代时的旋转矩阵为R(k)，当R(k)固定时，使得加权物方残差和优化目标函数(2)最小的平移向量为
Figure QLYQS_5
第k+1次迭代时的旋转矩阵为
Figure QLYQS_6
其中
Figure QLYQS_7
记
Figure QLYQS_8
其中
Figure QLYQS_9
表示矩阵的张量积，用r(k)＝ve(cR(k))表示将旋转矩阵R(k)的每一列依次排列形成的一个列向量，通过矩阵G，可将公式(2)重新写成t(k)＝Gr(k)，同理可将
Figure QLYQS_10
改写成
Figure QLYQS_11
其中
Figure QLYQS_12
使用
Figure QLYQS_13
表示加权重心坐标，记
Figure QLYQS_14
由于
Figure QLYQS_15
且
Figure QLYQS_16
与旋转R无关，因此最小化式(4)等价于最大化目标函数
Figure QLYQS_17
其中
Figure QLYQS_18
tr()为矩阵的迹运算，则
Figure QLYQS_19
这里
Figure QLYQS_20
将矩阵M(k)进行奇异值分解得到
Figure QLYQS_21
其中U(k)和V(k)为正交矩阵，
Figure QLYQS_22
为M(k)的奇异值，则使得(5)式最小的旋转矩阵为
R(k+1)＝V(k)TU(k)，                             (6)
并作为下一次迭代的旋转矩阵，
当权值固定时，通过公式(6)可不断迭代计算R(k)，直至物方残差小于阙值或正交迭代次数超过设定的最大迭代次数时停止迭代，得到误差
Figure QLYQS_23
计算当前权值对应的旋转矩阵，使用旋转矩阵R(k)，通过公式(3)计算对应的平移向量t(k)，得到当前权值对应加权位姿估计参数；
步骤3，使用S估计方法更新每个标志点的权值，再重复步骤2计算当前权值对应的旋转矩阵和平移向量，进一步筛选更新每个标志点的权值；
步骤4,不断循环往复步骤2和步骤3，直至物方残差小于阈值或使用S估计方法的迭代次数超过最大迭代次数时停止迭代；
步骤5，输出旋转矩阵和平移向量，获得位姿参数。
2.根据权利要求1所述快速稳健的单目相机位姿估计算法，其特征在于：所述S估计方法更新权值的方法如下，
当循环次数为1时，对残差进行归一化得到
Figure QLYQS_24
其中
Figure QLYQS_25
表示|ei|的中位数，使用
Figure QLYQS_26
更新权值；
当循环次数大于1时，使用
Figure QLYQS_27
作为归一化残差，使用
Figure QLYQS_28
更新权值。
CN202110607849.5A 2021-06-01 2021-06-01 一种快速稳健的单目相机位姿估计算法 Active CN113436249B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110607849.5A CN113436249B (zh) 2021-06-01 2021-06-01 一种快速稳健的单目相机位姿估计算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110607849.5A CN113436249B (zh) 2021-06-01 2021-06-01 一种快速稳健的单目相机位姿估计算法

Publications (2)

Publication Number Publication Date
CN113436249A CN113436249A (zh) 2021-09-24
CN113436249B true CN113436249B (zh) 2023-04-11

Family

ID=77803391

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110607849.5A Active CN113436249B (zh) 2021-06-01 2021-06-01 一种快速稳健的单目相机位姿估计算法

Country Status (1)

Country Link
CN (1) CN113436249B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114240998A (zh) * 2021-11-17 2022-03-25 乐山师范学院 一种机器人视觉识别定位方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10388029B1 (en) * 2017-09-07 2019-08-20 Northrop Grumman Systems Corporation Multi-sensor pose-estimate system
CN112066879A (zh) * 2020-09-11 2020-12-11 哈尔滨工业大学 基于计算机视觉的气浮运动模拟器位姿测量装置及方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107871327A (zh) * 2017-10-23 2018-04-03 武汉大学 基于特征点线的单目相机位姿估计和优化方法及系统
US10929654B2 (en) * 2018-03-12 2021-02-23 Nvidia Corporation Three-dimensional (3D) pose estimation from a monocular camera
CN108711166B (zh) * 2018-04-12 2022-05-03 浙江工业大学 一种基于四旋翼无人机的单目相机尺度估计方法
CN109816725B (zh) * 2019-01-17 2023-03-14 合肥哈工慧拣智能科技有限公司 一种基于深度学习的单目相机物体位姿估计方法及装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10388029B1 (en) * 2017-09-07 2019-08-20 Northrop Grumman Systems Corporation Multi-sensor pose-estimate system
CN112066879A (zh) * 2020-09-11 2020-12-11 哈尔滨工业大学 基于计算机视觉的气浮运动模拟器位姿测量装置及方法

Also Published As

Publication number Publication date
CN113436249A (zh) 2021-09-24

Similar Documents

Publication Publication Date Title
CN108510551B (zh) 一种远距离大视场条件下相机参数的标定方法及系统
CN110807809B (zh) 基于点线特征和深度滤波器的轻量级单目视觉定位方法
CN104019799B (zh) 一种利用局部参数优化计算基础矩阵的相对定向方法
CN109523595B (zh) 一种建筑工程直线棱角间距视觉测量方法
CN111383272B (zh) 一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法
CN111429494B (zh) 一种基于生物视觉的点云高精度自动配准方法
CN113920205B (zh) 一种非同轴相机的标定方法
CN107680139A (zh) 一种远心双目立体视觉测量系统的通用性标定方法
CN110223355B (zh) 一种基于双重极线约束的特征标志点匹配方法
CN108470356A (zh) 一种基于双目视觉的目标对象快速测距方法
CN112329726B (zh) 人脸识别方法及装置
CN113436249B (zh) 一种快速稳健的单目相机位姿估计算法
Jianchao Image registration based on both feature and intensity matching
Ren et al. High precision calibration algorithm for binocular stereo vision camera using deep reinforcement learning
CN109341588B (zh) 一种双目结构光三系统法视角加权的三维轮廓测量方法
CN111998823B (zh) 基于双目异光源测距装置的目标测距方法
CN114581515A (zh) 一种基于最优路径转换的多相机标定参数优化方法
CN116894876A (zh) 基于实时图像的6-dof的定位方法
CN116363205A (zh) 基于深度学习的空间目标位姿解算方法及计算机程序产品
CN114399547B (zh) 一种基于多帧的单目slam鲁棒初始化方法
Wang et al. Facilitating PTZ camera auto-calibration to be noise resilient with two images
CN115018922A (zh) 畸变参数标定方法、电子设备和计算机可读存储介质
JP5215615B2 (ja) ３次元位置情報復元装置およびその方法
Zhou et al. Distortion correction using a single image based on projective invariability and separate model
CN109840894A (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