CN113298879A - 视觉定位方法、装置及存储介质和电子设备 - Google Patents

视觉定位方法、装置及存储介质和电子设备 Download PDF

Info

Publication number
CN113298879A
CN113298879A CN202110574586.2A CN202110574586A CN113298879A CN 113298879 A CN113298879 A CN 113298879A CN 202110574586 A CN202110574586 A CN 202110574586A CN 113298879 A CN113298879 A CN 113298879A
Authority
CN
China
Prior art keywords
pose
coordinate system
apriltag
camera
deviation
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
Application number
CN202110574586.2A
Other languages
English (en)
Other versions
CN113298879B (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.)
Beijing Jingdong Qianshi Technology Co Ltd
Original Assignee
Beijing Jingdong Qianshi Technology Co Ltd
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 Beijing Jingdong Qianshi Technology Co Ltd filed Critical Beijing Jingdong Qianshi Technology Co Ltd
Priority to CN202110574586.2A priority Critical patent/CN113298879B/zh
Publication of CN113298879A publication Critical patent/CN113298879A/zh
Application granted granted Critical
Publication of CN113298879B publication Critical patent/CN113298879B/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/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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
    • 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)
  • Image Processing (AREA)

Abstract

本发明实施例提出视觉定位方法、装置及存储介质和电子设备。方法包括：获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；获取相机在两两相邻位置间的测量位姿变化量；根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。本发明实施例提高了视觉定位准确度。

Description

