CN112857215B - 一种基于正二十面体的单目6d位姿估计方法 - Google Patents

一种基于正二十面体的单目6d位姿估计方法 Download PDF

Info

Publication number
CN112857215B
CN112857215B CN202110023412.7A CN202110023412A CN112857215B CN 112857215 B CN112857215 B CN 112857215B CN 202110023412 A CN202110023412 A CN 202110023412A CN 112857215 B CN112857215 B CN 112857215B
Authority
CN
China
Prior art keywords
regular icosahedron
aruco
icosahedron
current frame
code
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
CN202110023412.7A
Other languages
English (en)
Other versions
CN112857215A (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.)
Hebei University of Technology
Original Assignee
Hebei University of Technology
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 Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202110023412.7A priority Critical patent/CN112857215B/zh
Publication of CN112857215A publication Critical patent/CN112857215A/zh
Application granted granted Critical
Publication of CN112857215B publication Critical patent/CN112857215B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/02—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness
    • G01B11/03—Measuring arrangements characterised by the use of optical techniques for measuring length, width or thickness by measuring coordinates of points
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于正二十面体的单目6D位姿估计方法。本方法将ArUco码与正二十面体相结合，再使用单目相机采集被ArUco码标记的正二十面体图像，计算ArUco码的中心点的空间三维坐标，获得其内切球的稀疏点云，进而利用多元非线性拟合以及迭代最近点算法比对相机坐标处的虚拟正二十面体，进而计算出当前正二十面体的空间位移量以及旋转角度，间接计算出被测物体的位姿。本方法克服了现有方法对光照、环境及设备的高要求，检测迅速，提升了稳定性；不需要在前期投入大量的精力进行模板采集或模型训练，显著降低了位姿估计成本，保证了位姿估计精度并加强通用性，并且能够进行物体大幅度运动的位姿估计。

Description

