CN113298870A - 一种物体的姿态跟踪方法、装置、终端设备和存储介质 - Google Patents

一种物体的姿态跟踪方法、装置、终端设备和存储介质 Download PDF

Info

Publication number
CN113298870A
CN113298870A CN202110493461.7A CN202110493461A CN113298870A CN 113298870 A CN113298870 A CN 113298870A CN 202110493461 A CN202110493461 A CN 202110493461A CN 113298870 A CN113298870 A CN 113298870A
Authority
CN
China
Prior art keywords
dimensional
coordinate system
point
target object
straight line
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
CN202110493461.7A
Other languages
English (en)
Other versions
CN113298870B (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.)
Shenzhen Institute of Advanced Technology of CAS
Original Assignee
Shenzhen Institute of Advanced Technology of CAS
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 Shenzhen Institute of Advanced Technology of CAS filed Critical Shenzhen Institute of Advanced Technology of CAS
Priority to CN202110493461.7A priority Critical patent/CN113298870B/zh
Publication of CN113298870A publication Critical patent/CN113298870A/zh
Application granted granted Critical
Publication of CN113298870B publication Critical patent/CN113298870B/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
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • 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/10—Image acquisition modality
    • G06T2207/10004—Still image; Photographic image
    • G06T2207/10012—Stereo images

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本申请涉及图像处理技术领域，提出一种物体的姿态跟踪方法、装置、终端设备和存储介质。该方法将目标物体的三维图像中的边缘轮廓拟合成直线，投影到该目标物体对应的二维图像中，得到对应的直线段特征的投影，然后通过最小化该投影与匹配的二维图像点之间的几何距离，构建似然函数进行计算得到该目标物体的姿态参数的最优估计值。与传统的将整个三维模型与二维图像点的特征进行匹配的方法相比，本申请将三维模型简化为直线段的组合，能够减少特征匹配的计算量，提高对物体进行姿态跟踪的实时性。

Description