视觉定位方法、装置及存储介质和电子设备
技术领域
本发明涉及定位技术领域，尤其涉及视觉定位方法、装置及可读存储介质和电子设备。
背景技术
Apriltag是一种视觉定位标志图形，与二维码、条形码等相似。使用相机拍摄一张包含Apriltag的图像后，根据已有的Apriltag算法，通过解码、位姿计算等步骤可以计算出相机坐标系在Apriltag坐标系下的位姿包括：位置和角度。
Apriltag常被用来为搭载相机的设备(如：移动机器人)进行定位。当相机距离Apriltag较远时，由于相机的分辨率有限，导致根据Apriltag计算出的相机坐标系的位姿不够准确。
发明内容
本发明实施例提出视觉定位方法、装置及可读存储介质和电子设备，以提高视觉定位准确度。
本发明实施例的技术方案是这样实现的：
一种视觉定位方法，该方法包括：
获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；
根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；
获取相机在两两相邻位置间的测量位姿变化量；
根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
所述根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，包括：
根据相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度，以及相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正。
所述相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度通过如下方式得到：
将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和。
所述相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度通过如下方式得到：
将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二偏离度之和。
所述对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正包括：
将第一偏离度之和与第二偏离度之和相加，得到总偏离度；以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值；
且，将相机坐标系在Apriltag坐标系下的各实际位姿的最优值作为相机坐标系在Apriltag坐标系下的各最优位姿。
所述将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，包括：
分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿；
将每个实际位姿分别右减其对应的估计位姿，对每个右减运算结果分别与预设的第一方差矩阵进行二范数运算，则每个二范数运算结果分别为每个实际位姿与其对应的估计位姿之间的第一偏离度，其中，所述右减为二维刚体运动群的右减操作。
所述将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，包括：
分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿；
分别对相机在每两个相邻位置间的实际位姿进行右减运算，将各右减运算结果作为相机在两两相邻位置间的各实际位姿变化量；其中，所述右减为二维刚体运动群的右减操作；
分别对相机在两两相邻位置间的实际位姿变化量与其对应的测量位姿变化量进行向量减运算；
将每个向量减运算结果分别与预设的第二方差矩阵进行二范数运算，则每个二范数运算结果分别为相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的第二偏离度。
一种视觉定位装置，该装置包括：
图像获取模块，用于获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；
位姿估计模块，用于根据获取到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；
位姿变化量测量模块，用于获取相机在两两相邻位置间的测量位姿变化量；
位姿优化模块，用于根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
一种非瞬时计算机可读存储介质，所述非瞬时计算机可读存储介质存储指令，所述指令在由处理器执行时使得所述处理器执行如上任一项所述的视觉定位方法的步骤。
一种电子设备，包括如上所述的非瞬时计算机可读存储介质、以及可访问所述非瞬时计算机可读存储介质的所述处理器。
本发明实施例中，在根据相机采集到的每帧图像中的Apriltag，分别计算出每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿后，再根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，从而提高了视觉定位的准确度。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一实施例提供的视觉定位方法流程图；
图2为本发明另一实施例提供的视觉定位方法流程图；
图3为本发明一实施例提供的视觉定位装置的结构示意图；
图4为本发明实施例提供的一种电子设备的示例性结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其他步骤或单元。
下面以具体实施例对本发明的技术方案进行详细说明。下面几个具体实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例不再赘述。
本发明实施例提出一种视觉定位方法，该方法中，获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；获取相机在两两相邻位置间的测量位姿变化量；根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。本发明实施例通过相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，从而提高了视觉定位的准确度。
图1为本发明一实施例提供的视觉定位方法流程图，其具体步骤如下：
步骤101：获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像。
步骤102：根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿。
Apriltag通常为一个四边形。本步骤中，对于每帧图像，先对该帧图像中的Apriltag进行解码，再根据解码出的Apriltag计算该帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿，具体的解码和计算过程都属于已有技术，不再赘述。
其中，相机坐标系和Apriltag坐标系预先定义，一种定义方式如下：
相机坐标系：以相机的镜头的中心为原点，以镜头的法线(即垂直于镜头所在平面的线)为X轴，其中，指向Apriltag的方向为X轴正向，在镜头所在平面上选择一条以原点为起始点的射线作为Y轴正向，由于镜头所在平面与法线垂直，则Y轴必然与X轴垂直；
Apriltag坐标系：以解码出的Apriltag的中心为原点，以Apriltag所在平面的法线为X轴，且以指向相机镜头的方向为X轴正向，在Apriltag所在平面上选择一条以原点为起始点的射线作为Y轴正向。
步骤103：获取相机在两两相邻位置间的测量位姿变化量。
例如：相机分别在N个位置处，一共采集到N帧包含Apriltag的图像，则有N-1对相邻位置，从而有N-1个位姿变化量。
实际应用中，可根据相机上的里程计测量的数据，得到相机在两两相邻位置间的位姿变化量，为了与之后的实际位姿变化量进行区分，本步骤中获取的位姿变化量称为测量位姿变化量。
步骤104：根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
本发明实施例中，在根据相机采集到的每帧图像中的Apriltag，分别计算出每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿后，再根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，从而提高了视觉定位的准确度。
一可选实施例中，步骤104中，根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，包括：根据相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度，以及相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正。
一可选实施例中，相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度通过如下方式得到：将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和。
一可选实施例中，相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度通过如下方式得到：将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二偏离度之和。
一可选实施例中，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正包括：将第一偏离度之和与第二偏离度之和相加，得到总偏离度；以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值；且，将相机坐标系在Apriltag坐标系下的各实际位姿的最优值作为相机坐标系在Apriltag坐标系下的各最优位姿。
上述实施例，将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度和加上相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度和，得到总偏离度，并以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值，从而提高了视觉定位的准确度。
一可选实施例中，将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，包括：分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿；将每个实际位姿分别右减其对应的估计位姿，对每个右减运算结果分别与预设的第一方差矩阵进行二范数运算，则每个二范数运算结果分别为每个实际位姿与其对应的估计位姿之间的第一偏离度，其中，所述右减为二维刚体运动群的右减操作。一可选实施例中，将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，包括：分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿；分别对相机在每两个相邻位置间的实际位姿进行右减运算，将各右减运算结果作为相机在两两相邻位置间的各实际位姿变化量；其中，所述右减为二维刚体运动群的右减操作；分别对相机在两两相邻位置间的实际位姿变化量与其对应的测量位姿变化量进行向量减运算；将每个向量减运算结果分别与预设的第二方差矩阵进行二范数运算，则每个二范数运算结果分别为相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的第二偏离度。
图2为本发明另一实施例提供的视觉定位方法流程图，其具体步骤如下：
步骤201：获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像。
步骤202：根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿。
Apriltag通常为一个四边形。本步骤中，对于每帧图像，先对该帧图像中的Apriltag进行解码，再根据解码出的Apriltag计算该帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿，具体的解码和计算过程都属于已有技术，不再赘述。
其中，相机坐标系和Apriltag坐标系预先定义，一种定义方式如下：
相机坐标系：以相机的镜头的中心为原点，以镜头的法线(即垂直于镜头所在平面的线)为X轴，其中，指向Apriltag的方向为X轴正向，在镜头所在平面上选择一条以原点为起始点的射线作为Y轴正向，由于镜头所在平面与法线垂直，则Y轴必然与X轴垂直；
Apriltag坐标系：以解码出的Apriltag的中心为原点，以Apriltag所在平面的法线为X轴，且以指向相机镜头的方向为X轴正向，在Apriltag所在平面上选择一条以原点为起始点的射线作为Y轴正向。
步骤203：获取相机在两两相邻位置间的测量位姿变化量。
例如：相机分别在N个位置处，一共采集到N帧包含Apriltag的图像，则有N-1对相邻位置，从而有N-1个位姿变化量。
实际应用中，可根据相机上的里程计测量的数据，得到相机在两两相邻位置间的位姿变化量，为了与之后的实际位姿变化量进行区分，本步骤中获取的位姿变化量称为测量位姿变化量。
步骤204：将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和。
实际位姿(或真实位姿)即本发明实施例待求解的最优位姿。
相机坐标系在Apriltag坐标系下的估计位姿、实际位姿都可由二维刚体运动群元素表示，是满足一定条件的三维实矩阵
Figure BDA0003083845370000091
即
Figure BDA0003083845370000092
其中，x、y表示相机坐标系在Apriltag坐标系下的位置坐标，具体地，x、y的取值可为：相机坐标系的原点在Apriltag坐标系下的x、y坐标；θ表示相机坐标系在Apriltag坐标系下的角度，具体地，θ的取值可为：相机坐标系的X轴正向和Apriltag坐标系的X轴正向之间的夹角。
设：根据相机采集的第k(1≤k≤N)帧包含Apriltag的图像，计算得到的相机坐标系在Apriltag坐标系下的估计位姿为meask，meask对应的实际位姿为posek，即，相机采集第k帧包含Apriltag的图像时，相机坐标系在Apriltag坐标系下的实际位姿(或真实位姿)为posek，其中，meask和posek都采用式子(1)所示的矩阵表示；
则：相机采集第k帧包含Apriltag的图像时，posek与meask之间的偏离度(即第一偏离度)可表示为：
Figure BDA0003083845370000093
其中，
Figure BDA0003083845370000108
为二维刚体运动群的减操作符，
Figure BDA0003083845370000109
可分为：左
Figure BDA00030838453700001010
和右
Figure BDA00030838453700001011
较佳地，本发明实施例中采用右
Figure BDA00030838453700001012
右
Figure BDA00030838453700001013
是二维刚体运动群的一种减操作，右
Figure BDA00030838453700001014
的计算方式为已有技术，例如：右
Figure BDA00030838453700001015
的一种计算方式定义如下：
Figure BDA0003083845370000101
其中，Y、X为以式子(1)表示的二维刚体运动群元素，X-1为X的逆矩阵，ο为矩阵乘运算符，Log()为将二维刚体运动群元素转换为三维向量的运算符。
根据以上右
Figure BDA00030838453700001016
的定义：
Figure BDA0003083845370000102
Figure BDA0003083845370000103
为二范数运算符，R为预设的第一方差矩阵，R表示posek与meask之间的方差，可表示为如下的对角矩阵：
Figure BDA0003083845370000104
其中，r1、r2、r3的取值预先设定，可根据经验等设定，0≤r1、r2、r3≤1。
二范数的计算公式如下：
Figure BDA0003083845370000105
其中，T为转置运算符。
步骤205：将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二第二偏离度之和。
设：相机在相邻位置间的各测量位姿变化量表示为：controlk(1≤k≤N)，即，在采集第k+1帧和第k帧包含Apriltag的图像时相机的测量位姿变化量为controlk，controlk可由相机上的里程计采集到的数据得到；
Figure BDA0003083845370000106
为controlk对应的实际位姿变化量，即，在采集第k+1帧和第k帧包含Apriltag的图像时相机的实际位姿变化量(或真实位姿变化量)为
Figure BDA0003083845370000107
则：在采集第k+1帧和第k帧包含Apriltag的图像时相机的实际位姿变化量与其对应的测量位姿变化量之间的偏离度(即第二偏离度)可表示为：
Figure BDA0003083845370000111
其中，posek+1为meask+1对应的实际位姿，meask+1为根据采集到的第k+1帧包含Apriltag的图像计算得到的相机坐标系在Apriltag坐标系下的估计位姿；controlk为在采集第k+1帧和第k帧包含Apriltag的图像时相机的测量位姿变化量；
Figure BDA0003083845370000112
为二范数运算符，Q为预设的第二方差矩阵；
posek+1由二维刚体运动群元素表示，例如可由式子(1)表示；
controlk由三维向量表示，由位置变化量和角度变化量组成，如：controlk＝(Δxk、Δyk、Δθk)，其中，Δxk、Δyk表示采集第k+1帧和第k帧包含Apriltag的图像时相机的测量位置变化量、Δθk表示采集第k+1帧和第k帧包含Apriltag的图像时相机的测量角度变化量；
Q可表示为如下的对角矩阵：
Figure BDA0003083845370000113
其中，q1、q2、q3的取值预先设定，可根据经验等设定，0≤q1、q2、q3≤1。
步骤206：将第一偏离度之和与第二偏离度之和相加，得到总偏离度。
步骤207：以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值；将相机坐标系在Apriltag坐标系下的各实际位姿的最优值作为相机坐标系在Apriltag坐标系下的各最优位姿。
即，对如下式子进行求解：
Figure BDA0003083845370000114
得到posek(1≤k≤N)，则：posek(1≤k≤N)即为相机在每个位置处时相机坐标系在Apriltag坐标系下的各实际位姿的最优解。
其中，可采用高斯牛顿法、隆贝格-马奎特法(LM法)、或者狗腿法(dog-leg法)等对式子(2)求解。
上述实施例中，将计算得到的相机坐标系在Apriltag坐标系下的各估计位姿与其对应的实际位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和；将相机在两两相邻位置间的各测量位姿变化量与其对应的实际位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二偏离度之和；将第一偏离度之和与第二偏离度之和相加，得到总偏离度；以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值，从而最大可能地减少了最终计算得到的相机坐标系在Apriltag坐标系下的位姿的误差。
图3为本发明实施例提供的视觉定位装置的结构示意图，该装置主要包括：
图像获取模块31，用于获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；
位姿估计模块32，用于根据图像获取模块31获取到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；
位姿变化量测量模块33，用于获取相机在两两相邻位置间的测量位姿变化量；
位姿优化模块34，用于根据位姿变化量测量模块33获取到的相机在两两相邻位置间的测量位姿变化量，对位姿估计模块32计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
一可选实施例中，位姿优化模块34根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，包括：根据相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度，以及相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正。
一可选实施例中，位姿优化模块34通过如下方式得到相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度：将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和。
一可选实施例中，位姿优化模块34通过如下方式得到相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度：将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二偏离度之和。
一可选实施例中，位姿优化模块34对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正包括：将第一偏离度之和与第二偏离度之和相加，得到总偏离度；以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值；
且，将相机坐标系在Apriltag坐标系下的各实际位姿的最优值作为相机坐标系在Apriltag坐标系下的各最优位姿。
一可选实施例中，位姿优化模块34将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，包括：分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿；将每个实际位姿分别右减其对应的估计位姿，对每个右减运算结果分别与预设的第一方差矩阵进行二范数运算，则每个二范数运算结果分别为每个实际位姿与其对应的估计位姿之间的第一偏离度，其中，所述右减为二维刚体运动群的右减操作。
一可选实施例中，位姿优化模块34将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，包括：分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿；分别对相机在每两个相邻位置间的实际位姿进行右减运算，将各右减运算结果作为相机在两两相邻位置间的各实际位姿变化量；其中，所述右减为二维刚体运动群的右减操作；分别对相机在两两相邻位置间的实际位姿变化量与其对应的测量位姿变化量进行向量减运算；将每个向量减运算结果分别与预设的第二方差矩阵进行二范数运算，则每个二范数运算结果分别为相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的第二偏离度。
本申请实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储指令，所述指令在由处理器执行时可执行如上所述视觉定位方法中的步骤。实际应用中，所述的计算机可读介质可以是上述实施例各设备/装置/系统所包含的，也可以是单独存在，而未装配入该设备/装置/系统中。其中，在计算机可读存储介质中存储指令，其存储的指令在由处理器执行时可执行如上视觉定位方法中的步骤。
根据本申请公开的实施例，计算机可读存储介质可以是非易失性的计算机可读存储介质，例如可以包括但不限于：便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件，或者上述的任意合适的组合，但不用于限制本申请保护的范围。在本申请公开的实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
如图4所示，本发明实施例还提供一种电子设备。如图4所示，其示出了本发明实施例所涉及的电子设备的结构示意图，具体来讲：
该电子设备可以包括一个或一个以上处理核心的处理器41、一个或一个以上计算机可读存储介质的存储器42以及存储在存储器上并可在处理器上运行的计算机程序。在执行所述存储器42的程序时，可以实现上述视觉定位方法。
具体的，实际应用中，该电子设备还可以包括电源43、输入输出单元44等部件。本领域技术人员可以理解，图4中示出的电子设备的结构并不构成对该电子设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。其中：
处理器41是该电子设备的控制中心，利用各种接口和线路连接整个电子设备的各个部分，通过运行或执行存储在存储器42内的软件程序和/或模块，以及调用存储在存储器42内的数据，执行服务器的各种功能和处理数据，从而对该电子设备进行整体监控。
存储器42可用于存储软件程序以及模块，即上述计算机可读存储介质。处理器41通过运行存储在存储器42的软件程序以及模块，从而执行各种功能应用以及数据处理。存储器42可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序等；存储数据区可存储根据服务器的使用所创建的数据等。此外，存储器42可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。相应地，存储器42还可以包括存储器控制器，以提供处理器41对存储器42的访问。
该电子设备还包括给各个部件供电的电源43，可以通过电源管理系统与处理器41逻辑相连，从而通过电源管理系统实现管理充电、放电、以及功耗管理等功能。电源43还可以包括一个或一个以上的直流或交流电源、再充电系统、电源故障检测电路、电源转换器或者逆变器、电源状态指示器等任意组件。
该电子设备还可包括输入输出单元44，该输入单元输出44可用于接收输入的数字或字符信息，以及产生与用户设置以及功能控制有关的键盘、鼠标、操作杆、光学或者轨迹球信号输入。该输入单元输出44还可以用于显示由用户输入的信息或提供给用户的信息以及各种图像用户接口，这些图形用户接口可以由图形、文本、图标、视频和其任意组合来构成。
本申请附图中的流程图和框图，示出了按照本申请公开的各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或者代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应该注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同附图中所标准的顺序发生。例如，两个连接地表示的方框实际上可以基本并行地执行，它们有时也可以按照相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或者流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
本领域技术人员可以理解，本公开的各个实施例和/或权利要求中记载的特征可以进行多种组合和/或结合，即使这样的组合或结合没有明确记载于本申请中。特别地，在不脱离本申请精神和教导的情况下，本申请的各个实施例和/或权利要求中记载的特征可以进行多种组合和/或结合，所有这些组合和/或结合均落入本申请公开的范围。
本文中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思路，并不用于限制本申请。对于本领域的技术人员来说，可以依据本发明的思路、精神和原则，在具体实施方式及应用范围上进行改变，其所做的任何修改、等同替换、改进等，均应包含在本申请保护的范围之内。

