CN111179342B - 物体位姿的估计方法、装置、存储介质和机器人 - Google Patents

物体位姿的估计方法、装置、存储介质和机器人 Download PDF

Info

Publication number
CN111179342B
CN111179342B CN201911268545.XA CN201911268545A CN111179342B CN 111179342 B CN111179342 B CN 111179342B CN 201911268545 A CN201911268545 A CN 201911268545A CN 111179342 B CN111179342 B CN 111179342B
Authority
CN
China
Prior art keywords
template
image
coordinates
coordinate system
queried
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
CN201911268545.XA
Other languages
English (en)
Other versions
CN111179342A (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.)
Flexiv Robotics Ltd
Original Assignee
Flexiv Robotics 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 Flexiv Robotics Ltd filed Critical Flexiv Robotics Ltd
Priority to CN201911268545.XA priority Critical patent/CN111179342B/zh
Publication of CN111179342A publication Critical patent/CN111179342A/zh
Application granted granted Critical
Publication of CN111179342B publication Critical patent/CN111179342B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/50—Information retrieval; Database structures therefor; File system structures therefor of still image data
    • G06F16/58—Retrieval characterised by using metadata, e.g. metadata not derived from the content or metadata generated manually
    • 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
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D10/00—Energy efficient computing, e.g. low power processors, power management or thermal management

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Library & Information Science (AREA)
  • Data Mining & Analysis (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)

Abstract

本申请涉及一种物体位姿的估计方法、装置、存储介质和机器人，所述方法包括：获取特征模板，特征模板包括与待查询物体对应的模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标和特征向量；获取包括待查询物体的查询图像，提取查询图像中待查询物体的多个特征点，并获取多个特征点在查询图像的图像坐标系下的2D坐标和特征向量；将多个特征点与多个参考特征点的特征向量进行匹配，根据相匹配的参考特征点在模板物体的物体坐标系下的3D坐标得到多个特征点在待查询物体的物体坐标系下的3D坐标；根据多个特征点在查询图像的图像坐标系下的2D坐标以及在待查询物体的物体坐标系下的3D坐标确定待查询物体的位姿。本申请提升了位姿的检测精度。

Description