一种物体的姿态跟踪方法、装置、终端设备和存储介质
技术领域
本申请涉及图像处理技术领域，尤其涉及一种物体的姿态跟踪方法、装置、终端设备和存储介质。
背景技术
一个物体的姿态，是指拍摄该物体的图像时，该物体自身所处的坐标系相对于相机坐标系所发生的平移变换和旋转变换。目前，通常使用基于单目视觉的三维跟踪方法来实现对物体的姿态跟踪，该方法通过将目标的三维模型与从相机拍摄的图像中提取的特征相匹配，得到该目标在各个时刻的姿态参数。然而，将三维模型与图像特征进行匹配时需要进行复杂的计算，耗时较长，导致对物体进行姿态跟踪的实时性较差。
发明内容
有鉴于此，本申请实施例提供了一种物体的姿态跟踪方法、装置、终端设备和存储介质，能够提高对物体进行姿态跟踪的实时性。
本申请实施例的第一方面提供了一种物体的姿态跟踪方法，包括：
获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数，所述第一时刻为与所述第二时刻相邻的前一时刻，所述第一图像为采用相机拍摄得到的所述目标物体在预定义的二维图像坐标系下的图像；
针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，其中，所述二维直线段由在三维目标坐标系下的所述目标物体的第二图像中选取的三维直线段投影至所述二维图像坐标系获得，所述三维目标坐标系的原点为所述目标物体上的一点，所述三维直线段为所述第二图像中根据所述目标物体的边缘轮廓线拟合得到的包含该位置点的直线段；
针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
在本申请实施例中，首先获取目标物体前一时刻的姿态参数以及当前时刻的二维图像；然后，针对该二维图像中该目标物体的边缘上的每个位置点，均分别确定与该位置点对应的特征匹配点，具体的操作方式是在该目标物体对应的三维图像中，将包含该位置点的边缘轮廓线拟合成一个三维直线段，然后将该三维直线段投影到该二维图像中形成一个二维直线段，最后从该二维直线段中选取与该位置点之间的距离最小的点作为该位置点对应的特征匹配点；接着，根据每个特征匹配点在二维图像坐标系下的坐标分别计算得到每个特征匹配点在三维相机坐标系下的坐标，并根据目标物体前一时刻的姿态参数计算得到每个特征匹配点在该目标物体所在的三维目标坐标系下的坐标；最后，根据各个特征匹配点在三维目标坐标系下的坐标，以该目标物体的姿态参数作为待估计变量构建似然函数，并将该似然函数的最优解作为该目标物体在当前时刻的姿态参数，从而完成当前时刻的姿态估计。上述过程将目标物体的三维图像中的边缘轮廓拟合成直线，投影到该目标物体对应的二维图像中，得到对应的直线段特征的投影，然后通过最小化该投影与匹配的二维图像点之间的几何距离，构建似然函数进行计算得到该目标物体的姿态参数的最优估计值。与传统的将整个三维模型与二维图像点的特征进行匹配的方法相比，本申请将三维模型简化为直线段的组合，能够减少特征匹配的计算量，提高对物体进行姿态跟踪的实时性。
在本申请的一个实施例中，从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，可以包括：
在所述二维直线段上等间距地划分出多个采样点；
根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点。
进一步的，根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点，可以包括：
分别计算所述多个采样点中的每个采样点对应的法向量和该位置点对应的法向量之间的夹角；
将所述多个采样点中对应的所述夹角小于设定阈值且和该位置点之间的距离最小的采样点确定为该位置点的特征匹配点。
在本申请的一个实施例中，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，可以包括：
根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
获取所述三维直线段在所述三维目标坐标系下的两个端点坐标；
根据所述旋转矩阵、所述平移向量以及所述三维直线段在所述三维目标坐标系下的两个端点坐标，计算得到所述二维直线段在所述二维图像坐标系下的两个端点坐标；
根据所述二维直线段在所述二维图像坐标系下的两个端点坐标，构建得到所述二维直线段。
进一步的，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，可以包括：
根据所述三维直线段在所述三维目标坐标系下的两个端点坐标以及所述二维直线段在所述二维图像坐标系下的两个端点坐标，计算得到第一平面的法向量，所述第一平面为通过所述三维直线段且垂直于第二平面的平面，所述第二平面为根据所述三维直线段的两个端点和所述三维相机坐标系的原点确定的平面；
根据该特征匹配点在所述二维图像坐标系下的坐标、所述旋转矩阵、所述平移向量以及所述第一平面的法向量，计算得到该特征匹配点在所述三维相机坐标系下的坐标。
在本申请的一个实施例中，根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标，可以包括：
根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
根据该特征匹配点在所述三维相机坐标系下的坐标、所述旋转矩阵和所述平移向量，计算得到该特征匹配点在所述三维目标坐标系下的坐标。
在本申请的一个实施例中，根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数，可以包括：
通过最小化各个所述特征匹配点与各自对应的所述位置点之间的几何距离，构建得到所述目标物体的姿态参数的最大似然估计的目标函数；
所述将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数，可以包括：
采用列文伯格-马夸尔特算法计算得到所述目标函数的最优解；
将所述目标函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
本申请实施例的第二方面提供了一种物体的姿态跟踪装置，包括：
数据获取模块，用于获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数，所述第一时刻为与所述第二时刻相邻的前一时刻，所述第一图像为采用相机拍摄得到的所述目标物体在预定义的二维图像坐标系下的图像；
特征匹配点选取模块，用于针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，其中，所述二维直线段由在三维目标坐标系下的所述目标物体的第二图像中选取的三维直线段投影至所述二维图像坐标系获得，所述三维目标坐标系的原点为所述目标物体上的一点，所述三维直线段为所述第二图像中根据所述目标物体的边缘轮廓线拟合得到的包含该位置点的直线段；
坐标计算模块，用于针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
似然函数构建模块，用于根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
姿态参数确定模块，用于将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
本申请实施例的第三方面提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如本申请实施例的第一方面提供的物体的姿态跟踪方法。
本申请实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如本申请实施例的第一方面提供的物体的姿态跟踪方法。
本申请实施例的第五方面提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行本申请实施例的第一方面所述的物体的姿态跟踪方法。
可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种物体的姿态跟踪方法的流程图；
图2是本申请实施例提供的从二维直线段中选取特征匹配点的处理示意图；
图3是本申请实施例提供的航天器在普通深空背景下的姿态跟踪效果示意图；
图4是本申请实施例提供的航天器在复杂外空背景下的姿态跟踪效果示意图；
图5是本申请实施例提供的一种物体的姿态跟踪装置的结构图；
图6是本申请实施例提供的一种终端设备的示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。另外，在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
本申请提出一种物体的姿态跟踪方法、装置、终端设备和存储介质，能够解决在训练图像分割模型时缺少训练数据的问题。应当理解，本申请各个方法实施例的执行主体为各种类型的终端设备或服务器，比如手机、平板电脑、笔记本电脑、台式电脑和可穿戴设备等。
请参阅图1，示出了本申请实施例提供的一种物体的姿态跟踪方法，包括：
101、获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数；
目标物体是需要执行姿态跟踪的各种类别的物体，例如可以是人物、汽车或者航天器等物体，对该目标物体执行姿态跟踪，也即需要确定该目标物体在各个不同时刻的姿态参数。首先，获取目标物体在第二时刻的第一图像以及该目标物体在第一时刻的姿态参数，其中，第一时刻为与第二时刻相邻的前一时刻，例如，若第二时刻为当前时刻，则第一时刻为当前时刻的前一时刻。该第一图像是采用相机拍摄得到的该目标物体在第二时刻的二维图像，而该二维图像是在预定义的二维图像坐标系下的图像。
本申请实施例通过目标物体在前一时刻的姿态参数以及当前时刻该目标物体的二维图像确定该目标物体在当前时刻的姿态参数，在确定当前时刻的姿态参数后，则可以将当前时刻的姿态参数和下一时刻该目标物体的二维图像作为输入，采用相同的方式确定该目标物体在下一时刻的姿态参数，如此反复，则可以确定该目标物体在各个不同时刻的姿态参数，完成对该目标物体的姿态跟踪。另外，对于该目标物体在初始时刻(第一个时刻)的姿态参数，可以人工设置一个经验值，例如旋转矩阵可以取单位矩阵，平移向量的三个元素可以都为0。
以下对本申请实施例所使用到的各个坐标系进行定义和说明：
(1)三维相机坐标系
三维相机坐标系(Camera Coordinate System，CCS)可以表示为Oc-XcYcZc，其中，Oc为原点，位于相机(拍摄该目标物体使用的相机)的光心；Xc、Yc和Zc是空间三个方向的坐标轴，其中Zc轴沿着相机的光轴指向相机前方；
(2)二维图像坐标系
在平面Zc＝f上定义一个二维图像坐标系(Image Coordinate System，ICS)，其中，f是该相机的焦距，该二维图像坐标系可以表示为o-uv，其中原点o为平面Zc＝f上的任意一点，坐标轴u和坐标轴v分别平行于坐标轴Xc和坐标轴Yc。
(3)三维目标坐标系
在该目标物体上建立一个三维目标坐标系(Object Coordinate System，OCS)，可以表示为Oo-XoYoZo，其中，Oo为原点，可以位于该目标物体上的任意一点，Xo、Yo和Zo是空间三个方向的坐标轴。
在给定相机内参矩阵K的情况下，在三维目标坐标系OCS下的一个点X与其在二维图像坐标系ICS下的投影点x之间的关系可以表示为以下的公式(1.1)：
Figure BDA0003053316710000081
其中，
Figure BDA0003053316710000082
和
Figure BDA0003053316710000083
分别是X和x的齐次坐标，R和t分别表示从三维目标坐标系OCS转换至三维相机坐标系CCS的旋转矩阵和平移向量，矩阵[R t]表示由旋转矩阵R和平移向量t拼接得到的矩阵。
具体的，旋转矩阵R可以表示为如下的公式(1.2)：
Figure BDA0003053316710000084
在公式(2.2)中，3个3*3的矩阵中缺失的矩阵元素为0，三个欧拉角α，β和γ分别是物体绕坐标系三个坐标轴的旋转角度。
平移向量t可以表示为如下的公式(1.3)：
t＝[tx ty tz]T (1.3)
其中，tx、ty和tz分别表示相对于当前坐标系原点的各个方向的位置偏移量。目标物体的六自由度姿态参数p可以表示为如下的公式(1.4)：
p＝[α,β,γ,tx,ty,tz]T (1.4)
在对目标物体执行姿态跟踪时，即需要估计该目标物体在各个不同时刻下的姿态参数p。
根据以上的内容可以获知，在步骤101中获取到的目标物体在第二时刻的第一图像是在二维图像坐标系ICS下的图像，在步骤101中获取到的该目标物体在第一时刻的姿态参数可以用p表示，在已知p的情况下，可以计算得到对应的旋转矩阵R和平移向量t。
102、针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点；
在获得第一图像之后，针对该第一图像中包含的该目标物体的边缘上的每个位置点，都可以采用相同的方法确定该位置点对应的特征匹配点。具体的处理步骤可以分为两步，第一步是根据该目标物体在第一时刻的姿态参数在该第一图像中确定与该位置点对应的二维直线段，其中，该二维直线段由在三维目标坐标系下的该目标物体的第二图像中选取的三维直线段投影至该二维图像坐标系获得，该三维目标坐标系的原点为该目标物体上的一点，该三维直线段为该第二图像中根据该目标物体的边缘轮廓线拟合得到的包含该位置点的直线段。第二图像是相同时刻下该目标物体在三维目标坐标系(即前文所述的OCS坐标系)下的三维图像(例如可以是该目标物体的三维模型)，在该第二图像中找到对应的位置点(例如该位置点在第一图像中是目标物体的左上角点，则在第二图像中同样找到该目标物体的左上角点)，然后将包含该位置点的物体边缘轮廓线拟合成一条直线段，从而得到一个三维直线段。接着，将三维目标坐标系下的该三维直线段投影至该二维图像坐标系，会得到一个与该位置点对应的二维直线段。
在本申请的一个实施例中，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，可以包括：
(1)根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
(2)获取所述三维直线段在所述三维目标坐标系下的两个端点坐标；
(3)根据所述旋转矩阵、所述平移向量以及所述三维直线段在所述三维目标坐标系下的两个端点坐标，计算得到所述二维直线段在所述二维图像坐标系下的两个端点坐标；
(4)根据所述二维直线段在所述二维图像坐标系下的两个端点坐标，构建得到所述二维直线段。
根据该目标物体在第一时刻的姿态参数，可以确定从该三维目标坐标系(OCS)转换至三维相机坐标系(CCS)的旋转矩阵和平移向量。例如，根据姿态参数p，可以采用上述公式(1.2)计算得到旋转矩阵R，采用上述公式(1.3)计算得到平移向量t。在通过直线拟合的方式生成该三维直线段后，该三维直线段在三维目标坐标系(OCS)下的两个端点坐标是已知的，可以表示为Po和Pe这两个端点投影至二维图像坐标系(ICS)后，分别得到对应的投影点po和pe。
根据上述公式(1.1)，可以获得表达式
Figure BDA0003053316710000101
其中R和t根据姿态参数求出，K是已知的相机内参矩阵，因此可以计算得到投影至该二维图像坐标系后的两个端点坐标，而在获得这两个端点坐标之后，即可构建出对应的二维直线段pope。
在确定二维直线段之后，下一步是从该二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点。
具体的，若令η＝pe-po，则该二维直线段上任意一点M的向量可以用q＝po+kη表示，其中，0≤k≤1，即向量q被限制在二维直线段pope的两个端点的范围内，该向量的方向由二维图像坐标系(ICS)的原点指向点M。
假设x为该位置点，
Figure BDA0003053316710000111
为该位置点对应的特征匹配点，具体是在该三维直线段投影得到的二维直线段pope上与x最接近的一点。当x与
Figure BDA0003053316710000112
足够逼近时，可以得到以下的公式(1.5)：
k＝argmin||x-po-kη||2,0≤k≤1 (1.5)
对公式(1.5)进行松弛式的处理，即将k作为变量，求解公式(1.5)中的一元二次方法，可以得到以下的公式(1.6)：
k＝(x-po)Tη/||η||2 (1.6)
为了防止对
Figure BDA0003053316710000113
的估计超过该二维直线段的范围，k的取值可以采用以下的公式(1.7)表示：
k＝Median([0,1,(x-po)Tη/||η||2]) (1.7)
其中，Median(*)表示中值算子，可以根据k的取值确定
Figure BDA0003053316710000114
是二维直线段pope上的哪一点。例如，若k等于0，则表示
Figure BDA0003053316710000115
是二维直线段pope的起点，即po；若k等于1，则表示
Figure BDA0003053316710000116
是二维直线段pope的终点，即pe；若k等于0-1之间的数值，则表示
Figure BDA0003053316710000117
是二维直线段pope两个端点之间的点，如若k等于0.5，则表示
Figure BDA0003053316710000118
是二维直线段pope的中点，以此类推。
在本申请的一个实施例中，从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，可以包括：
(1)在所述二维直线段上等间距地划分出多个采样点；
(2)根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点。
在位置点相当于一个图像数据点，为了找到该图像数据点与直线段特征投影的匹配(即该位置点对应的特征匹配点)，可以在直线段特征的投影线(即该二维直线段)上等间距地划分出多个采样点，然后从这些采样点中选取出该位置点对应的特征匹配点。
具体的，根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点，可以包括：
(2.1)分别计算所述多个采样点中的每个采样点对应的法向量和该位置点对应的法向量之间的夹角；
(2.2)将所述多个采样点中对应的所述夹角小于设定阈值且和该位置点之间的距离最小的采样点确定为该位置点的特征匹配点。
针对每个采样点，可以分别计算该采样点的法向量和该位置点的法向量之间的夹角，然后从中筛选出该夹角小于某个设定阈值(例如15°)的采样点，再从筛选出的采样点中查找和该位置点之间的距离最小的采样点作为特征匹配点。
如图2所示，在二维直线段pope上等间距采样等到多个采样点{qs}，然后以每个采样点为中心，在该采样点的法线上的一定范围内搜索图像边缘点(即该位置点)，这个搜索范围可以称作搜索区间。在图2中，x为目标物体边缘上的一个点，其对应的法向量为e，分别计算每个采样点qs的法向量与法向量e之间的夹角，然后从该夹角小于15°的各个采样点中查找与x最近的采样点，作为x对应的特征匹配点
Figure BDA0003053316710000121
103、针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
在获得每个位置点对应的特征匹配点之后，首先针对每个特征匹配点，根据该特征匹配点在二维图像坐标系(ICS)下的坐标计算得到该特征匹配点在三维相机坐标系(CCS)下的坐标。在本申请的一个实施例中，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，可以包括：
(1)根据所述三维直线段在所述三维目标坐标系下的两个端点坐标以及所述二维直线段在所述二维图像坐标系下的两个端点坐标，计算得到第一平面的法向量，所述第一平面为通过所述三维直线段且垂直于第二平面的平面，所述第二平面为根据所述三维直线段的两个端点和所述三维相机坐标系的原点确定的平面；
(2)根据该特征匹配点在所述二维图像坐标系下的坐标、所述旋转矩阵、所述平移向量以及所述第一平面的法向量，计算得到该特征匹配点在所述三维相机坐标系下的坐标。
以前文所述的三维直线段PoPe为例，其是一个由Po指向Pe的向量，它与三维相机坐标系(CCS)之间存在旋转变换R(R可以是根据该目标物体在第一时刻的姿态参数确定的旋转矩阵)的映射关系，该向量在三维相机坐标系(CCS)下的坐标可以表示为R(Po-Pe)。令平面U为通过该三维直线段且垂直于Po、Pe、Oc所在平面的平面(U为第一平面，Po、Pe、Oc所在平面为第二平面)，则平面U的法向量n可以表示为以下的公式(1.8)：
Figure BDA0003053316710000131
另外，假设
Figure BDA0003053316710000132
为该特征匹配点在二维图像坐标系下的坐标，则该特征匹配点在三维相机坐标系下的坐标
Figure BDA0003053316710000133
可以视作它的反投影线
Figure BDA0003053316710000134
与平面U的交点，并通过以下的公式(1.9)计算：
Figure BDA0003053316710000135
其中，旋转矩阵R和平移向量t可以根据该目标物体在第一时刻的姿态参数确定。在公式(1.8)和(1.9)中，上标“～”表示对应的齐次坐标。
在根据每个特征匹配点在二维图像坐标系(ICS)下的坐标分别计算得到每个特征匹配点在三维相机坐标系(CCS)下的坐标之后，根据该目标物体在第一时刻的姿态参数和每个特征匹配点在三维相机坐标系(CCS)下的坐标，分别计算得到每个特征匹配点在三维目标坐标系(OCS)下的坐标。
在本申请的一个实施例中，根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标，可以包括：
(1)根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
(2)根据该特征匹配点在所述三维相机坐标系下的坐标、所述旋转矩阵和所述平移向量，计算得到该特征匹配点在所述三维目标坐标系下的坐标。
根据该目标物体在第一时刻的姿态参数，可以计算得到从三维目标坐标系(OCS)转换至三维相机坐标系(CCS)的旋转矩阵R和平移向量t，然后可以采用如下的公式(1.10)计算得到该特征匹配点在三维目标坐标系(OCS)下的坐标：
Figure BDA0003053316710000141
其中，
Figure BDA0003053316710000142
为某个特征匹配点在三维相机坐标系(CCS)下的坐标，ξj为
Figure BDA0003053316710000143
在三维目标坐标系(OCS)下的坐标。
104、根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
为了估计该目标物体在第二时刻的姿态参数，可以将该目标物体的姿态参数作为待估计的变量，并根据各个特征匹配点在三维目标坐标系(CCS)下的坐标构建一个似然函数，通过最小化三维直线段的投影与匹配的二维图像点之间的几何距离对该似然函数进行求解，从而获得变量估计的最优解，作为该目标物体在第二时刻的姿态参数。
具体的，根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数，可以包括：
通过最小化各个所述特征匹配点与各自对应的所述位置点之间的几何距离，构建得到所述目标物体的姿态参数的最大似然估计的目标函数。
根据统计学理论可以获知，最大似然估计为渐进有效估计，是通常所能获得的最好估计，因此可以通过计算该目标物体姿态参数的最大似然估计以确定最优的姿态参数估计值。三维的直线段特征在二维图像平面上的投影可以通过以下的公式(1.11)表示为目标物体姿态参数p的闭合解析方程：
Figure BDA0003053316710000151
其中，
Figure BDA0003053316710000152
表示三维直线段在像平面投影(即前文所述的二维直线段)上的一点，即前文所述的任意一个特征匹配点。姿态参数p的最大似然估计可通过最小化三维直线段的投影与匹配的二维图像点之间的几何距离得到，相应的几何拟合相当于使如下的最大似然估计的目标函数取极大值：
Figure BDA0003053316710000153
其中，g(p|{xj})表示目标函数，xj是前文所述的二维图像(即第一图像)中包含的该目标物体的边缘上的某个位置点，该目标物体的二维图像中总共有m个这样的位置点，
Figure BDA0003053316710000154
是xj对应的特征匹配点，σ是图像点xj的噪声水平，可以根据经验设置为0.1或0.2等数值。
对公式(1.12)取负对数，可以得到以下的目标函数表达式：
Figure BDA0003053316710000155
结合公式(1.1)和公式(1.10)，特征匹配点
Figure BDA0003053316710000156
可以表示为：
Figure BDA0003053316710000157
其中，K12表示相机内参矩阵K的第一、第二行，
Figure BDA0003053316710000158
表示
Figure BDA0003053316710000159
的第三维度，R3表示旋转矩阵R的第三行。将公式(1.14)代入公式(1.13)，可以得到：
Figure BDA0003053316710000161
针对公式(1.15)所表示的目标函数进行求解，可以得到姿态参数p的最优解，具体的求解方法可以参照步骤105中的相关说明。
105、将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
在获得该似然函数的最优解后，即可将该最优解作为该目标物体在第二时刻的姿态参数，从而完成第二时刻的姿态估计。
具体的，若构建的是姿态参数的最大似然函数，其目标函数如公式(1.15)所示，则步骤105可以包括：
(1)采用列文伯格-马夸尔特算法计算得到所述目标函数的最优解；
(2)将所述目标函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
列文伯格-马夸尔特算法，也即Levenberg-Marquardt算法，采用该算法对目标函数进行求解，可以获得一个对应的最优解，该最优解即为最终估计得到的该目标物体在第二时刻的姿态参数。采用Levenberg-Marquardt算法计算目标函数待估计参数最优解的基本原理可以参照现有技术，在此不再赘述。与其它常用的迭代求函数极值算法(例如牛顿法和梯度法)相比，Levenberg-Marquardt算法综合了牛顿法和梯度法的优点，用容易计算的雅可比矩阵代替牛顿法复杂的H矩阵进行计算，而在算法的迭代求解过程中，当梯度下降太快时，可以使用较小的梯度下降步长，使整个公式接近于高斯牛顿法；当梯度下降太慢时，可以使用较大的梯度下降步长，使整个公式接近于梯度法。
本申请实施例将目标物体的三维图像中的边缘轮廓拟合成直线，投影到该目标物体对应的二维图像中，得到对应的直线段特征的投影，然后通过最小化该投影与匹配的二维图像点之间的几何距离，构建似然函数进行计算得到该目标物体的姿态参数的最优估计值。与传统的将整个三维模型与二维图像点的特征进行匹配的方法相比，本申请将三维模型简化为直线段的组合，能够减少特征匹配的计算量，提高对物体进行姿态跟踪的实时性。
一般来说，一个物体的边缘可由直线段直接表示或者可用直线段拟合进行表示，本申请将目标物体简化成若干直线段的组合，利用这些直线段特征可以简化姿态估计的计算，提高实时性以及算法的鲁棒性。首先，基于法向搜索方法找到三维直线段特征投影的匹配图像数据，然后利用Levenberg-Marquardt等算法最小化直线段特征的投影与匹配的图像点之间的几何距离，得到姿态的极大似然估计。
为了证明本申请提出的姿态跟踪方法能够实现对目标物体的实时三维姿态跟踪，且在目标物体处于复杂背景时算法依然具有较好的姿态跟踪效果，本申请将某个航天器作为目标物体，并采用本申请提出的方法实现了对该航天器的姿态跟踪，具体的效果示意图如图3和图4所示。其中，图3是航天器在普通的深空背景下的姿态跟踪效果示意图，图4是航天器在复杂外空背景下的姿态跟踪效果示意图。通过图3和图4可以获知，采用本申请提出的姿态跟踪方法，无论目标物体所处的背景是否复杂，均能实现对该目标物体的实时且准确的姿态跟踪效果。
应理解，上述各个实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
上面主要描述了一种物体的姿态跟踪方法，下面将对一种物体的姿态跟踪装置进行描述。
请参阅图5，本申请实施例中一种物体的姿态跟踪装置的一个实施例包括：
数据获取模块501，用于获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数，所述第一时刻为与所述第二时刻相邻的前一时刻，所述第一图像为采用相机拍摄得到的所述目标物体在预定义的二维图像坐标系下的图像；
特征匹配点选取模块502，用于针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，其中，所述二维直线段由在三维目标坐标系下的所述目标物体的第二图像中选取的三维直线段投影至所述二维图像坐标系获得，所述三维目标坐标系的原点为所述目标物体上的一点，所述三维直线段为所述第二图像中根据所述目标物体的边缘轮廓线拟合得到的包含该位置点的直线段；
坐标计算模块503，用于针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
似然函数构建模块504，用于根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
姿态参数确定模块505，用于将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
在本申请的一个实施例中，所述特征匹配点选取模块可以包括：
采样点划分单元，用于在所述二维直线段上等间距地划分出多个采样点；
采样点选取单元，用于根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点。
进一步的，所述采样点选取单元可以包括：
法向量夹角计算子单元，用于分别计算所述多个采样点中的每个采样点对应的法向量和该位置点对应的法向量之间的夹角；
特征匹配点确定子单元，用于将所述多个采样点中对应的所述夹角小于设定阈值且和该位置点之间的距离最小的采样点确定为该位置点的特征匹配点。
在本申请的一个实施例中，所述特征匹配点选取模块可以包括：
第一转换参数确定单元，用于根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
端点坐标获取单元，用于获取所述三维直线段在所述三维目标坐标系下的两个端点坐标；
端点坐标计算单元，用于根据所述旋转矩阵、所述平移向量以及所述三维直线段在所述三维目标坐标系下的两个端点坐标，计算得到所述二维直线段在所述二维图像坐标系下的两个端点坐标；
二维直线段构建单元，用于根据所述二维直线段在所述二维图像坐标系下的两个端点坐标，构建得到所述二维直线段。
在本申请的一个实施例中，所述坐标计算模块可以包括：
法向量计算单元，用于根据所述三维直线段在所述三维目标坐标系下的两个端点坐标以及所述二维直线段在所述二维图像坐标系下的两个端点坐标，计算得到第一平面的法向量，所述第一平面为通过所述三维直线段且垂直于第二平面的平面，所述第二平面为根据所述三维直线段的两个端点和所述三维相机坐标系的原点确定的平面；
坐标计算单元，用于根据该特征匹配点在所述二维图像坐标系下的坐标、所述旋转矩阵、所述平移向量以及所述第一平面的法向量，计算得到该特征匹配点在所述三维相机坐标系下的坐标。
在本申请的一个实施例中，所述坐标计算模块可以包括：
第二转换参数确定单元，用于根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
特征匹配点坐标计算单元，用于根据该特征匹配点在所述三维相机坐标系下的坐标、所述旋转矩阵和所述平移向量，计算得到该特征匹配点在所述三维目标坐标系下的坐标。
在本申请的一个实施例中，所述似然函数构建模块可以包括：
目标函数构建单元，用于通过最小化各个所述特征匹配点与各自对应的所述位置点之间的几何距离，构建得到所述目标物体的姿态参数的最大似然估计的目标函数；
所述姿态参数确定模块可以包括：
目标函数计算单元，用于采用列文伯格-马夸尔特算法计算得到所述目标函数的最优解；
姿态参数确定单元，用于将所述目标函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
本申请实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如图1表示的任意一种物体的姿态跟踪方法。
本申请实施例还提供一种计算机程序产品，当该计算机程序产品在终端设备上运行时，使得终端设备执行实现如图1表示的任意一种物体的姿态跟踪方法。
图6是本申请一实施例提供的终端设备的示意图。如图6所示，该实施例的终端设备6包括：处理器60、存储器61以及存储在所述存储器61中并可在所述处理器60上运行的计算机程序62。所述处理器60执行所述计算机程序62时实现上述各个物体的姿态跟踪方法的实施例中的步骤，例如图1所示的步骤101至105。或者，所述处理器60执行所述计算机程序62时实现上述各装置实施例中各模块/单元的功能，例如图5所示模块501至505的功能。
所述计算机程序62可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器61中，并由所述处理器60执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序62在所述终端设备6中的执行过程。
所称处理器60可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器61可以是所述终端设备6的内部存储单元，例如终端设备6的硬盘或内存。所述存储器61也可以是所述终端设备6的外部存储设备，例如所述终端设备6上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器61还可以既包括所述终端设备6的内部存储单元也包括外部存储设备。所述存储器61用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器61还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括是电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