一种基于正二十面体的单目6D位姿估计方法
技术领域
本发明属于图像识别位姿检测领域，具体是一种基于正二十面体的单目6D位姿估计方法。
背景技术
目标的位姿估计无论是在生产还是生活方面都扮演者重要的角色，是实现机器人运动灵活有效的前提。目前工业自动化行业发展迅猛，对机器人的状态进行实时检测并反馈的要求也不断提高，而基于视觉的目标位姿估计对机器人性能的提高有重要意义。单目视觉***仅使用一台相机，结构简单成本低廉，应用比较广泛。目前目标的位姿估计的主要方法有：
文献《LEPETIT V,PILET J,FUA P.Point matching as a classificationproblem for fast and robust object pose estimation[C]//Proceedings of the2004IEEE Computer Society Conference on Computer Vision and PatternRecognition,2004.CVPR 2004.:volume 2.IEEE,2004:II-II.》中采用基于特征点的位姿估计方法，该方法通过使用单目或多目相机进行图像内特征点的挖掘，并匹配不同图像间相似的特征点，根据特征点的运动估计目标的姿态变化。但是基于特征点的位姿估计方法极易受到光照与环境的影响，进而导致特征点跟踪丢失的情况，造成目标位姿估计质量下降。
文献《PAVLAKOS G,ZHOU X,CHAN A,et al.6-dof object pose from semantickeypoints[C]//2017IEEE international conference on robotics and automation(ICRA).IEEE,2017:2011-2018.》中采用基于模板的方法，通过构建待检测目标的二维或者三维模板，通过比对当前目标的实际位姿图像与模板的关系，寻找与实际位姿最接近的模板，估计出目标当前的位姿。但是这种方法需要在前期构建大量的二维、三维映射的模板匹配库，工作量庞大，结果精度依赖于模板匹配库的大小，并且一个模板库只适用于单一物体的位姿估计，无法满足通用性。
使用基于深度学习的方法，近年来深度学习的应用非常广泛，通过训练深度神经网络并构建损失函数以达到对图像中的物体进行位姿识别的效果。文献《REDMON J,DIVVALA S,GIRSHICK R,et al.You only look once:Unified,real-time objectdetection[C]//Proceedings of the IEEE conference on computer vision andpattern recognition.2016:779-788.》中采用YOLO网络，通过图像中目标二维边框恢复其位姿，但是基于深度学习的方法需要大量的样本资源来构建大量的样本数据并进行真实位姿的标注，并需要大量计算资源进行网络的训练，难以控制成本，且大多为针对性的开发，无法满足通用性。
发明内容
针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于正二十面体的单目6D位姿估计方法。
本发明解决所述技术问题的技术方案是，提供一种基于正二十面体的单目6D位姿估计方法，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正二十面体进行标记，得到ArUco码标记的正二十面体；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正二十面体；
第二步，分别对当前帧和初始帧中所有观测到的若干个ArUco码进行定位，再分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n和ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
第三步、对第二步得到的当前帧ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正二十面体内切球球心坐标作为当前帧正二十面体体心的空间坐标t0，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正二十面体内切球球心坐标作为初始帧正二十面体体心的空间坐标tO；再通过公式trel＝t0-tO求得当前帧正二十面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟正二十面体，虚拟正二十面体体心与相机坐标系原点重合，再使用ArUco码对虚拟正二十面体进行标记并得到虚拟正二十面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出当前帧正二十面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出初始帧正二十面体的旋转矩阵RO，再通过公式
Figure BDA0002889386430000021
计算得到当前帧正二十面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正二十面体相对于初始帧正二十面体的位姿矩阵Trel：
Figure BDA0002889386430000022
式(2)中，SE(3)表示矩阵的属性；
第四步、首先构建被测物体坐标系，将正二十面体固定于被测物体上，并计算正二十面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref，得到正二十面体相对于被测物体的位姿矩阵Tref：
Figure BDA0002889386430000031
则被测物体的相对位姿矩阵T：
Figure BDA0002889386430000032
得到当前帧被测物体相对于初始帧的空间姿态变化。
与现有技术相比，本发明有益效果在于：
(1)本方法创造性地将ArUco码与正二十面体相结合，将位姿识别从二维标签识别扩展为对三维空间体进行识别，大幅度提升了位姿识别的性能，再使用单目相机采集被ArUco码标记的正二十面体图像，利用PnP算法计算ArUco码的中心点的空间三维坐标，获得其内切球的稀疏点云，进而利用多元非线性拟合以及迭代最近点算法比对相机坐标处的虚拟正二十面体，将位置求解和旋转矩阵求解问题转化为优化问题，进而计算出当前正二十面体的空间位移量以及旋转角度，间接计算出被测物体的位姿。
(2)本方法克服了现有方法对光照、环境及设备的高要求，检测迅速，提升了稳定性，平均每帧图像识别时间约为10ms左右；不需要在前期投入大量的精力进行模板采集或模型训练，显著降低了位姿估计成本，保证了位姿估计精度并加强通用；使用了三维物体作为标志，能够进行空间内物体大幅度运动的位姿估计。
(3)本发明采用的正二十面体是空间面数最多的正多面体，它是本发明能够实现空间任意角度以及大幅度运动位姿识别的重要因素，与其他正多面体相比具有更高的精度。
(4)相较于基于特征点的方法，本发明采用单目相机识别ArUco码的方法，克服了其对光照、环境等因素的敏感程度，可以在多数环境下稳定运行。
(5)相较于基于模板的方法，本发明使用正二十面体间接测量目标位姿，通用性较强，并且不需要构建大量的模板库，节省内存空间与工作量。
(6)相较于基于深度学习的方法，本发明算法不需要相对较强的硬件性能，节省成本，满足通用性。
附图说明
图1为本发明的ArUco码标记的正二十面体的示意图；
图2为本发明的正二十面体及其内切球的示意图；
图3为本发明的虚拟正二十面体与ArUco码标记的实际正二十面体的位置关系示意图。
具体实施方式
下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。
本发明提供了一种基于正二十面体的单目6D位姿估计方法(简称方法)，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正二十面体进行标记，得到ArUco码标记的正二十面体(如图1所示)；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正二十面体；
优选地，第一步中，使用ArUco码对正二十面体进行标记的过程为：使用OpenCV图像处理库生成20个ArUco码，将20个ArUco码按编码顺序分别贴附于正二十面体的20个面上，且ArUco码的几何中心与其贴附的面的几何中点重合。
优选地，第一步中，单目相机的标定使用棋盘网格标准标定板，并使用OpenCV图像处理库进行相机内部参数标定，生成相机内参矩阵K；
所述相机内参矩阵K为将空间内坐标转化为平面坐标的矩阵，标准化公式为：
Figure BDA0002889386430000041
式(1)中，fx、fy为相机的焦距参数；cx、cy为像素的平移量。
第二步，使用图像处理算法分别对当前帧和初始帧中所有可观测到的若干个ArUco码进行定位，再使用图像处理算法分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n以及使用位姿计算方法获得当前帧和初始帧中所有观测到的ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
当前帧是正二十面体当前所在位置的图像，初始帧是单目相机拍摄的正二十面体初始位置的图像；
优选的，第二步中，ArUco码的定位过程是：对单目相机采集到的图像依次进行灰度处理、中值滤波及自适应阈值分割，从分割后的图像中提取符合要求的候选区域。
优选的，第二步中，ArUco码的编码的获得：首先对候选区域应用透视变换获得规范正方形标记，分离黑白色位，并根据标记大小分为不同的单元格；再根据每一个单元格上数量最多的像素所对应的颜色来确定该单元格的颜色，最后根据颜色转化为二进制数值以确定该标记的编码。
优选的，第二步中，ArUco码中心点的空间坐标的获得：根据候选区域的四个角点信息求出ArUco码中心点的空间坐标，使用OpenCV框架中的ArUco库函数的位姿计算函数，将四个角点信息作为输入，使用PnP方式(即2D-3D匹配算法)即可得到ArUco码中心点在相机坐标系下的空间坐标。
第三步、对第二步得到的当前帧所有ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正二十面体内切球球心坐标作为当前帧正二十面体体心的空间坐标t0＝(x0,y0,z0)，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正二十面体内切球球心坐标作为初始帧正二十面体体心的空间坐标tO＝(xO,yO,zO)；再用当前帧正二十面体体心的空间坐标t0＝(x0,y0,z0)减去初始帧正二十面体体心的空间坐标tO＝(xO,yO,zO)，通过公式trel(x,y,z)＝t0(x0,y0,z0)-tO(xO,yO,zO)求得当前帧正二十面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟状态的正二十面体，其与实际的正二十面体的位置关系如图3所示，虚拟正二十面体体心与相机坐标系原点Oc重合，再使用ArUco码对虚拟正二十面体进行标记并得到虚拟正二十面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码Ai(xi,yi,zi,idi),i＝1,2,3,...,n与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出当前帧正二十面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出初始帧正二十面体的旋转矩阵RO，再通过公式
Figure BDA0002889386430000052
计算得到当前帧正二十面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正二十面体相对于初始帧正二十面体的位姿矩阵Trel：
Figure BDA0002889386430000051
式(2)中，SE(3)表示李群，是一种数学符号表达，表示矩阵的属性；
优选地，第三步中，当前帧正二十面体体心的空间坐标的获得：由于ArUco码的几何中心与其贴附的面的几何中点重合，而正二十面体的内切球与正二十面体的交点正好位于正二十面体每个面的几何中心处(如图2所示)，所以能够将第二步得到的单目相机在当前帧观测到的正二十面体部分面上的每个ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n视为当前帧正二十面体内切球球面点(xi,yi,zi)的稀疏点云坐标；当单目相机获得了大于三个ArUco码中心点的空间坐标后即满足多元非线性拟合的数据条件(一般情况下，单目相机每次观测可以获得6-8个ArUco码中心点坐标，即每次观测均能够满足条件)，将当前帧正二十面体内切球球面点(xi,yi,zi)与内切球半径视为参数，则内切球的标准方程为：
(xi-x0)2+(yi-y0)2+(zi-z0)2-R内 2＝0 (3)
式(3)中，R内为正二十面体内切球半径，
Figure BDA0002889386430000061
a为正二十面体的棱长；
利用非线性最小二乘，对式(3)进行该球面多元非线性函数的球心拟合，构建损失函数如下：
Figure BDA0002889386430000062
由于多元非线性拟合对初始点要求较高，极容易陷入局部极小值中，因此利用已知球面点坐标(xi,yi,zi)对内切球球心坐标生成一组多元非线性拟合的初始值R初：
Figure BDA0002889386430000063
再将R初代入式(4)中获得损失函数J的一个值，随后不断更改R初的值使得式(4)中损失函数J的值不断减小，经过多次迭代求解后可以获得一组使得损失函数J最小的(x0,y0,z0)即为当前帧内切球球心坐标(即当前帧正二十面体体心在相机坐标系下的空间坐标t0＝(x0,y0,z0))。
优选地，第三步中，初始帧正二十面体体心的空间坐标的获得：将第二步得到的单目相机在初始帧观测到的正二十面体部分面上的每个ArUco码中心点的空间坐标视为初始帧正二十面体内切球球面点的稀疏点云坐标，与当前帧正二十面体体心的空间坐标的获得同理，即可得到初始帧正二十面体体心在相机坐标系下的空间坐标tO＝(xO,yO,zO)。
优选地，第三步中，当前帧正二十面体的旋转矩阵的获得：
搜索AWi中与第二步得到的当前帧实际正二十面体的ArUco码的编码对应的虚拟正二十面体的ArUco码的编码，将编码相同的进行匹配，进而获得空间中的一组匹配点：
(AWi,Ai),i＝1,2,3,...,n (6)
该组匹配点由实际正二十面体上观测到的ArUco码中心点的空间坐标与虚拟正二十面体上与之对应的ArUco码中心点的空间坐标组成，通过式(7)分别将其去质心化(即将实际正二十面体上观测到的ArUco码中心点的空间坐标与虚拟正二十面体上与之对应的ArUco码中心点的空间坐标的质心均平移至原点处)：
Figure BDA0002889386430000064
得到的qi为实际正二十面体ArUco码中心点的空间坐标的去质心点，qi'为虚拟正二十面体ArUco码中心点的空间坐标的去质心点，进而得到qi和qi'的关系如下：
Figure BDA0002889386430000071
定义误差项：
Figure BDA0002889386430000072
根据式(9)求取使得误差项E最小的当前帧正二十面体的旋转矩阵R，式(9)中第一项
Figure BDA0002889386430000076
与优化目标无关，第二项中RTR＝I亦与优化目标无关，则误差项变为：
Figure BDA0002889386430000073
式(10)中，tr表示矩阵的迹；
为求解式(10)中的优化目标R，定义矩阵：
Figure BDA0002889386430000074
对式(11)中的W进行奇异值分解，得到：
W＝UΣVT (12)
式(12)中，Σ为W的特征值矩阵，U与V为对角矩阵，则当前帧正二十面体的旋转矩阵R为：
R＝UVT (13)。
优选地，第三步中，初始帧正二十面体的旋转矩阵的获得：搜索AWi中与第二步得到的初始帧实际正二十面体的ArUco码的编码对应的虚拟正二十面体的ArUco码的编码，将编码相同的进行匹配，与当前帧正二十面体的旋转矩阵的获得同理，即可获得初始帧正二十面体的旋转矩阵RO。
第四步、根据正二十面体与被测物体的空间相对位置关系间接计算出被测物体的6D位姿。
优选地，第四步具体是：首先构建被测物体坐标系，将正二十面体固定于被测物体上，并计算正二十面体与被测物体之间的相对位姿关系(即计算正二十面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref)，得到正二十面体相对于被测物体的位姿矩阵Tref：
Figure BDA0002889386430000075
则被测物体的相对位姿矩阵T：
Figure BDA0002889386430000081
即得到当前帧被测物体相对于初始帧的空间姿态变化。
所述相机标定方法、ArUco码的生成方法、图像处理算法、位姿计算方法是本领域公知方法。
本发明未述及之处适用于现有技术。