物体位姿的估计方法、装置、存储介质和机器人
技术领域
本申请涉及图像识别技术领域，特别是涉及一种物体位姿的估计方法、装置、存储介质和机器人。
背景技术
位姿估计主要用于检测图像中的目标物体在世界坐标系中的位置和姿态。位姿估计对机器人、增强现实、自动驾驶等技术至关重要。比如，当机器人执行抓取操作时，只有精准地检测到目标物体的位姿，才可以利用目标物体的位姿规划自身的运动路径，并调整机械臂姿态来移动目标物体。但是，在实际应用中，传统的物体位姿的估计方法存在检测精度低的问题。
发明内容
基于此，有必要针对传统的物体位姿的估计方法检测精度低的问题，提供一种物体位姿的估计方法、装置、存储介质和机器人。
一种物体位姿的估计方法，所述方法包括：
获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；以及
根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
在其中一个实施例中，所述获取特征模板的步骤，包括：
获取包括所述模板物体的至少一个模板图像；
获取所述模板图像中的所述模板物体的参考位姿；
提取所述模板图像中所述模板物体的多个参考特征点，并获取所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标；
根据所述模板图像中的所述模板物体的所述参考位姿以及所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标，确定所述多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量。
在其中一个实施例中，所述获取所述模板图像中的所述模板物体的参考位姿的步骤，包括：
获取所述模板图像中的所述模板物体的多个参考关键点，并获取所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标；
根据所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标，确定所述模板图像中的所述模板物体的参考位姿。
在其中一个实施例中，所述模板图像为多个，且所述多个模板图像包括不同视角的所述模板物体。
在其中一个实施例中，在所述根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿的步骤之后，还包括：
获取所述查询图像中所述待查询物体的多个关键点，并获取所述多个关键点在所述待查询物体的物体坐标系下的3D坐标；
根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿。
在其中一个实施例中，所述根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿的步骤，包括：
根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述多个关键点在所述查询图像的图像坐标系下的2D坐标；
对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正；
根据纠正后的所述多个关键点在所述查询图像的图像坐标系下的2D坐标以及所述多个关键点在所述待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
在其中一个实施例中，所述对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正，包括：
根据关键点跟踪算法对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正。
一种物体位姿的估计装置，所述装置包括：
获取模块，用于获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
所述获取模块，还用于获取包括待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
匹配模块，用于将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；
确定模块，用于根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
一种存储介质，所述存储介质上存储有计算机可执行指令，所述计算机可执行指令被处理器执行以下物体位姿的估计方法的步骤：
获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；以及
根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
一种机器人，包括存储器、处理器和摄像设备，所述存储器中储存有计算机程序，所述计算机可读指令被所述处理器执行以下物体位姿的估计方法的步骤：
获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；以及
根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
上述物体位姿的估计方法、装置、存储介质和机器人，获取特征模板，特征模板包括与待查询物体对应的模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标和特征向量，获取包括待查询物体的查询图像，提取查询图像中待查询物体的多个特征点，并获取多个特征点在查询图像的图像坐标系下的2D坐标和特征向量，将多个特征点的特征向量与多个参考特征点的特征向量进行匹配，根据多个特征点相匹配的参考特征点在模板物体的物体坐标系下的3D坐标得到多个特征点在待查询物体的物体坐标系下的3D坐标，根据多个特征点在查询图像的图像坐标系下的2D坐标以及在待查询物体的物体坐标系下的3D坐标确定待查询物体的位姿。由于特征模板直接携带有模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标，提升了查询图像与特征模板匹配成功的概率，进而提升了位姿检测的精准度。
附图说明
图1为一个实施例中物体位姿的估计方法的应用环境图；
图2为一个实施例中物体位姿的估计方法的流程示意图；
图3为一个实施例中特征模板的示意图；
图4为另一个实施例中物体位姿的估计方法的流程示意图；
图5为一个实施例中物体位姿的估计装置的结构框图；
图6为一个实施例中机器人的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
图1为一个实施例中物体位姿的估计方法的应用环境图。参照图1，该物体位姿的估计方法应用于机器人100，该机器人100可包括基座102、操作臂104、末端执行器106和摄像头108。具体地，基座102用于机器人100的固定；所述操作臂104的数量可以根据需要确定，以在空间实现多自由度的运动；所述末端执行器106用于直接抓放物体或进行其它操作(例如，机加工)；所述摄像头108用于采集包含物体的查询图像。其中，摄像头108可安装在机器人100上，比如末端执行器106上，也可安装在机器人100的外部。
如图2所示，在一个实施例中，提供了一种物体位姿的估计方法。本实施例主要以该方法应用于上述图1中的机器人100来举例说明。参照图2，该物体位姿的估计方法具体包括如下步骤：
S202，获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量。
其中，特征模板可为由多个模板图像建立的数据集合。模板图像是指预先建立的包含有模板物体的图像。模板物体可以是与待查询物体相对应的物体，通常可选择与待查询物体相同的物体作为模板物体，或者可选择与待查询物体等比例放大或缩小的物体作为模板物体。特征模板可包括以下信息：模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标和特征向量。用于建立特征模板的每个模板图像的拍摄视角不同，即多个模板图像中的模板物体具有多个位姿，可呈现该模板物体的多个视角的结构。图3示出了模板图像的两个示例，如图3上图所示，模板图像为包含有模板物体的图像，如图3下图所示，机器人100可在每个模板图像上筛选出若干参考特征点，多个视角的模板图像使得特征模板包含丰富的特征点，使得查询图像与特征模板匹配时不受图像视角限制，提升了查询图像与特征模板匹配成功的概率。
参考特征点是指模板图像中模板物体的特征点，特征点是指图像中具备特征不变性的像素点，特征向量是指描述模板图像的特征点的数据结构。可通过特征提取算法获取模板图像中待查询物体的多个参考特征点以及特征向量，比如HOG(Histogram ofOriented Gradien，方向梯度直方图)、SIFT(Scale-invariant feature transform，尺度不变特征变换)、SURF(Speeded Up Robust Features,加速稳健特征)、LBP(Local BinaryPattern，局部二值模式)、ORB(Oriented FAST and Rotated BRIEF)、Haar-like等。
物体坐标系为3D坐标系，其与物体相关联，例如，当待查询物体或者模板物体发生移动或者旋转，其对应的待查询物体或模板物体的物体坐标系发生相同的移动或旋转。本申请还涉及图像坐标系和世界坐标系。图像坐标系为2D坐标系，其基于摄像头拍摄的图像建立。世界坐标系为3D坐标系，通常情况下，机器人坐标系和/或摄像头坐标系相对世界坐标系的关系是已知或事先建立的，因此某点或某矢量在这些坐标系下的坐标之间可以互相转换。
位姿用于描述物体在空间坐标系中的位置和姿态。在空间坐标系(O,X,Y,Z)中，物体的位置通过x、y、z坐标进行表征，x、y、z坐标是指附着于物体上的坐标系相对于空间坐标系的坐标；物体的姿态通过物体所在的位置与OX轴的夹角rx、物体所在的位置与OY轴的夹角ry、物体所在的位置与OZ的夹角rz进行表征。例如，在机器人运动控制中，若需要抓取某物体或者对某物体进行加工，可采集该物体的图像，从而确定该物体在世界坐标系或机器人坐标系下的位姿，从而可以准确的控制末端执行器向该物体运动并对该物体进行抓取或加工。
S204，获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量。
其中，待查询物体是指机器人执行操作的对象；查询图像是指，机器人在执行操作时，由摄像头采集的包括待查询物体的图像。
可通过特征提取算法获取查询图像中待查询物体的多个特征点以及特征向量，比如HOG(Histogram of Oriented Gradien，方向梯度直方图)、SIFT(Scale-invariantfeature transform，尺度不变特征变换)、SURF(Speeded Up Robust Features,加速稳健特征)、LBP(Local Binary Pattern，局部二值模式)、ORB(Oriented FAST and RotatedBRIEF)、Haar-like等。
S206，将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标。
将多个特征点的特征向量与多个参考特征点的特征向量进行匹配的方式可以是：获取多个特征点的特征向量与多个参考特征点的特征向量之间的距离，将满足预设条件的点作为匹配点，其中，预设条件可以是距离最小。
特征点的特征向量与参考特征点的特征向量之间的距离的计算方式可由特征提取算法确定，比如，若采用SIFT算法，可根据欧式距离衡量特征点的特征向量与参考特征点的特征向量之间的距离，若采用HOG算法，可根据汉明距离衡量特征点的特征向量与参考特征点的特征向量之间的距离。
通过匹配，可以得到与特征点匹配的参考特征点，根据参考特征点在模板物体的物体坐标系下的3D坐标就可确定待查询物体的特征点在待查询物体的物体坐标系下的3D坐标。例如，当模板物体与待查询物体是相同的物体且建立物体坐标系的方式相同时，参考特征点在模板物体的物体坐标系下的3D坐标即为特征点在待查询物体的物体坐标系下的3D坐标。
S208，根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
可根据多个特征点在图像坐标系下的2D坐标以及在物体坐标系下的3D坐标，通过PnP(perspective-n-point)算法求解待查询物体的位姿。PnP(perspective-n-point)算法是指：根据一一对应的3D空间点与2D图像点的点对，计算物体的位姿。
PnP求解公式如下：
Pc＝KTPo
其中，Pc是物体的图像点的2D坐标；K是相机的内参矩阵，默认为已知项；T是位姿，其为矩阵表示；Po是物体的空间点的3D坐标。
将多个特征点在图像坐标系下的2D坐标代入Pc，多个特征点在物体坐标系下的3D坐标代入Po，得到K，即待查询物体的位姿。
本实施例提供的物体位姿的估计方法，由于特征模板直接携带有模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标，提升了查询图像与特征模板匹配成功的概率，进而提升了位姿检测的精准度。
在一个实施例中，所述获取特征模板的步骤，包括：获取包括所述模板物体的至少一个模板图像；获取所述模板图像中的所述模板物体的参考位姿；提取所述模板图像中所述模板物体的多个参考特征点，并获取所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标；根据所述模板图像中的所述模板物体的所述参考位姿以及所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标，确定所述多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量。
获取模板图像中待查询物体的参考位姿的方式，可以是：获取模板图像中的模板物体的多个参考关键点，并获取多个参考关键点在模板图像的图像坐标系下的2D坐标以及在模板物体的物体坐标系下的3D坐标；根据多个参考关键点在模板图像的图像坐标系下的2D坐标以及在模板物体的物体坐标系下的3D坐标，确定模板图像中的模板物体的参考位姿。在其他一些实施例中，模板图像中的模板物体的参考位姿也可以通过其他方式获取，例如通过位置确定的夹具/安装结构固定的模板物体的参考位姿可以由人工输入。
在知道参考位姿的情况下，就可以根据待查询物体上任意像素点在图像坐标系下的2D坐标，得到该像素点在物体坐标系下的3D坐标。
获取参考特征点在模板物体的物体坐标系下的3D坐标的方式，可以是：提取参考特征点，并根据参考位姿以及参考特征点在模板物体的物体坐标系下的2D坐标，通过PnP(perspective-n-point)算法求解参考特征点在模板物体的物体坐标系下的3D坐标。
PnP求解公式如下：
Po＝T-1K-1Pc
其中，Pc是物体的图像点的2D坐标；K是相机的内参矩阵，默认为已知项；T是位姿，其为矩阵表示；Po是物体的空间点的3D坐标。
将参考位姿代入K，参考特征点在模板图像的图像坐标系下的2D坐标代入Pc，得到Po，即参考特征点在模板物体的物体坐标系下的3D坐标。
本实施例提供的物体位姿的估计方法，针对每一个参考特征点，均可得到参考特征点在模板物体的物体坐标系下的3D坐标。
在一个实施例中，所述获取所述模板图像中的所述模板物体的参考位姿的步骤，包括：获取所述模板图像中的所述模板物体的多个参考关键点，并获取所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标；根据所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标，确定所述模板图像中的所述模板物体的参考位姿。
其中，参考关键点是指模板图像中待查询物体的关键点。关键点是指图像上具备语义信息的像素点，比如对于人脸图像来说，关键点可以是人脸图像中的双眼、鼻尖的像素点等。关键点的坐标可以由人工标记，用于确定各模板图像中物体的位姿；特征点为根据算法自动提取，其用于后续查询图像中的匹配。
首先，针对每一张模板图像，标记模板物体的参考关键点在模板图像的图像坐标系下的2D坐标，并对应在模板物体的3D模型上标记参考关键点在模板物体的物体坐标系下的3D坐标，得到模板图像中模板物体的参考关键点在模板物体的物体坐标系下的3D坐标。
然后，根据多组参考关键点在模板图像的图像坐标系下的2D坐标与参考关键点在模板物体的物体坐标系下的3D坐标之间的匹配关系，获取模板图像中模板物体的参考位姿。
可通过PnP(perspective-n-point)算法求解模板图像中物体的参考位姿。
PnP求解公式如下：
Pc＝KTPo
其中，Pc是物体的图像点的2D坐标；K是相机的内参矩阵，默认为已知项；T是位姿，其为矩阵表示；Po是物体的空间点的3D坐标。
将参考关键点在模板图像的图像坐标系下的2D坐标代入Pc，参考关键点在模板物体的物体坐标系下的3D坐标代入Po，得到K，即参考位姿。
本实施例提供的物体位姿的估计方法，根据参考关键点在模板图像的图像坐标系下的2D坐标以及在模板物体的物体坐标系下的3D坐标获取参考位姿，以用来计算参考特征点在模板物体的物体坐标系下的3D坐标。
在一个实施例中，在所述根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿的步骤之后，还包括：获取所述查询图像中所述待查询物体的多个关键点，并获取所述多个关键点在所述待查询物体的物体坐标系下的3D坐标；根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿。
获取待查询物体的优化位姿的方式，可以是：根据多个关键点在待查询物体的物体坐标系下的3D坐标以及待查询物体的位姿，确定多个关键点在查询图像的图像坐标系下的2D坐标；对多个关键点在查询图像的图像坐标系下的2D坐标进行纠正；根据纠正后的多个关键点在查询图像的图像坐标系下的2D坐标以及多个关键点在待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
本实施例提供的物体位姿的估计方法，对待查询物体的位姿进行优化，增强待查询物体的位姿的精准性。
在一个实施例中，所述根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿的步骤，包括：根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述多个关键点在所述查询图像的图像坐标系下的2D坐标；对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正；根据纠正后的所述多个关键点在所述查询图像的图像坐标系下的2D坐标以及所述多个关键点在所述待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
获取多个关键点在查询图像的图像坐标系下的2D坐标的方式，可以是：根据待查询物体的位姿以及关键点在物体坐标系下的3D坐标，通过PnP(perspective-n-point)算法获取关键点在图像坐标系下的2D坐标。
PnP求解公式如下：
Po＝T-1K-1Pc
其中，Pc是物体的图像点的2D坐标；K是相机的内参矩阵，默认为已知项；T是位姿，其为矩阵表示；Po是物体的空间点的3D坐标。
将待查询物体的位姿代入K，关键点在待查询物体的物体坐标系下的3D坐标代入Po，得到Pc，即关键点在查询图像的图像坐标系下的2D坐标。
对所述多个关键点在查询图像的图像坐标系下的2D坐标进行纠正的方式，可以是：根据关键点跟踪算法纠正多个关键点在查询图像的图像坐标系下的2D坐标。关键点跟踪算法可以是：KCF(Kernel Correlation Filter，核相关滤波)、DCF(DiscriminativeCorrelationFilter，判别相关滤波器)等。
根据纠正后的多个关键点在查询图像的图像坐标系下的2D坐标以及多个关键点在待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿的方式，可以是：根据纠正后的关键点在图像坐标系下的2D坐标以及在物体坐标系下的3D坐标，通过PnP(perspective-n-point)算法获取待查询物体的优化位姿。
PnP求解公式如下：
Pc＝KTPo
其中，Pc是物体的图像点的2D坐标；K是相机的内参矩阵，默认为已知项；T是位姿，其为矩阵表示；Po是物体的空间点的3D坐标。
将纠正后的关键点在查询图像的图像坐标系下的2D坐标代入Pc，关键点在待查询物体的物体坐标系下的3D坐标代入Po，得到K，即待查询物体的优化位姿。
本实施例提供的物体位姿的估计方法，通过纠正关键点在图像坐标系下的2D坐标，增强待查询物体的位姿的精准性。
如图4所示，在一个具体的实施例中，所述物体位姿的估计方法包括以下步骤：
S402，获取模板图像中的模板物体的多个参考关键点，并获取多个参考关键点在模板图像的图像坐标系下的2D坐标以及在模板物体的物体坐标系下的3D坐标，根据多个参考关键点在模板图像的图像坐标系下的2D坐标以及在模板物体的物体坐标系下的3D坐标，确定模板图像中的模板物体的参考位姿。
S404，获取包括模板物体的至少一个模板图像，提取模板图像中模板物体的多个参考特征点，并获取多个参考特征点在模板图像的图像坐标系下的2D坐标，根据模板图像中的模板物体的参考位姿以及多个参考特征点在模板图像的图像坐标系下的2D坐标，确定多个参考特征点在模板物体的物体坐标系下的3D坐标和特征向量。
S406，获取特征模板，特征模板包括与待查询物体对应的模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标和特征向量，获取包括待查询物体的查询图像，提取查询图像中待查询物体的多个特征点，并获取多个特征点在查询图像的图像坐标系下的2D坐标和特征向量，将多个特征点的特征向量与多个参考特征点的特征向量进行匹配，根据多个特征点相匹配的参考特征点在模板物体的物体坐标系下的3D坐标得到多个特征点在待查询物体的物体坐标系下的3D坐标，根据多个特征点在查询图像的图像坐标系下的2D坐标以及在待查询物体的物体坐标系下的3D坐标确定待查询物体的位姿。
S408，获取查询图像中待查询物体的多个关键点，并获取多个关键点在待查询物体的物体坐标系下的3D坐标，根据多个关键点在待查询物体的物体坐标系下的3D坐标以及待查询物体的位姿，确定多个关键点在查询图像的图像坐标系下的2D坐标，对多个关键点在查询图像的图像坐标系下的2D坐标进行纠正，根据纠正后的多个关键点在查询图像的图像坐标系下的2D坐标以及多个关键点在待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
本申请提供的物体位姿的估计方法，由于特征模板直接携带有模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标，提升了查询图像与特征模板匹配成功的概率，进而提升了位姿检测的精准度。
图2和图4为一个实施例中物体位姿的估计方法的流程示意图。应该理解的是，虽然图2和图4的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2和图4中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
如图5所示，在一个实施例中，提供了一种物体位姿的估计装置500，包括：获取模块502、匹配模块504和确定模块506。
获取模块502，用于获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
所述获取模块502，还用于获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
匹配模块504，用于将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；
确定模块506，用于根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
上述物体位姿的估计装置500，由于特征模板直接携带有模板物体的多个参考特征点在模板物体的物体坐标系下的3D坐标，提升了查询图像与特征模板匹配成功的概率，进而提升了位姿检测的精准度。
在一个实施例中，所述获取模块502，还用于：获取包括所述模板物体的至少一个模板图像；获取所述模板图像中的所述模板物体的参考位姿；提取所述模板图像中所述模板物体的多个参考特征点，并获取所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标；根据所述模板图像中的所述模板物体的所述参考位姿以及所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标，确定所述多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量。
在一个实施例中，所述获取模块502，还用于：获取所述模板图像中的所述模板物体的多个参考关键点，并获取所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标；根据所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标，确定所述模板图像中的所述模板物体的参考位姿。
在一个实施例中，所述获取模块502，还用于：获取所述查询图像中所述待查询物体的多个关键点，并获取所述多个关键点在所述待查询物体的物体坐标系下的3D坐标；根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿。
在一个实施例中，所述获取模块502，还用于：根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述多个关键点在所述查询图像的图像坐标系下的2D坐标；对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正；根据纠正后的所述多个关键点在所述查询图像的图像坐标系下的2D坐标以及所述多个关键点在所述待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
在一个实施例中，所述获取模块502，还用于：根据关键点跟踪算法对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正。
图6示出了一个实施例中机器人的内部结构图。该机器人具体可以是图1中的机器人100。如图6所示，该机器人包括通过系统总线连接的处理器、存储器和网络接口。其中，存储器包括非易失性存储介质和内存储器。该机器人的非易失性存储介质存储有操作系统，还可存储有计算机程序，该计算机程序被处理器执行时，可使得处理器实现物体位姿的估计方法。该内存储器中也可储存有计算机程序，该计算机程序被处理器执行时，可使得处理器执行物体位姿的估计方法。
本领域技术人员可以理解，图6中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的机器人的限定，具体的机器人可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，本申请提供的物体位姿的估计装置可以实现为一种计算机程序的形式，计算机程序可在如图6所示的机器人上运行。机器人的存储器中可存储组成该物体位姿的估计装置的各个程序模块，比如，图5所示的获取模块502、匹配模块504以及确定模块506。各个程序模块构成的计算机程序使得处理器执行本说明书中描述的本申请各个实施例的物体位姿的估计方法中的步骤。
在一个实施例中，提供了一种机器人，包括存储器、处理器和摄像设备，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行上述物体位姿的估计方法的步骤。此处物体位姿的估计方法的步骤可以是上述各个实施例的物体位姿的估计方法中的步骤。
在一个实施例中，提供了一种存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行上述物体位姿的估计方法的步骤。此处物体位姿的估计方法的步骤可以是上述各个实施例的物体位姿的估计方法中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Sync小时link)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种物体位姿的估计方法，其特征在于，所述方法包括：
获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；所述特征向量是指描述模板图像的特征点的数据结构；所述获取特征模板的步骤包括：获取包括所述模板物体的至少一个模板图像；获取所述模板图像中的所述模板物体的参考位姿；提取所述模板图像中所述模板物体的多个参考特征点，并获取所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标；根据所述模板图像中的所述模板物体的所述参考位姿以及所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标，确定所述多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；
获取包括所述待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；以及
根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
2.根据权利要求1所述的方法，其特征在于，所述获取所述模板图像中的所述模板物体的参考位姿的步骤，包括：
获取所述模板图像中的所述模板物体的多个参考关键点，并获取所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标；
根据所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标，确定所述模板图像中的所述模板物体的参考位姿。
3.根据权利要求1所述的方法，其特征在于，所述模板图像为多个，且多个模板图像包括不同视角的所述模板物体。
4.根据权利要求1所述的方法，其特征在于，在所述根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿的步骤之后，还包括：
获取所述查询图像中所述待查询物体的多个关键点，并获取所述多个关键点在所述待查询物体的物体坐标系下的3D坐标；
根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿。
5.根据权利要求4所述的方法，其特征在于，所述根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述待查询物体的优化位姿的步骤，包括：
根据所述多个关键点在所述待查询物体的物体坐标系下的3D坐标以及所述待查询物体的位姿，确定所述多个关键点在所述查询图像的图像坐标系下的2D坐标；
对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正；
根据纠正后的所述多个关键点在所述查询图像的图像坐标系下的2D坐标以及所述多个关键点在所述待查询物体的物体坐标系下的3D坐标，确定待查询物体的优化位姿。
6.根据权利要求5所述的方法，其特征在于，所述对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正，包括：
根据关键点跟踪算法对所述多个关键点在所述查询图像的图像坐标系下的2D坐标进行纠正。
7.一种物体位姿的估计装置，其特征在于，所述装置包括：
获取模块，用于获取特征模板，所述特征模板包括与待查询物体对应的模板物体的多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；所述特征向量是指描述模板图像的特征点的数据结构；所述获取模块还用于：获取包括所述模板物体的至少一个模板图像；获取所述模板图像中的所述模板物体的参考位姿；提取所述模板图像中所述模板物体的多个参考特征点，并获取所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标；根据所述模板图像中的所述模板物体的所述参考位姿以及所述多个参考特征点在所述模板图像的图像坐标系下的2D坐标，确定所述多个参考特征点在所述模板物体的物体坐标系下的3D坐标和特征向量；所述获取模块还用于获取包括待查询物体的查询图像，提取所述查询图像中所述待查询物体的多个特征点，并获取所述多个特征点在所述查询图像的图像坐标系下的2D坐标和特征向量；
匹配模块，用于将所述多个特征点的特征向量与所述多个参考特征点的特征向量进行匹配，根据所述多个特征点相匹配的所述参考特征点在所述模板物体的物体坐标系下的3D坐标得到所述多个特征点在所述待查询物体的物体坐标系下的3D坐标；
确定模块，用于根据所述多个特征点在所述查询图像的图像坐标系下的2D坐标以及在所述待查询物体的物体坐标系下的3D坐标确定所述待查询物体的位姿。
8.根据权利要求7所述的装置，其特征在于，所述获取模块还用于：获取所述模板图像中的所述模板物体的多个参考关键点，并获取所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标；根据所述多个参考关键点在所述模板图像的图像坐标系下的2D坐标以及在所述模板物体的物体坐标系下的3D坐标，确定所述模板图像中的所述模板物体的参考位姿。
9.一种机器人，包括存储器、处理器和摄像设备，所述存储器中储存有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1至6中任一项所述的方法的步骤。
10.一种存储介质，其特征在于，所述存储介质上存储有计算机可执行指令，所述计算机可执行指令被处理器执行时，使得所述处理器执行权利要求1至6中任一项所述方法的步骤。
CN201911268545.XA 2019-12-11 2019-12-11 物体位姿的估计方法、装置、存储介质和机器人 Active CN111179342B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911268545.XA CN111179342B (zh) 2019-12-11 2019-12-11 物体位姿的估计方法、装置、存储介质和机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911268545.XA CN111179342B (zh) 2019-12-11 2019-12-11 物体位姿的估计方法、装置、存储介质和机器人