1.一种物体的姿态跟踪方法，其特征在于，包括：
获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数，所述第一时刻为与所述第二时刻相邻的前一时刻，所述第一图像为采用相机拍摄得到的所述目标物体在预定义的二维图像坐标系下的图像；
针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，其中，所述二维直线段由在三维目标坐标系下的所述目标物体的第二图像中选取的三维直线段投影至所述二维图像坐标系获得，所述三维目标坐标系的原点为所述目标物体上的一点，所述三维直线段为所述第二图像中根据所述目标物体的边缘轮廓线拟合得到的包含该位置点的直线段；
针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
2.如权利要求1所述的方法，其特征在于，从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，包括：
在所述二维直线段上等间距地划分出多个采样点；
根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点。
3.如权利要求2所述的方法，其特征在于，根据该位置点对应的法向量从所述多个采样点中选取出一个采样点，作为该位置点的特征匹配点，包括：
分别计算所述多个采样点中的每个采样点对应的法向量和该位置点对应的法向量之间的夹角；
将所述多个采样点中对应的所述夹角小于设定阈值且和该位置点之间的距离最小的采样点确定为该位置点的特征匹配点。
4.如权利要求1所述的方法，其特征在于，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，包括：
根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
获取所述三维直线段在所述三维目标坐标系下的两个端点坐标；
根据所述旋转矩阵、所述平移向量以及所述三维直线段在所述三维目标坐标系下的两个端点坐标，计算得到所述二维直线段在所述二维图像坐标系下的两个端点坐标；
根据所述二维直线段在所述二维图像坐标系下的两个端点坐标，构建得到所述二维直线段。
5.如权利要求4所述的方法，其特征在于，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，包括：
根据所述三维直线段在所述三维目标坐标系下的两个端点坐标以及所述二维直线段在所述二维图像坐标系下的两个端点坐标，计算得到第一平面的法向量，所述第一平面为通过所述三维直线段且垂直于第二平面的平面，所述第二平面为根据所述三维直线段的两个端点和所述三维相机坐标系的原点确定的平面；
根据该特征匹配点在所述二维图像坐标系下的坐标、所述旋转矩阵、所述平移向量以及所述第一平面的法向量，计算得到该特征匹配点在所述三维相机坐标系下的坐标。
6.如权利要求1所述的方法，其特征在于，根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标，包括：
根据所述目标物体在第一时刻的姿态参数，确定从所述三维目标坐标系转换至所述三维相机坐标系的旋转矩阵和平移向量；
根据该特征匹配点在所述三维相机坐标系下的坐标、所述旋转矩阵和所述平移向量，计算得到该特征匹配点在所述三维目标坐标系下的坐标。
7.如权利要求1至6中任一项所述的方法，其特征在于，根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数，包括：
通过最小化各个所述特征匹配点与各自对应的所述位置点之间的几何距离，构建得到所述目标物体的姿态参数的最大似然估计的目标函数；
所述将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数，包括：
采用列文伯格-马夸尔特算法计算得到所述目标函数的最优解；
将所述目标函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
8.一种物体的姿态跟踪装置，其特征在于，包括：
数据获取模块，用于获取目标物体在第二时刻的第一图像以及所述目标物体在第一时刻的姿态参数，所述第一时刻为与所述第二时刻相邻的前一时刻，所述第一图像为采用相机拍摄得到的所述目标物体在预定义的二维图像坐标系下的图像；
特征匹配点选取模块，用于针对所述第一图像中包含的所述目标物体的边缘上的每个位置点，根据所述目标物体在第一时刻的姿态参数在所述第一图像中确定与该位置点对应的二维直线段，并从所述二维直线段中选取与该位置点之间的距离最小的点作为该位置点的特征匹配点，其中，所述二维直线段由在三维目标坐标系下的所述目标物体的第二图像中选取的三维直线段投影至所述二维图像坐标系获得，所述三维目标坐标系的原点为所述目标物体上的一点，所述三维直线段为所述第二图像中根据所述目标物体的边缘轮廓线拟合得到的包含该位置点的直线段；
坐标计算模块，用于针对每个所述特征匹配点，根据该特征匹配点在所述二维图像坐标系下的坐标计算得到该特征匹配点在所述相机对应的三维相机坐标系下的坐标，并根据所述目标物体在第一时刻的姿态参数和该特征匹配点在所述三维相机坐标系下的坐标，计算得到该特征匹配点在所述三维目标坐标系下的坐标；
似然函数构建模块，用于根据各个所述特征匹配点在所述三维目标坐标系下的坐标，以所述目标物体的姿态参数作为待估计变量构建似然函数；
姿态参数确定模块，用于将所述似然函数的最优解确定为所述目标物体在所述第二时刻的姿态参数。
9.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的物体的姿态跟踪方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的物体的姿态跟踪方法。
CN202110493461.7A 2021-05-07 2021-05-07 一种物体的姿态跟踪方法、装置、终端设备和存储介质 Active CN113298870B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110493461.7A CN113298870B (zh) 2021-05-07 2021-05-07 一种物体的姿态跟踪方法、装置、终端设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110493461.7A CN113298870B (zh) 2021-05-07 2021-05-07 一种物体的姿态跟踪方法、装置、终端设备和存储介质