Claims (7)

1.一种基于正二十面体的单目6D位姿估计方法，其特征在于，该方法包括以下步骤：
第一步、使用ArUco码对正二十面体进行标记，得到ArUco码标记的正二十面体；标定单目相机，再通过标定后的单目相机观测ArUco码标记的正二十面体；
第二步，分别对当前帧和初始帧中所有观测到的若干个ArUco码进行定位，再分别获得当前帧和初始帧中所有观测到的ArUco码的编码idi,i＝1,2,3,...,n和ArUco码中心点的空间坐标Ai(xi,yi,zi),i＝1,2,3,...,n，n表示某一帧观测到的ArUco码的数量；
第三步、对第二步得到的当前帧ArUco码中心点的空间坐标进行多元非线性拟合计算出当前帧正二十面体内切球球心坐标作为当前帧正二十面体体心的空间坐标t0，对初始帧ArUco码中心点的空间坐标进行多元非线性拟合计算出初始帧正二十面体内切球球心坐标作为初始帧正二十面体体心的空间坐标tO；再通过公式trel＝t0-tO求得当前帧正二十面体体心相对于初始帧的相对空间坐标trel；
在相机坐标系原点处构建一个虚拟正二十面体，虚拟正二十面体体心与相机坐标系原点重合，再使用ArUco码对虚拟正二十面体进行标记并得到虚拟正二十面体所有ArUco码的编码及其中心点的空间坐标AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20；再将第二步得到的当前帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出当前帧正二十面体的旋转矩阵R，将初始帧ArUco码中心点的空间坐标及其编码与AWi(xWi,yWi,zWi,idWi),i＝1,2,3,...,20通过迭代最近点方法计算出初始帧正二十面体的旋转矩阵RO，再通过公式
Figure FDA0003413263500000012
计算得到当前帧正二十面体相对于初始帧的相对旋转矩阵Rrel；
再根据相对旋转矩阵Rrel和相对空间坐标trel，得到当前帧正二十面体相对于初始帧正二十面体的位姿矩阵Trel：
Figure FDA0003413263500000011
式(2)中，SE(3)表示矩阵的属性；
第三步中，当前帧正二十面体的旋转矩阵的获得：
搜索AWi中与第二步得到的当前帧实际正二十面体的ArUco码的编码对应的虚拟正二十面体的ArUco码的编码，将编码相同的进行匹配，进而获得空间中的一组匹配点：
(AWi,Ai),i＝1,2,3,...,n (6)
该组匹配点由实际正二十面体上观测到的ArUco码中心点的空间坐标与虚拟正二十面体上与之对应的ArUco码中心点的空间坐标组成，通过式(7)分别将其去质心化：
Figure FDA0003413263500000021
得到的qi为实际正二十面体ArUco码中心点的空间坐标的去质心点，qi'为虚拟正二十面体ArUco码中心点的空间坐标的去质心点，进而得到qi和qi'的关系如下：
Figure FDA0003413263500000022
定义误差项：
Figure FDA0003413263500000023
根据式(9)求取使得误差项E最小的当前帧正二十面体的旋转矩阵R，式(9)中第一项
Figure FDA0003413263500000024
与优化目标无关，第二项中RTR＝I亦与优化目标无关，则误差项变为：
Figure FDA0003413263500000025
式(10)中，tr表示矩阵的迹；
为求解式(10)中的优化目标R，定义矩阵：
Figure FDA0003413263500000026
对式(11)中的W进行奇异值分解，得到：
W＝UΣVT (12)
式(12)中，Σ为W的特征值矩阵，U与V为对角矩阵，则当前帧正二十面体的旋转矩阵R为：
R＝UVT (13)
第四步、首先构建被测物体坐标系，将正二十面体固定于被测物体上，并计算正二十面体相对于被测物体的相对空间坐标tref和相对旋转矩阵Rref，得到正二十面体相对于被测物体的位姿矩阵Tref：
Figure FDA0003413263500000027
则被测物体的相对位姿矩阵T：
Figure FDA0003413263500000031
得到当前帧被测物体相对于初始帧的空间姿态变化。
2.根据权利要求1所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第一步中，使用ArUco码对正二十面体进行标记的过程为：使用OpenCV图像处理库生成20个ArUco码，将20个ArUco码按编码顺序分别贴附于正二十面体的20个面上，且ArUco码的几何中心与其贴附的面的几何中心重合。
3.根据权利要求1所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第一步中，单目相机的标定使用棋盘网格标准标定板，并使用OpenCV图像处理库进行相机内部参数标定，生成相机内参矩阵K；
所述相机内参矩阵K为将空间内坐标转化为平面坐标的矩阵，标准化公式为：
Figure FDA0003413263500000032
式(1)中，fx、fy为相机的焦距参数；cx、cy为像素的平移量。
4.根据权利要求1所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码的定位过程是：对单目相机采集到的图像依次进行灰度处理、中值滤波及自适应阈值分割，从分割后的图像中提取符合要求的候选区域。
5.根据权利要求4所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码的编码的获得：首先对候选区域应用透视变换获得规范正方形标记，分离黑白色位，并根据标记大小分为不同的单元格；再根据每一个单元格上数量最多的像素所对应的颜色来确定该单元格的颜色，最后根据颜色转化为二进制数值以确定该标记的编码。
6.根据权利要求4所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第二步中，ArUco码中心点的空间坐标的获得：根据候选区域的四个角点信息求出ArUco码中心点的空间坐标，使用OpenCV框架中的ArUco库函数的位姿计算函数，将四个角点信息作为输入，使用PnP方式即可得到ArUco码中心点在相机坐标系下的空间坐标。
7.根据权利要求1所述的基于正二十面体的单目6D位姿估计方法，其特征在于，第三步中，当前帧正二十面体体心的空间坐标的获得：将第二步得到的当前帧观测到的正二十面体部分面上的ArUco码中心点的空间坐标视为当前帧正二十面体内切球球面点(xi,yi,zi)的稀疏点云坐标；当单目相机获得了大于三个ArUco码中心点的空间坐标后，将当前帧正二十面体内切球球面点(xi,yi,zi)与内切球半径视为参数，则内切球的标准方程为：
(xi-x0)2+(yi-y0)2+(zi-z0)2-R内 2＝0 (3)
式(3)中，R内为正二十面体内切球半径，
Figure FDA0003413263500000041
a为正二十面体的棱长；
利用非线性最小二乘，对式(3)进行该球面多元非线性函数的球心拟合，构建损失函数如下：
Figure FDA0003413263500000042
由于多元非线性拟合对初始点要求较高，极容易陷入局部极小值中，因此利用已知球面点坐标(xi,yi,zi)对内切球球心坐标生成一组多元非线性拟合的初始值R初：
Figure FDA0003413263500000043
再将R初代入式(4)中获得损失函数J的一个值，随后不断更改R初的值使得式(4)中损失函数J的值不断减小，经过多次迭代求解后可以获得一组使得损失函数J最小的(x0,y0,z0)即为当前帧内切球球心坐标。
CN202110023412.7A 2021-01-08 2021-01-08 一种基于正二十面体的单目6d位姿估计方法 Active CN112857215B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110023412.7A CN112857215B (zh) 2021-01-08 2021-01-08 一种基于正二十面体的单目6d位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110023412.7A CN112857215B (zh) 2021-01-08 2021-01-08 一种基于正二十面体的单目6d位姿估计方法