Claims (10)

1.一种视觉定位方法，其特征在于，该方法包括：
获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；
根据采集到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；
获取相机在两两相邻位置间的测量位姿变化量；
根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
2.根据权利要求1所述的方法，其特征在于，所述根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，包括：
根据相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度，以及相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正。
3.根据权利要求2所述的方法，其特征在于，所述相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度通过如下方式得到：将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，将各第一偏离度相加得到第一偏离度之和。
4.根据权利要求3所述的方法，其特征在于，所述相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度通过如下方式得到：
将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，将各第二偏离度相加得到第二偏离度之和。
5.根据权利要求4所述的方法，其特征在于，所述对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正包括：
将第一偏离度之和与第二偏离度之和相加，得到总偏离度；以总偏离度最小为目标，计算相机坐标系在Apriltag坐标系下的各实际位姿的最优值；
且，将相机坐标系在Apriltag坐标系下的各实际位姿的最优值作为相机坐标系在Apriltag坐标系下的各最优位姿。
6.根据权利要求3或5所述的方法，其特征在于，所述将相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿之间的偏离度分别作为一第一偏离度，包括：
分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿与其对应的估计位姿；
将每个实际位姿分别右减其对应的估计位姿，对每个右减运算结果分别与预设的第一方差矩阵进行二范数运算，则每个二范数运算结果分别为每个实际位姿与其对应的估计位姿之间的第一偏离度，其中，所述右减为二维刚体运动群的右减操作。
7.根据权利要求4或5所述的方法，其特征在于，所述将相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的偏离度分别作为一第二偏离度，包括：
分别采用二维刚体运动群的三维实矩阵表示相机坐标系在Apriltag坐标系下的各实际位姿；
分别对相机在每两个相邻位置间的实际位姿进行右减运算，将各右减运算结果作为相机在两两相邻位置间的各实际位姿变化量；其中，所述右减为二维刚体运动群的右减操作；
分别对相机在两两相邻位置间的实际位姿变化量与其对应的测量位姿变化量进行向量减运算；
将每个向量减运算结果分别与预设的第二方差矩阵进行二范数运算，则每个二范数运算结果分别为相机在两两相邻位置间的各实际位姿变化量与其对应的测量位姿变化量之间的第二偏离度。
8.一种视觉定位装置，其特征在于，该装置包括：
图像获取模块，用于获取相机在不同位置处分别采集的包含Apriltag的图像，其中，相机在每个位置处分别采集一帧包含Apriltag的图像；
位姿估计模块，用于根据获取到的每帧图像中的Apriltag，分别计算每帧图像对应的相机坐标系在Apriltag坐标系下的估计位姿；
位姿变化量测量模块，用于获取相机在两两相邻位置间的测量位姿变化量；
位姿优化模块，用于根据相机在两两相邻位置间的测量位姿变化量，对计算得到的相机坐标系在Apriltag坐标系下的各估计位姿进行修正，得到相机坐标系在Apriltag坐标系下的各最优位姿。
9.一种非瞬时计算机可读存储介质，所述非瞬时计算机可读存储介质存储指令，其特征在于，所述指令在由处理器执行时使得所述处理器执行如权利要求1至7中任一项所述的视觉定位方法的步骤。
10.一种电子设备，其特征在于，包括如权利要求9所述的非瞬时计算机可读存储介质、以及可访问所述非瞬时计算机可读存储介质的所述处理器。
CN202110574586.2A 2021-05-26 2021-05-26 视觉定位方法、装置及存储介质和电子设备 Active CN113298879B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110574586.2A CN113298879B (zh) 2021-05-26 2021-05-26 视觉定位方法、装置及存储介质和电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110574586.2A CN113298879B (zh) 2021-05-26 2021-05-26 视觉定位方法、装置及存储介质和电子设备