Publications (2)

Publication Number Publication Date
CN113298870A true CN113298870A (zh) 2021-08-24
CN113298870B CN113298870B (zh) 2023-03-28

Family

ID=77320928

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110493461.7A Active CN113298870B (zh) 2021-05-07 2021-05-07 一种物体的姿态跟踪方法、装置、终端设备和存储介质

Country Status (1)

Country Link
CN (1) CN113298870B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113920160A (zh) * 2021-09-17 2022-01-11 北京航空航天大学 初始位姿的估计方法、设备、计算机可读介质和程序产品
CN114800504A (zh) * 2022-04-26 2022-07-29 平安普惠企业管理有限公司 机器人位姿分析方法、装置、设备及存储介质
CN115631308A (zh) * 2022-12-15 2023-01-20 北京集度科技有限公司 人造杆重建方法、装置、车辆及介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6072903A (en) * 1997-01-07 2000-06-06 Kabushiki Kaisha Toshiba Image processing apparatus and image processing method
US6243106B1 (en) * 1998-04-13 2001-06-05 Compaq Computer Corporation Method for figure tracking using 2-D registration and 3-D reconstruction
CN109087323A (zh) * 2018-07-25 2018-12-25 武汉大学 一种基于精细cad模型的图像车辆三维姿态估计方法
CN109872343A (zh) * 2019-02-01 2019-06-11 视辰信息科技(上海)有限公司 弱纹理物体姿态跟踪方法、系统及装置
US20200184682A1 (en) * 2018-12-11 2020-06-11 Ubtech Robotics Corp Ltd Object pose tracking method and apparatus
CN111504317A (zh) * 2020-03-09 2020-08-07 中振同辂(江苏)机器人有限公司 一种基于单线激光雷达的室内定位方法
EP3739356A1 (en) * 2019-05-12 2020-11-18 Origin Wireless, Inc. Method, apparatus, and system for wireless tracking, scanning and monitoring
CN112435294A (zh) * 2020-11-02 2021-03-02 中国科学院深圳先进技术研究院 目标物体的六自由度姿态跟踪方法及终端设备

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6072903A (en) * 1997-01-07 2000-06-06 Kabushiki Kaisha Toshiba Image processing apparatus and image processing method
US6243106B1 (en) * 1998-04-13 2001-06-05 Compaq Computer Corporation Method for figure tracking using 2-D registration and 3-D reconstruction
CN109087323A (zh) * 2018-07-25 2018-12-25 武汉大学 一种基于精细cad模型的图像车辆三维姿态估计方法
US20200184682A1 (en) * 2018-12-11 2020-06-11 Ubtech Robotics Corp Ltd Object pose tracking method and apparatus
CN109872343A (zh) * 2019-02-01 2019-06-11 视辰信息科技(上海)有限公司 弱纹理物体姿态跟踪方法、系统及装置
EP3739356A1 (en) * 2019-05-12 2020-11-18 Origin Wireless, Inc. Method, apparatus, and system for wireless tracking, scanning and monitoring
CN111504317A (zh) * 2020-03-09 2020-08-07 中振同辂(江苏)机器人有限公司 一种基于单线激光雷达的室内定位方法
CN112435294A (zh) * 2020-11-02 2021-03-02 中国科学院深圳先进技术研究院 目标物体的六自由度姿态跟踪方法及终端设备

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
朱世昕: "基于半直接方法的序列影像直线特征跟踪匹配算法", 《计算机科学》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113920160A (zh) * 2021-09-17 2022-01-11 北京航空航天大学 初始位姿的估计方法、设备、计算机可读介质和程序产品
CN114800504A (zh) * 2022-04-26 2022-07-29 平安普惠企业管理有限公司 机器人位姿分析方法、装置、设备及存储介质
CN115631308A (zh) * 2022-12-15 2023-01-20 北京集度科技有限公司 人造杆重建方法、装置、车辆及介质
CN115631308B (zh) * 2022-12-15 2023-03-31 北京集度科技有限公司 人造杆重建方法、装置、车辆及介质

