CN112686950B - 位姿估计方法、装置、终端设备及计算机可读存储介质 - Google Patents
位姿估计方法、装置、终端设备及计算机可读存储介质 Download PDFInfo
- Publication number
- CN112686950B CN112686950B CN202011398886.1A CN202011398886A CN112686950B CN 112686950 B CN112686950 B CN 112686950B CN 202011398886 A CN202011398886 A CN 202011398886A CN 112686950 B CN112686950 B CN 112686950B
- Authority
- CN
- China
- Prior art keywords
- pose
- image
- target object
- data
- target
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 239000011159 matrix material Substances 0.000 claims description 54
- 238000004590 computer program Methods 0.000 claims description 22
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000009877 rendering Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 description 9
- 241000190070 Sarracenia purpurea Species 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 239000011521 glass Substances 0.000 description 2
- 241001122767 Theaceae Species 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000003708 edge detection Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000002945 steepest descent method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Abstract
本申请适用于计算机视觉技术领域，提供了一种位姿估计方法、装置、终端设备及计算机可读存储介质，包括：通过拍摄装置获取目标物体的拍摄图像；将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，其中，所述预设模板库中包括所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的所述目标物体的模板图像；根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿；根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿。通过上述方法，能够有效提高位姿估计结果的准确性。
Description
技术领域
本申请属于计算机视觉技术领域，尤其涉及一种位姿估计方法、装置、终端设备及计算机可读存储介质。
背景技术
随着科技的进步，服务机器人已逐步进入到人们的日常生活中。机器人在日常的服务中需要检测一些家庭常见物体、并估计其位姿，根据物体的位姿规划下一步的操作路径。比如最常见的端茶倒水操作，机器人需要知道水杯在哪里，所以机器人首先需要利用自身挂载的相机检测到水杯，然后估计水杯在相机坐标系下的位姿，再根据位姿完成抓取。可见，位姿估计在计算机视觉领域扮演着十分重要的角色。
位姿估计的实质是确定目标物体和相机之间的相对姿态。现有的位姿估计方法，通常是基于特征匹配的方法，即根据目标物体与图像之间匹配的特征点确定位姿。但是实际应用中，目标物体可能为透明物体，由于透明物体缺乏纹理信息，导致基于特征匹配的方法确定出的位姿是不准确的。
发明内容
本申请实施例提供了一种位姿估计方法、装置、终端设备及计算机可读存储介质，可以提供位姿估计结果的准确性。
第一方面，本申请实施例提供了一种方法，包括：
通过拍摄装置获取目标物体的拍摄图像；
将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，其中，所述预设模板库中包括所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的所述目标物体的模板图像；
根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿；
根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿。
本申请实施例中，将拍摄图像与预设模板库中的模板图像相匹配，将匹配的目标图像对应的位姿数据确定为目标物体的初始位姿，通过上述方法，能够较快地确定目标物体的位姿的大致范围；然后根据初始位姿进行迭代计算，获得目标物体的最终位姿，能够在大致范围内进一步确定目标物体的准确位姿。通过上述方法，能够有效提高位姿估计结果的准确性，同时能够大大提高位姿估计的计算效率。
在第一方面的一种可能的实现方式中，所述方法还包括：
获取所述拍摄装置的内参数据；
基于参考距离和所述内参数据，确定所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据，其中，所述参考距离为预设的所述目标物体与所述拍摄装置之间的距离；
获取每组所述位姿数据对应的模板图像；
根据所述位姿数据以及所述模板图像建立所述预设模板库。
在第一方面的一种可能的实现方式中，所述获取每组所述位姿数据对应的模板图像，包括：
对于每组所述位姿数据，获取所述位姿数据对应的所述目标物体的渲染图像；
检测所述渲染图像中所述目标物体的边缘，获得所述目标物体的第一边缘图像；
将所述目标物体的第一边缘图像确定为所述位姿数据对应的所述目标物体的模板图像。
在第一方面的一种可能的实现方式中，所述将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，包括：
检测所述拍摄图像中所述目标物体的边缘，获得所述目标物体的第二边缘图像；
将所述预设模板库中与所述第二边缘图像相匹配的模板图像确定为所述目标图像。
在第一方面的一种可能的实现方式中，所述根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿，包括：
根据所述目标图像对应的所述位姿数据，计算所述目标物体相对于所述拍摄装置的姿态投影矩阵；
将所述姿态投影矩阵确定为所述目标物体的初始位姿。
在第一方面的一种可能的实现方式中，所述目标图像对应的所述位姿数据包括所述目标图像对应的旋转矩阵和第一目标点坐标，其中，所述旋转矩阵用于表示所述目标物体相对于所述拍摄装置的姿态，所述第一目标点坐标为所述目标图像中所述目标物体的中心点坐标；
所述根据所述目标图像对应的所述位姿数据，计算所述目标物体相对于所述拍摄装置的姿态投影矩阵，包括：
获取预设的距离比例因子，其中，所述距离比例因子用于表示实际距离和所述参考距离之间的比例关系，所述实际距离为获取所述拍摄图像时所述目标物体与所述拍摄装置之间的距离；
根据所述距离比例因子和所述参考距离计算所述实际距离；
根据所述实际距离、所述第一目标点坐标和所述拍摄装置的内参数据计算第二目标点坐标，其中，所述第二目标点坐标表示获取所述目标图像时所述目标物体的中心相对于所述拍摄装置的位置；
根据所述旋转矩阵、所述实际距离和所述第二目标点坐标构建所述姿态投影矩阵。
在第一方面的一种可能的实现方式中，所述根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿，包括：
获取第一坐标数据和第二坐标数据，其中，所述第一坐标数据为获取所述拍摄图像时所述目标物体在世界坐标系下的坐标数据，所述第二坐标数据为所述拍摄图像中所述目标物体的坐标数据；
将所述第一坐标数据和所述第二坐标数据确定为常量、将所述姿态投影矩阵中的参数作为变量、将所述初始位姿中的数据作为所述变量的初始值，进行迭代计算，获得迭代后的所述姿态投影矩阵；
将迭代后的所述姿态投影矩阵确定为所述目标物体的最终位姿。
在第一方面的一种可能的实现方式中，所述将所述第一坐标数据和所述第二坐标数据确定为常量、将所述姿态投影矩阵中的参数作为变量、将所述初始位姿中的数据作为所述变量的初始值，进行迭代计算，获得迭代后的所述姿态投影矩阵，包括：
对于第一次迭代计算，根据所述初始位姿计算所述第一坐标数据在所述拍摄图像中对应的投影坐标数据；
计算所述第二坐标数据和所述投影坐标数据的差值数据；
根据预设迭代算法和所述差值数据更新所述变量的数值。
第二方面，本申请实施例提供了一种位姿估计装置，包括：
拍摄图像获取单元，用于通过拍摄装置获取目标物体的拍摄图像；
目标图像确定单元，用于将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，其中，所述预设模板库中包括所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的所述目标物体的模板图像；
初始位姿确定单元，用于根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿；
迭代计算单元，用于根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿。
第三方面，本申请实施例提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上述第一方面中任一项所述的位姿估计方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上述第一方面中任一项所述的位姿估计方法。
第五方面，本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行上述第一方面中任一项所述的位姿估计方法。
可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的预设模板库的建立方法的流程示意图；
图2是本申请实施例提供的模板图像的示意图；
图3是本申请实施例提供的位姿估计方法的流程示意图；
图4是本申请实施例提供的位姿估计装置的结构框图；
图5是本申请实施例提供的终端设备的结构示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
另外，在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
在本申请说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。
先介绍本申请实施例提供的位姿估计方法的一个应用场景。机器人要抓取水杯时，机器人的处理器先通过安装于机器人上的相机获取水杯的拍摄图像；然后利用存储于机器人的存储介质中的预设模板库和拍摄图像对水杯的位姿进行初步估计，得到水杯的初始位姿；机器人的处理器根据初始位姿进行迭代计算，获得水杯的最终位姿；然后机器人的处理器控制机器人的机械臂根据计算出的水杯的最终位姿抓取水杯。
利用存储于机器人的存储介质中的预设模板库和拍摄图像对水杯的位姿进行初步估计时，将拍摄图像与预设模板库中的模板图像相匹配，将匹配的目标图像对应的位姿数据确定为水杯的初始位姿，即确定出水杯位姿的大致范围；然后根据初始位姿进行迭代计算，获得水杯的最终位姿，即在位姿的大致范围内进一步确定出水杯的精确位姿。
如上应用场景所示，在本申请实施例提供的位姿估计方法中，首先需要建立预设模板库，再利用建立好的预设模板库进行位姿估计。下面首先介绍本申请实施例提供的预设模板库的建立方法。参见图1，是本申请实施例提供的预设模板库的建立方法的流程示意图。如图1所示，预设模板库的建立方法可以包括以下步骤：
S101，获取拍摄装置的内参数据。
拍摄装置可以为相机、摄像头等具有拍摄功能的装置。通常，拍摄装置安装于机器人的机身上。
以相机为例，可以通过对相机进行标定获取相机的内参数据。通常，内参数据包括：相机的焦距在u轴和v轴方向上的尺度因子，以及相机主点坐标。可以利用一个矩阵进行表示，如下所示：
其中，fx为相机焦距在u轴方向上的尺度因子，fy为相机焦距在v轴方向上的尺度因子(u轴和v轴构成了相机坐标系)，(u0，v0)为相机的主点坐标。
可选的，相机标定可以采用张正友标定法等现有标定方法。
利用相机的内参数据，可以将相机坐标系下的点映射到像素坐标系中。
S102，基于参考距离和内参数据，确定目标物体相对于拍摄装置的不同姿态对应的位姿数据。
其中，所述参考距离为预设的所述目标物体与所述拍摄装置之间的距离。参考距离可以是人为预先设定的。
在获得多组位姿数据时，参考距离固定不变，相当于固定了目标物体和拍摄装置之间的距离。然后通过改变目标物体相对于拍摄装置的旋转角度，来获取不同姿态下对应的位姿数据。
示例性的，假设参考距离为Z0，令目标物体绕x轴旋转，每旋转θ度确定一组位姿数据。例如，当目标物体绕x轴旋转0度时，相应的位姿数据为(x1，y1，R1，Z0)；当目标物体绕x轴旋转θ度时，相应的位姿数据为(x1，y1，R2，Z0)。其中，(x1，y1)为目标物体在图像中的中心点坐标，R1、R2为旋转矩阵，R1到R2的转换可以表示目标物体绕x轴旋转了θ度。上述示例中，由于目标物体只是绕x轴旋转，而未发生平移，因此(x1，y1)不变。
当然，也可以令目标物体绕y轴或z轴旋转，还可以令目标物体绕两个轴旋转、绕三个轴旋转，还可以令目标物体同时平移和旋转，在此不做限定。
S103，获取每组位姿数据对应的模板图像。
示例性的，可以将目标物体放置在距离拍摄装置的参考距离处，然后变换目标物体相对于拍摄装置的姿态，并通过拍摄装置拍摄每种姿态下目标物的图像，获得模板图像。
但是，上述获取模板图像的方法步骤繁琐，不仅需要人工变换目标物体的位置，还需要拍摄多张图像，成本较高。
本申请实施例中，可以通过渲染的方式获取模板图像。具体的，对于任意一组位姿数据，获取该组位姿数据对应的模板图像的步骤可以包括：
获取该组位姿数据对应的目标物体的渲染图像；检测渲染图像中目标物体的边缘，获得目标物体的第一边缘图像；将目标物体的第一边缘图像确定为该组位姿数据对应的目标物体的模板图像。
示例性的，参见图2，是本申请实施例提供的模板图像的示意图。将内参数据载入到OpenGL中，构建一个虚拟相机；建立目标物体的三维模型(如图2中的(a)所示；三维模型可以通过如autoCAD等建模工具获取，建立的模型可以利用meshlab、Blender等工具进行可视化处理)；然后变换目标物体的三维模型相对于虚拟相机的姿态，并记录每种姿态下的位姿数据；在OpenGL中根据每种姿态下的位姿数据渲染得到相应的模板图像(如图2中的(b)所示)。
可选的，可以通过现有的边缘检测算法检测渲染图像中目标物体的边缘，如sobel、canny等边缘检测算法。
当目标物体为玻璃等纹理较浅材质的物体时，能够获取到的目标物体的特征信息(如纹理信息等)较少，导致采用特征匹配方法估计出的位姿是不准确的。本申请实施例中，获取渲染图像的边缘图像，在减少了特征点的同时，保证了特征信息的准确性。
可选的，可以将第一边缘图像进行二值化处理，得到二值化边缘图像。由于位姿估计主要关注与目标物体的姿态，而不必关注目标物体的颜色、花纹等细节特征。通过二值化处理可以有效描绘出目标物体的大致轮廓，还可以有效避免像素信息或背景信息等对位姿估计的干扰。
S104，根据所述位姿数据以及所述模板图像建立所述预设模板库。
建立的预设模板库中包括目标物体相对于拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的目标物体的模板图像。
在预设模板库中，每组位姿数据对应目标物体的一种姿态，而目标物体的每种姿态对应一张模板图像。换言之，每组位姿数据对应一张模板图像。
通过上述方法，能够快速建立预设模板库，还可以节约成本。
在建立预设模板库之后，可以利用预设模板库中的模板图像及其对应的位姿数据，估计目标物体的位姿。参见图3，是本申请实施例提供的位姿估计方法的流程示意图，作为示例而非限定，所述方法可以包括以下步骤：
S301，通过拍摄装置获取目标物体的拍摄图像。
步骤S301的实现场景是指，当需要对目标物体的位姿进行估计时，获取目标物体当前的拍摄图像。这里的目标物体是真实的物体，不是构建出的三维模型；这里的拍摄图像是对真实的目标物体进行拍摄得到的图像，不是通过OpenGL渲染出的图像。
S302，将预设模板库中与拍摄图像相匹配的模板图像确定为目标图像。
匹配的方式可以采用特征匹配方法，即选取拍摄图像与模板图像上的特征点，然后通过对特征点进行匹配，以确定拍摄图像与模板图像是否匹配。但是，采用特征匹配方法时，图像的背景图案容易对图像中的目标物体产生干扰。另外，当目标物体为玻璃等纹理较浅材质的物体时，能够获取到的目标物体的特征信息(如纹理信息等)较少，导致采用特征匹配方法得到的匹配结果可能是不准确的。因此，如图1实施例所示的预设模板库的建立方法中所述，预设模板库中的模板图像可以为渲染图像的边缘图像。这样在减少了特征点的同时，保证了特征信息的准确性。
相应的，在本申请实施例中，将拍摄图像与预设模板库中的模板图像进行匹配的方式可以包括：
检测拍摄图像中目标物体的边缘，获得目标物体的第二边缘图像；将预设模板库中与第二边缘图像相匹配的模板图像确定为目标图像。
当然，如果预设模板库中的模板图像为二值化边缘图像，那么在将拍摄图像与模板图像进行匹配的过程中，也需要将第二边缘图像进行二值化处理。换言之，对模板图像的处理过程与对拍摄图像的处理过程相同，这样才能保证匹配结果的准确性。
可选的，可以采用快速倒角匹配算法对拍摄图像与模板图像进行匹配。快速倒角匹配算法能够进行快速匹配，有利于提高位姿估计的处理效率。
S303，根据目标图像对应的位姿数据确定目标物体的初始位姿。
一种方式为，将目标图像对应的位姿数据作为目标物体的初始位姿。
但是，如图1实施例所示的预设模板库的建立方法中所述，模板图像对应的位姿数据均是基于参考距离的。而获取目标物体的拍摄图像时，目标物体与拍摄装置之间的距离可能不等于参考距离。如果直接目标图像对应的位姿数据作为目标物体的初始位姿，那么估计出的目标物体的位姿的范围是不准确的。
为了解决上述问题，本申请实施例中，确定目标物体的初始位姿的另一种方式为：根据目标图像对应的位姿数据，计算目标物体相对于拍摄装置的姿态投影矩阵；将姿态投影矩阵确定为目标物体的初始位姿。
其中，目标图像对应的位姿数据包括目标图像对应的旋转矩阵和第一目标点坐标。旋转矩阵用于表示目标物体相对于拍摄装置的姿态，第一目标点坐标为目标图像中所述目标物体的中心点坐标。
计算姿态投影矩阵的具体步骤可以包括：
I、获取预设的距离比例因子。
其中，距离比例因子用于表示实际距离和所述参考距离之间的比例关系，实际距离为获取拍摄图像时目标物体与拍摄装置之间的距离。
II、根据距离比例因子和参考距离计算实际距离。
例如：通过公式计算实际距离。Zm为实际距离，Z0为参考距离，σm为距离比例因子。
根据相机拍摄原理中近大远小的规律，获取距离比例因子的方式可以为：当目标物体距离拍摄装置第一距离时，拍摄目标物体的第一图像；当目标物体距离拍摄装置第二距离时，拍摄目标物体的第二图像；计算第一图像中目标物体的面积与第二图像中目标物体的面积的比值，该比值可作为距离比例因子。
III、根据实际距离、第一目标点坐标和拍摄装置的内参数据计算第二目标点坐标，其中，第二目标点坐标表示获取目标图像时目标物体的中心相对于拍摄装置的位置。
例如：通过下式计算第二目标点坐标：
其中，(Xm,Ym)为第二目标点坐标，(xm,ym)为第一目标点坐标，(u0，v0)为拍摄装置的内参数据中的相机主点坐标，fx为拍摄装置的内参数据中相机焦距在u轴方向上的尺度因子，fy为相机焦距在v轴方向上的尺度因子，Zm为实际距离。
IV、根据旋转矩阵、实际距离和第二目标点坐标构建姿态投影矩阵。
可以将旋转矩阵中的数据、实际距离和第二目标点坐标串联组合成向量，也可以组合成n×n的矩阵，如下式：
其中，Pm为姿态投影矩阵，Rm为旋转矩阵，01×3为一行三列的0矩阵。
将数据融合到一个矩阵中，便于后续的投影计算。
从该矩阵中可以看出，姿态投影矩阵相当于描述了目标物体相对于拍摄装置的姿态转换关系，即目标物体相对于拍摄装置的位姿。
S304，根据初始位姿进行迭代计算，获得目标物体的最终位姿。
迭代计算的基本思想是逐次逼近，先取一个粗糙的近似值作为初始值，然后用一个递推公式，反复校正初始值，直至达到预设精度。
具体的，步骤S304可以包括：
获取第一坐标数据和第二坐标数据；将第一坐标数据和第二坐标数据确定为常量、将姿态投影矩阵中的参数作为变量、将初始位姿中的数据作为变量的初始值，进行迭代计算，获得迭代后的姿态投影矩阵；将迭代后的姿态投影矩阵确定为目标物体的最终位姿。
其中，第一坐标数据为获取拍摄图像时目标物体在世界坐标系下的坐标数据，第二坐标数据为拍摄图像中目标物体的坐标数据。第一坐标数据中包括的坐标点与第二坐标数据中包括的坐标点相对应。
对于第一次迭代计算，根据初始位姿计算第一坐标数据在拍摄图像中对应的投影坐标数据；计算第二坐标数据和投影坐标数据的差值数据；根据预设迭代算法和差值数据更新变量的数值。
对于第M次迭代计算，根据第M-1次迭代计算更新后的变量的数值计算第一坐标数据在拍摄图像中对应的投影坐标数据；计算第二坐标数据和投影坐标数据的差值数据；根据预设迭代算法和差值数据更新变量的数值。
在本申请实施例中，第二坐标数据和投影坐标数据的差值数据可以为，第二坐标数据和投影坐标数据差值的二范数。例如：其中，pi为第二坐标数据中第i个坐标点的坐标，Pi为第一坐标数据中第i个坐标点的坐标，pm为姿态投影矩阵，N为坐标点的个数；Proj(Pi；pm)为投影函数，该函数的输出结果为投影坐标数据，即通过该函数能够根据姿态投影矩阵计算第一坐标数据在拍摄图像中对应的投影坐标数据。pi对应的坐标点与Pi对应的坐标点相匹配。
可选的，预设迭代算法可以为最小二乘法、梯度下降法或最速下降法等迭代算法。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
对应于上文实施例所述的方法，图4是本申请实施例提供的位姿估计装置的结构框图，为了便于说明，仅示出了与本申请实施例相关的部分。
参照图4，该装置包括：
拍摄图像获取单元41，用于通过拍摄装置获取目标物体的拍摄图像。
目标图像确定单元42，用于将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，其中，所述预设模板库中包括所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的所述目标物体的模板图像。
初始位姿确定单元43，用于根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿。
迭代计算单元44，用于根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿。
可选的，装置4还包括：
内参数据获取单元45，用于获取所述拍摄装置的内参数据。
位姿数据确定单元46，用于基于参考距离和所述内参数据，确定所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据，其中，所述参考距离为预设的所述目标物体与所述拍摄装置之间的距离。
模板图像获取单元47，用于获取每组所述位姿数据对应的模板图像。
模板库建立单元48，用于根据所述位姿数据以及所述模板图像建立所述预设模板库。
可选的，模板图像获取单元还用于：
对于每组所述位姿数据，获取所述位姿数据对应的所述目标物体的渲染图像；检测所述渲染图像中所述目标物体的边缘，获得所述目标物体的第一边缘图像；将所述目标物体的第一边缘图像确定为所述位姿数据对应的所述目标物体的模板图像。
可选的，目标图像确定单元42还用于：
检测所述拍摄图像中所述目标物体的边缘，获得所述目标物体的第二边缘图像；将所述预设模板库中与所述第二边缘图像相匹配的模板图像确定为所述目标图像。
可选的，初始位姿确定单元43还用于：
根据所述目标图像对应的所述位姿数据，计算所述目标物体相对于所述拍摄装置的姿态投影矩阵；将所述姿态投影矩阵确定为所述目标物体的初始位姿。
可选的，所述目标图像对应的所述位姿数据包括所述目标图像对应的旋转矩阵和第一目标点坐标，其中，所述旋转矩阵用于表示所述目标物体相对于所述拍摄装置的姿态，所述第一目标点坐标为所述目标图像中所述目标物体的中心点坐标。
可选的，初始位姿确定单元43还用于：
获取预设的距离比例因子，其中，所述距离比例因子用于表示实际距离和所述参考距离之间的比例关系，所述实际距离为获取所述拍摄图像时所述目标物体与所述拍摄装置之间的距离；根据所述距离比例因子和所述参考距离计算所述实际距离；根据所述实际距离、所述第一目标点坐标和所述拍摄装置的内参数据计算第二目标点坐标，其中，所述第二目标点坐标表示获取所述目标图像时所述目标物体的中心相对于所述拍摄装置的位置；根据所述旋转矩阵、所述实际距离和所述第二目标点坐标构建所述姿态投影矩阵。
可选的，迭代计算单元44还用于：
获取第一坐标数据和第二坐标数据，其中，所述第一坐标数据为获取所述拍摄图像时所述目标物体在世界坐标系下的坐标数据，所述第二坐标数据为所述拍摄图像中所述目标物体的坐标数据；将所述第一坐标数据和所述第二坐标数据确定为常量、将所述姿态投影矩阵中的参数作为变量、将所述初始位姿中的数据作为所述变量的初始值，进行迭代计算，获得迭代后的所述姿态投影矩阵；将迭代后的所述姿态投影矩阵确定为所述目标物体的最终位姿。
可选的，迭代计算单元44还用于：
对于第一次迭代计算，根据所述初始位姿计算所述第一坐标数据在所述拍摄图像中对应的投影坐标数据；计算所述第二坐标数据和所述投影坐标数据的差值数据；根据预设迭代算法和所述差值数据更新所述变量的数值。
需要说明的是，上述装置/单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。
另外，图4所示的装置可以是内置于现有的终端设备内的软件单元、硬件单元、或软硬结合的单元，也可以作为独立的挂件集成到所述终端设备中，还可以作为独立的终端设备存在。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
图5是本申请实施例提供的终端设备的结构示意图。如图5所示，该实施例的终端设备5包括：至少一个处理器50(图5中仅示出一个)处理器、存储器51以及存储在所述存储器51中并可在所述至少一个处理器50上运行的计算机程序52，所述处理器50执行所述计算机程序52时实现上述任意各个方法实施例中的步骤。
所述终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。该终端设备可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，图5仅仅是终端设备5的举例，并不构成对终端设备5的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。
所称处理器50可以是中央处理单元(Central Processing Unit，CPU)，该处理器50还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器51在一些实施例中可以是所述终端设备5的内部存储单元，例如终端设备5的硬盘或内存。所述存储器51在另一些实施例中也可以是所述终端设备5的外部存储设备，例如所述终端设备5上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器51还可以既包括所述终端设备5的内部存储单元也包括外部存储设备。所述存储器51用于存储操作系统、应用程序、引导装载程序(Boot Loader)、数据以及其他程序等，例如所述计算机程序的程序代码等。所述存储器51还可以用于暂时地存储已经输出或者将要输出的数据。
本申请实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现可实现上述各个方法实施例中的步骤。
本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行时实现可实现上述各个方法实施例中的步骤。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质至少可以包括：能够将计算机程序代码携带到装置/终端设备的任何实体或装置、记录介质、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random AccessMemory)、电载波信号、电信信号以及软件分发介质。例如U盘、移动硬盘、磁碟或者光盘等。在某些司法管辖区，根据立法和专利实践，计算机可读介质不可以是电载波信号和电信信号。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。
Claims (9)
1.一种位姿估计方法，其特征在于，包括：
通过拍摄装置获取目标物体的拍摄图像；
将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，其中，所述预设模板库中包括所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据以及每组位姿数据对应的所述目标物体的模板图像；
根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿；
根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿；
所述获取每组所述位姿数据对应的模板图像，包括：
对于每组所述位姿数据，获取所述位姿数据对应的所述目标物体的渲染图像；
检测所述渲染图像中所述目标物体的边缘，获得所述目标物体的第一边缘图像；
将所述目标物体的第一边缘图像确定为所述位姿数据对应的所述目标物体的模板图像。
2.如权利要求1所述的位姿估计方法，其特征在于，所述方法还包括：
获取所述拍摄装置的内参数据；
基于参考距离和所述内参数据，确定所述目标物体相对于所述拍摄装置的不同姿态对应的位姿数据，其中，所述参考距离为预设的所述目标物体与所述拍摄装置之间的距离；
获取每组所述位姿数据对应的模板图像；
根据所述位姿数据以及所述模板图像建立所述预设模板库。
3.如权利要求1所述的位姿估计方法，其特征在于，所述将预设模板库中与所述拍摄图像相匹配的模板图像确定为目标图像，包括：
检测所述拍摄图像中所述目标物体的边缘，获得所述目标物体的第二边缘图像；
将所述预设模板库中与所述第二边缘图像相匹配的模板图像确定为所述目标图像。
4.如权利要求2所述的位姿估计方法，其特征在于，所述根据所述目标图像对应的所述位姿数据确定所述目标物体的初始位姿，包括：
根据所述目标图像对应的所述位姿数据，计算所述目标物体相对于所述拍摄装置的姿态投影矩阵；
将所述姿态投影矩阵确定为所述目标物体的初始位姿。
5.如权利要求4所述的位姿估计方法，其特征在于，所述目标图像对应的所述位姿数据包括所述目标图像对应的旋转矩阵和第一目标点坐标，其中，所述旋转矩阵用于表示所述目标物体相对于所述拍摄装置的姿态，所述第一目标点坐标为所述目标图像中所述目标物体的中心点坐标；
所述根据所述目标图像对应的所述位姿数据，计算所述目标物体相对于所述拍摄装置的姿态投影矩阵，包括：
获取预设的距离比例因子，其中，所述距离比例因子用于表示实际距离和所述参考距离之间的比例关系，所述实际距离为获取所述拍摄图像时所述目标物体与所述拍摄装置之间的距离；
根据所述距离比例因子和所述参考距离计算所述实际距离；
根据所述实际距离、所述第一目标点坐标和所述拍摄装置的内参数据计算第二目标点坐标，其中，所述第二目标点坐标表示获取所述目标图像时所述目标物体的中心相对于所述拍摄装置的位置；
根据所述旋转矩阵、所述实际距离和所述第二目标点坐标构建所述姿态投影矩阵。
6.如权利要求4或5所述的位姿估计方法，其特征在于，所述根据所述初始位姿进行迭代计算，获得所述目标物体的最终位姿，包括：
获取第一坐标数据和第二坐标数据，其中，所述第一坐标数据为获取所述拍摄图像时所述目标物体在世界坐标系下的坐标数据，所述第二坐标数据为所述拍摄图像中所述目标物体的坐标数据；
将所述第一坐标数据和所述第二坐标数据确定为常量、将所述姿态投影矩阵中的参数作为变量、将所述初始位姿中的数据作为所述变量的初始值，进行迭代计算，获得迭代后的所述姿态投影矩阵；
将迭代后的所述姿态投影矩阵确定为所述目标物体的最终位姿。
7.如权利要求6所述的位姿估计方法，其特征在于，所述将所述第一坐标数据和所述第二坐标数据确定为常量、将所述姿态投影矩阵中的参数作为变量、将所述初始位姿中的数据作为所述变量的初始值，进行迭代计算，获得迭代后的所述姿态投影矩阵，包括：
对于第一次迭代计算，根据所述初始位姿计算所述第一坐标数据在所述拍摄图像中对应的投影坐标数据；
计算所述第二坐标数据和所述投影坐标数据的差值数据；
根据预设迭代算法和所述差值数据更新所述变量的数值。
8.一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的方法。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011398886.1A CN112686950B (zh) | 2020-12-04 | 2020-12-04 | 位姿估计方法、装置、终端设备及计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011398886.1A CN112686950B (zh) | 2020-12-04 | 2020-12-04 | 位姿估计方法、装置、终端设备及计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112686950A CN112686950A (zh) | 2021-04-20 |
| CN112686950B true CN112686950B (zh) | 2023-12-15 |
Family
ID=75445810
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011398886.1A Active CN112686950B (zh) | 2020-12-04 | 2020-12-04 | 位姿估计方法、装置、终端设备及计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112686950B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113393134B (zh) * | 2021-06-21 | 2023-11-21 | 广东三维家信息科技有限公司 | 数控多排钻的工序确定方法、装置、电子设备及存储介质 |
| CN115497087B (zh) * | 2022-11-18 | 2024-04-19 | 广州煌牌自动设备有限公司 | 一种餐具姿态的识别系统及其方法 |
| CN115922404B (zh) * | 2023-01-28 | 2024-04-12 | 中冶赛迪技术研究中心有限公司 | 一种拆卸方法、系统、电子设备及存储介质 |
| CN116071438B (zh) * | 2023-03-06 | 2023-06-27 | 航天宏图信息技术股份有限公司 | 一种无人机RigCamera影像的增量SfM方法及装置 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104596502A (zh) * | 2015-01-23 | 2015-05-06 | 浙江大学 | 一种基于cad模型与单目视觉的物体位姿测量方法 |
| CN109712172A (zh) * | 2018-12-28 | 2019-05-03 | 哈尔滨工业大学 | 一种初始位姿测量结合目标追踪的位姿测量方法 |
| CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台（上海）信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110047108A (zh) * | 2019-03-07 | 2019-07-23 | 中国科学院深圳先进技术研究院 | 无人机位姿确定方法、装置、计算机设备及存储介质 |
| CN110648361A (zh) * | 2019-09-06 | 2020-01-03 | 深圳市华汉伟业科技有限公司 | 一种三维目标物体的实时位姿估计方法及定位抓取系统 |
| WO2020038386A1 (zh) * | 2018-08-22 | 2020-02-27 | 杭州萤石软件有限公司 | 确定单目视觉重建中的尺度因子 |
| CN111311632A (zh) * | 2018-12-11 | 2020-06-19 | 深圳市优必选科技有限公司 | 一种物体位姿跟踪方法、装置及设备 |
| CN111311679A (zh) * | 2020-01-31 | 2020-06-19 | 武汉大学 | 一种基于深度相机的自由漂浮目标位姿估计方法 |
-
2020
- 2020-12-04 CN CN202011398886.1A patent/CN112686950B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104596502A (zh) * | 2015-01-23 | 2015-05-06 | 浙江大学 | 一种基于cad模型与单目视觉的物体位姿测量方法 |
| WO2020038386A1 (zh) * | 2018-08-22 | 2020-02-27 | 杭州萤石软件有限公司 | 确定单目视觉重建中的尺度因子 |
| CN111311632A (zh) * | 2018-12-11 | 2020-06-19 | 深圳市优必选科技有限公司 | 一种物体位姿跟踪方法、装置及设备 |
| CN109712172A (zh) * | 2018-12-28 | 2019-05-03 | 哈尔滨工业大学 | 一种初始位姿测量结合目标追踪的位姿测量方法 |
| CN109887003A (zh) * | 2019-01-23 | 2019-06-14 | 亮风台（上海）信息科技有限公司 | 一种用于进行三维跟踪初始化的方法与设备 |
| CN109974693A (zh) * | 2019-01-31 | 2019-07-05 | 中国科学院深圳先进技术研究院 | 无人机定位方法、装置、计算机设备及存储介质 |
| CN110047108A (zh) * | 2019-03-07 | 2019-07-23 | 中国科学院深圳先进技术研究院 | 无人机位姿确定方法、装置、计算机设备及存储介质 |
| CN110648361A (zh) * | 2019-09-06 | 2020-01-03 | 深圳市华汉伟业科技有限公司 | 一种三维目标物体的实时位姿估计方法及定位抓取系统 |
| CN111311679A (zh) * | 2020-01-31 | 2020-06-19 | 武汉大学 | 一种基于深度相机的自由漂浮目标位姿估计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112686950A (zh) | 2021-04-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112686950B (zh) | 位姿估计方法、装置、终端设备及计算机可读存储介质 | |
| CN107223269B (zh) | 三维场景定位方法和装置 | |
| CN109118545B (zh) | 基于旋转轴和双目摄像头的三维成像系统标定方法及系统 | |
| JP3735344B2 (ja) | キャリブレーション装置、キャリブレーション方法、及びキャリブレーション用プログラム | |
| CN108381549B (zh) | 一种双目视觉引导机器人快速抓取方法、装置及存储介质 | |
| CN111179339A (zh) | 基于三角测量的坐标定位方法、装置、设备及存储介质 | |
| CN106530358A (zh) | 仅用两幅场景图像标定ptz摄像机的方法 | |
| CN108182708B (zh) | 一种双目相机的标定方法、标定装置及终端设备 | |
| CN114119864A (zh) | 一种基于三维重建与点云匹配的定位方法和装置 | |
| CN110807807B (zh) | 一种单目视觉的目标定位的图案、方法、装置及设备 | |
| CN111951335A (zh) | 确定相机标定参数的方法、装置、处理器和图像采集系统 | |
| CN111583342A (zh) | 一种基于双目视觉的目标快速定位方法及装置 | |
| CN113362445B (zh) | 基于点云数据重建对象的方法及装置 | |
| CN110673607A (zh) | 动态场景下的特征点提取方法、装置、及终端设备 | |
| CN112197708B (zh) | 测量方法及装置、电子设备及存储介质 | |
| CN111161348A (zh) | 一种基于单目相机的物体位姿估计方法、装置及设备 | |
| CN111383264B (zh) | 一种定位方法、装置、终端及计算机存储介质 | |
| CN112102378A (zh) | 图像配准方法、装置、终端设备及计算机可读存储介质 | |
| CN111336938A (zh) | 一种机器人及其物体距离检测方法和装置 | |
| CN111223139B (zh) | 目标定位方法及终端设备 | |
| CN115131273A (zh) | 信息处理方法、测距方法及装置 | |
| CN111583317B (zh) | 图像对齐方法、装置及终端设备 | |
| CN116503387B (zh) | 图像检测方法、装置、设备、系统及可读存储介质 | |
| CN111383262A (zh) | 遮挡检测方法、系统、电子终端以及存储介质 | |
| CN111462309B (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 |