Publications (2)

Publication Number Publication Date
CN111179342A CN111179342A (zh) 2020-05-19
CN111179342B true CN111179342B (zh) 2023-11-17

Family

ID=70646332

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911268545.XA Active CN111179342B (zh) 2019-12-11 2019-12-11 物体位姿的估计方法、装置、存储介质和机器人

Country Status (1)

Country Link
CN (1) CN111179342B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112150551B (zh) * 2020-09-25 2023-07-25 北京百度网讯科技有限公司 物体位姿的获取方法、装置和电子设备
CN112894815B (zh) * 2021-01-25 2022-09-27 西安工业大学 视觉伺服机械臂物品抓取最佳位姿检测方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108053491A (zh) * 2017-12-12 2018-05-18 重庆邮电大学 动态视角条件下实现平面目标三维跟踪和增强现实的方法
CN109255749A (zh) * 2017-07-12 2019-01-22 来福机器人 自主和非自主平台中的地图构建优化
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN109887030A (zh) * 2019-01-23 2019-06-14 浙江大学 基于cad稀疏模板的无纹理金属零件图像位姿检测方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109214980B (zh) * 2017-07-04 2023-06-23 阿波罗智能技术(北京)有限公司 一种三维姿态估计方法、装置、设备和计算机存储介质

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109255749A (zh) * 2017-07-12 2019-01-22 来福机器人 自主和非自主平台中的地图构建优化
CN108053491A (zh) * 2017-12-12 2018-05-18 重庆邮电大学 动态视角条件下实现平面目标三维跟踪和增强现实的方法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN109887030A (zh) * 2019-01-23 2019-06-14 浙江大学 基于cad稀疏模板的无纹理金属零件图像位姿检测方法