Publications (2)

Publication Number Publication Date
CN113298879A true CN113298879A (zh) 2021-08-24
CN113298879B CN113298879B (zh) 2024-04-16

Family

ID=77325006

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110574586.2A Active CN113298879B (zh) 2021-05-26 2021-05-26 视觉定位方法、装置及存储介质和电子设备

Country Status (1)

Country Link
CN (1) CN113298879B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012123667A (ja) * 2010-12-09 2012-06-28 Panasonic Corp 姿勢推定装置および姿勢推定方法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
WO2019119328A1 (zh) * 2017-12-20 2019-06-27 深圳市大疆创新科技有限公司 一种基于视觉的定位方法及飞行器
CN110503688A (zh) * 2019-08-20 2019-11-26 上海工程技术大学 一种用于深度相机的位姿估计方法
CN110750094A (zh) * 2018-07-23 2020-02-04 杭州海康威视数字技术股份有限公司 确定可移动设备的位姿变化信息的方法、装置和系统
CN111442722A (zh) * 2020-03-26 2020-07-24 达闼科技成都有限公司 定位方法、装置、存储介质及电子设备
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012123667A (ja) * 2010-12-09 2012-06-28 Panasonic Corp 姿勢推定装置および姿勢推定方法
WO2019119328A1 (zh) * 2017-12-20 2019-06-27 深圳市大疆创新科技有限公司 一种基于视觉的定位方法及飞行器
CN110750094A (zh) * 2018-07-23 2020-02-04 杭州海康威视数字技术股份有限公司 确定可移动设备的位姿变化信息的方法、装置和系统
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN110503688A (zh) * 2019-08-20 2019-11-26 上海工程技术大学 一种用于深度相机的位姿估计方法
CN111442722A (zh) * 2020-03-26 2020-07-24 达闼科技成都有限公司 定位方法、装置、存储介质及电子设备
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质