Also Published As

Publication number Publication date
CN113298870B (zh) 2023-03-28

Similar Documents

Publication Publication Date Title
CN113298870B (zh) 一种物体的姿态跟踪方法、装置、终端设备和存储介质
US10043308B2 (en) Image processing method and apparatus for three-dimensional reconstruction
CN111598993B (zh) 基于多视角成像技术的三维数据重建方法、装置
CN110276317B (zh) 一种物体尺寸检测方法、物体尺寸检测装置及移动终端
CN110363817B (zh) 目标位姿估计方法、电子设备和介质
CN111208783B (zh) 一种动作模仿方法、装置、终端及计算机存储介质
CN112328715B (zh) 视觉定位方法及相关模型的训练方法及相关装置、设备
US9846974B2 (en) Absolute rotation estimation including outlier detection via low-rank and sparse matrix decomposition
CN109919971B (zh) 图像处理方法、装置、电子设备及计算机可读存储介质
CN111144349B (zh) 一种室内视觉重定位方法及系统
CN112509036B (zh) 位姿估计网络训练及定位方法、装置、设备、存储介质
US20220392201A1 (en) Image feature matching method and related apparatus, device and storage medium
CN112336342A (zh) 手部关键点检测方法、装置及终端设备
CN112651380A (zh) 人脸识别方法、人脸识别装置、终端设备及存储介质
CN112435193A (zh) 一种点云数据去噪的方法、装置、存储介质和电子设备
CN111105452A (zh) 基于双目视觉的高低分辨率融合立体匹配方法
CN111739071A (zh) 基于初始值的快速迭代配准方法、介质、终端和装置
CN113642397B (zh) 一种基于手机视频的物体长度测量方法
CN108447084B (zh) 基于orb特征的立体匹配补偿方法
CN115705651A (zh) 视频运动估计方法、装置、设备和计算机可读存储介质
CN112197708B (zh) 测量方法及装置、电子设备及存储介质
CN112767457A (zh) 一种基于主成分分析的平面点云匹配方法及装置
CN115661493A (zh) 一种对象位姿的确定方法及装置、设备及存储介质
CN117252912A (zh) 深度图像获取方法、电子设备及存储介质
KR102382883B1 (ko) 3차원 손 자세 인식 장치 및 방법

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