Also Published As

Publication number Publication date
CN111179342A (zh) 2020-05-19

Similar Documents

Publication Publication Date Title
CN110411441B (zh) 用于多模态映射和定位的系统和方法
WO2019042426A1 (zh) 增强现实场景的处理方法、设备及计算机存储介质
CN111160302B (zh) 基于自动驾驶环境的障碍物信息识别方法和装置
Li et al. DeepI2P: Image-to-point cloud registration via deep classification
CN108665508B (zh) 一种即时定位与地图构建方法、装置及存储介质
CN109285190B (zh) 对象定位方法、装置、电子设备和存储介质
US11205276B2 (en) Object tracking method, object tracking device, electronic device and storage medium
US9418313B2 (en) Method for searching for a similar image in an image database based on a reference image
CN111512317A (zh) 一种多目标实时跟踪方法、装置及电子设备
CN108038139B (zh) 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质
JP4709668B2 (ja) ３次元物体認識システム
CN111179342B (zh) 物体位姿的估计方法、装置、存储介质和机器人
US11625842B2 (en) Image processing apparatus and image processing method
Zhao et al. Visual odometry-A review of approaches
JP2778430B2 (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN114415698B (zh) 机器人、机器人的定位方法、装置和计算机设备
CN110880003A (zh) 一种图像匹配方法、装置、存储介质及汽车
CN116051873A (zh) 关键点匹配方法、装置及电子设备
CN113192123B (zh) 一种图像处理方法、装置及设备
CN109242894B (zh) 一种基于移动最小二乘法的图像对齐方法及系统
KR101967284B1 (ko) 다중 영상 정렬 장치 및 방법
KR102371594B1 (ko) 스테레오 카메라 영상 실시간 자동 캘리브레이션 장치, 그를 포함한 시스템 및 그 방법
CN117896626B (zh) 多摄像头检测运动轨迹的方法、装置、设备及存储介质
CN109903336A (zh) 基于局部特征的飞行器姿态跨视场估计方法及装置
CN113066103B (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