Publications (2)

Publication Number Publication Date
CN112857215A CN112857215A (zh) 2021-05-28
CN112857215B true CN112857215B (zh) 2022-02-08

Family

ID=76005415

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110023412.7A Active CN112857215B (zh) 2021-01-08 2021-01-08 一种基于正二十面体的单目6d位姿估计方法

Country Status (1)

Country Link
CN (1) CN112857215B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115690607B (zh) * 2023-01-04 2023-04-11 西湖大学 基于红外遥感和深度学习的降雨反演方法及装置
CN117351091A (zh) * 2023-09-14 2024-01-05 成都飞机工业（集团）有限责任公司 一种相机阵列标定装置及其使用方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457640A (zh) * 2014-09-05 2015-03-25 合肥工业大学 用于关节类坐标测量机标定的常见虚拟几何特征标准件
JP2016058043A (ja) * 2014-09-12 2016-04-21 キヤノン株式会社 情報処理装置、情報処理方法、プログラム
JP2018032111A (ja) * 2016-08-22 2018-03-01 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置、情報処理システム、操作用オブジェクト、および情報処理方法
CN108491880A (zh) * 2018-03-23 2018-09-04 西安电子科技大学 基于神经网络的物体分类和位姿估计方法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN110390694A (zh) * 2019-07-19 2019-10-29 中兵勘察设计研究院有限公司 一种用于物品三维重建的摄影定位方法
CN110827359A (zh) * 2019-10-29 2020-02-21 武汉大学 基于棋盘格三面体的相机与激光器外参检校方法及装置
CN111179347A (zh) * 2019-12-26 2020-05-19 广东星舆科技有限公司 基于区域性特征的定位方法、定位设备及存储介质
TWI708591B (zh) * 2019-12-06 2020-11-01 財團法人金屬工業研究發展中心 骨科手術之三維即時定位方法
CN111899301A (zh) * 2020-06-02 2020-11-06 广州中国科学院先进技术研究所 一种基于深度学习的工件6d位姿估计方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9418480B2 (en) * 2012-10-02 2016-08-16 Augmented Reailty Lab LLC Systems and methods for 3D pose estimation

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104457640A (zh) * 2014-09-05 2015-03-25 合肥工业大学 用于关节类坐标测量机标定的常见虚拟几何特征标准件
JP2016058043A (ja) * 2014-09-12 2016-04-21 キヤノン株式会社 情報処理装置、情報処理方法、プログラム
JP2018032111A (ja) * 2016-08-22 2018-03-01 株式会社ソニー・インタラクティブエンタテインメント 情報処理装置、情報処理システム、操作用オブジェクト、および情報処理方法
CN108491880A (zh) * 2018-03-23 2018-09-04 西安电子科技大学 基于神经网络的物体分类和位姿估计方法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN110390694A (zh) * 2019-07-19 2019-10-29 中兵勘察设计研究院有限公司 一种用于物品三维重建的摄影定位方法
CN110827359A (zh) * 2019-10-29 2020-02-21 武汉大学 基于棋盘格三面体的相机与激光器外参检校方法及装置
TWI708591B (zh) * 2019-12-06 2020-11-01 財團法人金屬工業研究發展中心 骨科手術之三維即時定位方法
CN111179347A (zh) * 2019-12-26 2020-05-19 广东星舆科技有限公司 基于区域性特征的定位方法、定位设备及存储介质
CN111899301A (zh) * 2020-06-02 2020-11-06 广州中国科学院先进技术研究所 一种基于深度学习的工件6d位姿估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Stereo-based real-time 6-DoF work tool tracking for robot programing by demonstration;Marcos Ferreira等;《Int J Adv Manuf Technol》;20140613;第57–69页 *
基于三维标识物的虚实配准方法研究;卢韶芳等;《吉林大学学报( 信息科学版)》;20170531;第35卷(第3期);第288-295页 *