Also Published As

Publication number Publication date
CN113298879B (zh) 2024-04-16

Similar Documents

Publication Publication Date Title
CN110322500B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN108805917B (zh) 空间定位的方法、介质、装置和计算设备
CN110570477B (zh) 一种标定相机和旋转轴相对姿态的方法、装置和存储介质
CN110111388B (zh) 三维物体位姿参数估计方法及视觉设备
CN110335317B (zh) 基于终端设备定位的图像处理方法、装置、设备和介质
US20150213617A1 (en) Method and apparatus for estimating position
CN111127422A (zh) 图像标注方法、装置、系统及主机
CN109241844B (zh) 三维物体的姿态估计方法、装置、设备及存储介质
CN112686877B (zh) 基于双目相机的三维房屋损伤模型构建测量方法及系统
CN110349212B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
US9591228B2 (en) Method for the localization of a tool in a workplace, corresponding system and computer program product
US10249058B2 (en) Three-dimensional information restoration device, three-dimensional information restoration system, and three-dimensional information restoration method
CN103875020A (zh) 基于定向传感器解决单应性分解不明确性
CN109255801B (zh) 视频中三维物体边缘追踪的方法、装置、设备及存储介质
CN111462179A (zh) 三维物体跟踪方法、装置及电子设备
US20200193637A1 (en) Image processing apparatus and image processing method
CN111627070A (zh) 一种对旋转轴进行标定的方法、装置和存储介质
JP2012164188A (ja) 画像処理装置、画像処理方法およびプログラム
CN113601510A (zh) 基于双目视觉的机器人移动控制方法、装置、系统及设备
CN113298879B (zh) 视觉定位方法、装置及存储介质和电子设备
CN112085842B (zh) 深度值确定方法及装置、电子设备和存储介质
CN115019150B (zh) 目标检测定点模型建立方法、装置及可读存储介质
CN113628284A (zh) 位姿标定数据集生成方法、装置、系统、电子设备及介质
WO2020166401A1 (ja) 学習データ生成装置、方法及びプログラム
Saeedi et al. 3D localization and tracking in unknown environments

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