Also Published As

Publication number Publication date
CN112857215A (zh) 2021-05-28

Similar Documents

Publication Publication Date Title
CN108549873B (zh) 三维人脸识别方法和三维人脸识别***
CN111775152B (zh) 基于三维测量引导机械臂抓取散乱堆叠工件的方法及***
CN109344882B (zh) 基于卷积神经网络的机器人控制目标位姿识别方法
CN112734844B (zh) 一种基于正八面体的单目6d位姿估计方法
CN107481284A (zh) 目标物跟踪轨迹精度测量的方法、装置、终端及***
CN112857215B (zh) 一种基于正二十面体的单目6d位姿估计方法
Duncan et al. Multi-scale superquadric fitting for efficient shape and pose recovery of unknown objects
CN107705322A (zh) 运动目标识别跟踪方法和***
Lin et al. Robotic grasping with multi-view image acquisition and model-based pose estimation
CN111476841B (zh) 一种基于点云和图像的识别定位方法及***
CN108171102A (zh) 一种基于视觉的零件快速识别方法
Lambrecht et al. Towards the usage of synthetic data for marker-less pose estimation of articulated robots in rgb images
Wunsch et al. Real-Time pose estimation of 3D objects from camera images using neural networks
Zheng et al. Industrial part localization and grasping using a robotic arm guided by 2D monocular vision
Ma et al. Binocular vision object positioning method for robots based on coarse-fine stereo matching
Sun et al. Robotic grasping using semantic segmentation and primitive geometric model based 3D pose estimation
CN112734843B (zh) 一种基于正十二面体的单目6d位姿估计方法
CN108090476A (zh) 一种针对具有外部遮挡的3d人脸识别方法
Sun et al. Automatic targetless calibration for LiDAR and camera based on instance segmentation
CN113256789A (zh) 一种三维实时人体姿态重建方法
Liu et al. Robust 3-d object recognition via view-specific constraint
CN108694348B (zh) 一种基于自然特征的跟踪注册方法及装置
Li et al. Online workpieces recognition for the robotic spray-painting production line with a low-cost RGB-D camera
CN115760984A (zh) 一种立方星基于单目视觉的非合作目标位姿测量方法
Wu et al. Object Pose Estimation with Point Cloud Data for Robot Grasping

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