CN114310951B - 位姿优化方法、装置、抓取设备和计算机可读存储介质 - Google Patents
位姿优化方法、装置、抓取设备和计算机可读存储介质 Download PDFInfo
- Publication number
- CN114310951B CN114310951B CN202111659480.9A CN202111659480A CN114310951B CN 114310951 B CN114310951 B CN 114310951B CN 202111659480 A CN202111659480 A CN 202111659480A CN 114310951 B CN114310951 B CN 114310951B
- Authority
- CN
- China
- Prior art keywords
- target object
- point cloud
- cloud data
- target
- point
- 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
- 238000005457 optimization Methods 0.000 title claims abstract description 63
- 238000000034 method Methods 0.000 title claims abstract description 55
- 239000013598 vector Substances 0.000 claims description 44
- 238000011176 pooling Methods 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 28
- 238000004422 calculation algorithm Methods 0.000 claims description 22
- 230000009466 transformation Effects 0.000 claims description 13
- 230000001131 transforming effect Effects 0.000 claims description 13
- 238000013528 artificial neural network Methods 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 12
- 238000004891 communication Methods 0.000 claims description 10
- 238000000605 extraction Methods 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 10
- 210000000078 claw Anatomy 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 9
- 230000006870 function Effects 0.000 description 35
- 238000012545 processing Methods 0.000 description 23
- 238000013527 convolutional neural network Methods 0.000 description 22
- 230000008569 process Effects 0.000 description 20
- 238000010586 diagram Methods 0.000 description 12
- 230000004913 activation Effects 0.000 description 8
- 238000011946 reduction process Methods 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 6
- 230000009467 reduction Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 239000007787 solid Substances 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000007635 classification algorithm Methods 0.000 description 2
- 239000000284 extract Substances 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Image Analysis (AREA)
Abstract
本发明实施例提出一种位姿优化方法、装置、抓取设备和计算机可读存储介质，涉及计算机技术领域。该方法由于利用了目标物体在前一时刻的优化位姿，则将目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征输入预设的位姿估计网络时，即可获取目标物体在当前时刻的估计位姿相对于真实位姿的相对位姿，进而根据相对位姿进行优化，获得目标物体在当前时刻的优化位姿，即目标物体在当前时刻的真实位姿，避免了初始化位姿精度不高时，引起的局部最优的问题，并且对位姿进行优化时，仅使用了点云数据，相对于现有技术，推理速度更快。
Description
技术领域
本发明涉及计算机技术领域，具体而言，涉及一种位姿优化方法、装置、抓取设备和计算机可读存储介质。
背景技术
六维位姿跟踪指根据目标物体的初始位姿和相机获取的输入数据持续地估计目标物体的位姿。刚体的六维位姿跟踪主要应用于机器人抓取和增强现实等领域，其中，在机器人抓取领域，随着目标物体的位姿变化，机器人能够实时的跟踪目标物体的位姿，进而计算抓取时机械爪的位姿，实现闭环抓取。
在现有技术中，刚体六维位姿跟踪方法主要包含两个阶段，首先对目标物体的位姿进行粗估计，然后通过优化算法进行迭代优化。常用的优化算法为迭代最近点算法(Iterative Closest Point，ICP)，即通过不断迭代最小化源数据与目标数据的对应点距离，以此来实现目标数据和源数据精确地拼合。
但是，现有的位姿优化技术在优化位姿时需要多次迭代，多次迭代过程耗时长，位姿跟踪技术往往需要实时跟踪，同时，迭代最近点算法对初始位姿的精确性要求高，如果初始化位姿精度较差，基于迭代最近点算法的优化技术容易产生局部最优化问题。
发明内容
有鉴于此，本发明的目的在于提供一种位姿优化方法、装置、抓取设备和计算机可读存储介质，以解决现有技术中，初始化位姿精度较差时，引起的局部最优解问题。
为了实现上述目的，本发明实施例采用的技术方案如下：
第一方面，本发明提供一种位姿优化方法，所述方法包括：
根据目标物体在当前时刻的图像，获取所述目标物体在当前时刻的估计位姿和点云数据；
根据所述估计位姿对所述目标物体在当前时刻的点云数据进行变换，得到所述目标物体在当前时刻的目标点云数据；
根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进行变换，得到所述目标物体在前一时刻的目标点云数据；
提取所述目标物体在当前时刻的目标点云数据对应的全局特征，以及提取所述目标物体在前一时刻的目标点云数据对应的全局特征；
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取所述目标物体在当前时刻的相对位姿；
根据所述相对位姿对所述估计位姿进行优化，以获得所述目标物体在当前时刻的优化位姿。
在可选的实施方式中，所述估计位姿和所述目标物体在前一时刻的优化位姿中均包括旋转矩阵和偏移矩阵，所述根据所述估计位姿对所述目标物体在当前时刻的点云数据进行变换，得到所述目标物体在当前时刻的目标点云数据，包括：
根据所述估计位姿的旋转矩阵和偏移矩阵，对所述目标物体在当前时刻的点云数据进行旋转和平移变化，以得到所述目标物体在当前时刻的目标点云数据；
所述根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进行变换，得到所述目标物体在前一时刻的目标点云数据，包括：
根据所述目标物体在前一时刻的优化位姿的旋转矩阵和偏移矩阵，对所述目标物体在前一时刻的点云数据进行旋转和平移变化，以得到所述目标物体在前一时刻的目标点云数据。
在可选的实施方式中，所述提取所述目标物体在当前时刻的目标点云数据对应的全局特征，包括：
根据预设的近邻算法，获取所述目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点；
针对所述目标物体在当前时刻的目标点云数据中的每个点，计算所述预设数量个点分别到所述每个点的边向量和法向量；
根据所述目标物体在当前时刻的目标点云数据中的每个点、所述每个点对应的预设数量个点以及所述每个点对应的边向量和法向量，计算所述目标物体在当前时刻的目标点云数据中的每个点的初始点云特征；
对所述目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得所述目标物体在当前时刻的目标点云数据中的每个点的点云特征；
对所述目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在当前时刻的目标点云数据对应的全局特征。
在可选的实施方式中，所述对所述目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在当前时刻的目标点云数据对应的全局特征，包括：
计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的平均值，获得所述目标物体在当前时刻的目标点云数据中的每个点的平均特征；
利用预设的第一卷积神经网络对所述目标物体在当前时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得所述目标物体在当前时刻的目标点云数据中的每个点的非线性特征；
根据预设的第二卷积神经网络和所述目标物体在当前时刻的目标点云数据中的每个点的非线性特征，计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的权重；
将所述目标物体在当前时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得所述目标物体在当前时刻的目标点云数据对应的全局特征。
在可选的实施方式中，所述提取所述目标物体在前一时刻的目标点云数据对应的全局特征，包括：
根据预设的近邻算法，获取所述目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点；
针对所述目标物体在前一时刻的目标点云数据中的每个点，计算所述预设数量个点分别到所述每个点的边向量和法向量；
根据所述目标物体在前一时刻的目标点云数据中的每个点、所述每个点对应的预设数量个点以及所述每个点对应的边向量和法向量，计算所述目标物体在前一时刻的目标点云数据中的每个点的初始点云特征；
对所述目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得所述目标物体在前一时刻的目标点云数据中的每个点的点云特征；
对所述目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在前一时刻的目标点云数据对应的全局特征。
在可选的实施方式中，所述对所述目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在前一时刻的目标点云数据对应的全局特征，包括：
计算所述目标物体在前一时刻的目标点云数据中的每个点的点云特征的平均值，获得所述目标物体在前一时刻的目标点云数据中的每个点的平均特征；
利用预设的第一卷积神经网络对所述目标物体在前一时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得所述目标物体在前一时刻的目标点云数据中的每个点的非线性特征；
根据预设的第二卷积神经网络和所述目标物体在前一时刻的目标点云数据中的每个点的非线性特征，计算所述目标物体在前一时刻的目标点云数据中的每个点的点云特征的权重；
将所述目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得所述目标物体在前一时刻的目标点云数据对应的全局特征。
在可选的实施方式中，所述预设的位姿估计网络包括位置估计网络和姿态估计网络，所述根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿优化网络，获取所述目标物体在当前时刻的相对位姿包括：
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及所述位置估计网络，获取所述目标物体在当前时刻的相对位置；
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及所述姿态估计网络，获取所述目标物体在当前时刻的相对姿态；
其中，所述相对位姿包括所述相对位置和所述相对姿态。
第二方面，本发明提供一种位姿优化装置，所述装置包括：
获取模块，用于根据目标物体在当前时刻的图像，获取所述目标物体在当前时刻的估计位姿和点云数据；
变换模块，用于根据所述估计位姿对所述目标物体在当前时刻的点云数据进行变换，得到所述目标物体在当前时刻的目标点云数据；
所述变换模块，还用于根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进行变换，得到所述目标物体在前一时刻的目标点云数据；
提取模块，用于提取所述目标物体在当前时刻的目标点云数据对应的全局特征，以及提取所述目标物体在前一时刻的目标点云数据对应的全局特征；
优化模块，用于根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取所述目标物体在当前时刻的相对位姿；
所述优化模块，还用于根据所述相对位姿对所述估计位姿进行优化，以获得所述目标物体在当前时刻的优化位姿。
第三方面，本发明提供一种抓取设备，所述抓取设备包括：上位机、相机和机械臂；所述上位机与相机和机械臂通信连接；
所述上位机包括处理器和存储器，所述存储器存储有计算机程序，所述处理器可执行所述计算机程序以实现前述实施方式任一所述的方法；
所述相机用于获取目标物体在当前时刻的图像，并发送至所述上位机；
所述机械臂用于接收所述上位机发送的抓取指令，并根据所述抓取指令对所述目标物体进行抓取。
第四方面，本发明提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如前述实施方式中任一项所述的方法。
本发明实施例提供的位姿优化方法、装置、抓取设备和计算机可读存储介质，根据目标物体在当前时刻的图像，获取目标物体在当前时刻的估计位姿和点云数据，根据估计位姿对目标物体在当前时刻的点云数据进行变换，得到目标物体在当前时刻的目标点云数据；并根据目标物体在前一时刻的优化位姿对目标物体在前一时刻的点云数据进行变换，得到目标物体在前一时刻的目标点云数据；再提取目标物体在当前时刻的目标点云数据对应的全局特征，以及提取目标物体在前一时刻的目标点云数据对应的全局特征，进而根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取目标物体在当前时刻的相对位姿，根据相对位姿对估计位姿进行优化，以获得目标物体在当前时刻的优化位姿。由于利用了目标物体在前一时刻的优化位姿，则将目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征输入预设的位姿估计网络时，即可获取目标物体在当前时刻和估计位姿相对于真实位姿的相对位姿，进而根据相对位姿进行优化，获得目标物体在当前时刻的优化位姿，即目标物体在当前时刻的真实位姿，避免了初始化位姿精度不高时，引起的局部最优的问题，并且对位姿进行优化时，仅使用了点云数据，相对于现有技术，推理速度更快。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1示出了本发明实施例提供的抓取设备的一种的组成示意图；
图2示出了本发明实施例提供的上位机的一种方框示意图；
图3示出了本发明实施例提供的位姿优化方法的一种流程示意图；
图4示出了图3中步骤S304的子步骤的一种流程示意图；
图5示出了图4中步骤S3044的子步骤的一种流程示意图；
图6示出了图4中步骤S3048的子步骤的一种流程示意图；
图7示出了图3中步骤S305的子步骤的一种流程示意图；
图8示出了本发明实施例提供的位姿优化装置的一种功能模块图。
图标：10-上位机；20-相机；30-机械臂；40-机械爪；210-存储器；220-处理器；230-通信模块；401-获取模块；402-变换模块；403-提取模块；404-优化模块。
具体实施方式
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。
因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
请参照图1，图1是本发明实施例提供的抓取设备的一种的组成示意图。如图1所述，该抓取设备包括上位机10、相机20和机械臂30和机械爪40。上位机10与相机20通信连接，也可以与相机20电性连接，上位机10与机械臂30和机械爪40通信连接，也可以与机械臂30和机械爪40电性连接，以实现数据的传输或交互。具体连接方式在发明实施例中不进行限制。
在本实施例中，相机20用于采集目标物体的图像，并将图像发送给上位机10，上位机10根据图像进行处理，以获得目标物体的真实位姿，并根据目标物体的真实位姿，获得机械臂30和机械爪40抓取目标物体时的相对位姿，以此发送抓取指令给机械臂30和机械爪40，进而实现机械臂30和机械爪40对目标物体的抓取操作；由于相机20是实时采集目标物体的图像，上位机10可以以此实时调整机械臂30和机械爪40抓取目标物体时相对位姿，当目标物体的位姿发生变化时，机械臂30和机械爪40都能够实时调整，以抓取目标物体。
请参考图2，图2是本发明实施例提供的上位机的一种方框示意图。该上位机10包括存储器210、处理器220和通信模块230。存储器210、处理器220和通信模块230各元件之间直接或间接地电性连接，以实现数据的传输或交互。例如，这些元件相互之间可通过一条过多条通讯总线或信号线实现电信连接。
需要说明的是，该上位机10可以是个人计算机(Personal Computer，PC)、工控机(Industrial Personal Computer，IPC)、分散控制系统(Distributed Control System，DCS)等，本发明实施例对此不进行限定。
其中，存储器210用于存储程序或者数据。所述存储器210可以是，但不限于，固态硬盘(Solid State Disk或Solid State Drive，SSD)，随机存取存储器(Random AccessMemory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(ProgrammableRead-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-OnlyMemory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-OnlyMemory，EEPROM)等。
处理器220可能是一种集成电路芯片，具有信号的处理能力。该处理器220可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、通用处理器(GeneralProcessing Unit，简称GPU)，网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
其中，该处理器220用于读写存储器210中存储的数据或程序，并执行相应的功能。例如，处理器220执行存储器210存储的计算机程序时，可以实现本发明各实施例揭示的位姿优化方法。
通信模块230用于通过网络建立上述上位机10与相机20、机械臂30和机械爪40之间的通信连接，并用于通过网络收发数据。
应当理解的是，图2所示的结构仅为上位机10的结构示意图，所述上位机10还可包括比图2中所示更多或者更少的组件，或者具有与图2所示不同的配置。图2中所示的各组件可以采用硬件、软件或其组合实现。
本发明实施例还提供一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时可以实现本发明各实施例揭示的位姿优化方法。
现有技术中，刚体六维位姿跟踪方法主要包含两个阶段，首先对目标物体的位姿进行粗估计，然后通过优化算法进行迭代优化。常用的优化算法为迭代最近点算法，即通过不断迭代最小化源数据与目标数据的对应点，以此来实现目标数据和源数据精确地拼合。但是，现有的位姿优化技术在优化位姿时需要多次迭代，多次迭代过程耗时长，位姿跟踪技术往往需要实时跟踪，同时，迭代最近点算法对初始位姿的精确性要求高，如果初始化位姿精度较差，基于迭代最近点算法的优化技术容易产生局部最优化问题。
基于上述问题，为了避免初始化位姿精度不高时，引起的局部最优的问题，本发明实施例提供了一种位姿优化方法，请参阅图3，图3为本发明实施例提供的位姿优化方法的一种流程示意图。该方法包括：
步骤S301，根据目标物体在当前时刻的图像，获取目标物体在当前时刻的估计位姿和点云数据。
可选的，根据相机拍摄的目标物体在当前时刻的图像，通过姿态估计(DenseFusion)网络，获取目标物体在当前时刻的估计位姿和点云数据。
需要说明的是，上述姿态估计网络的选择不受限制，还可以选择三维像素级投票网络(Pixel-wise Voting Network 3D，PVN3D)和六维单阶段多目标框检测器(SingleShot MultiBox Detector 6D，SSD-6D)等。
步骤S302，根据估计位姿对目标物体在当前时刻的点云数据进行变换，得到目标物体在当前时刻的目标点云数据。
可选的，由于根据相机拍摄的目标物体在当前时刻的图像，获取的估计位姿和点云数据是在相机坐标系下的，而为了对目标物体的位姿进行估计，需要对点云数据进行变换，即将该点云数据从相机坐标系转换到物体坐标系，以得到目标物体在当前时刻的目标点云数据。
步骤S303，根据目标物体在前一时刻的优化位姿对目标物体在前一时刻的点云数据进行变换，得到目标物体在前一时刻的目标点云数据。
同样的，也需要对前一时刻的点云数据进行变换，以得到目标物体在前一时刻的目标点云数据。
步骤S304，提取目标物体在当前时刻的目标点云数据对应的全局特征，以及提取目标物体在前一时刻的目标点云数据对应的全局特征。
可选的，由于全局特征具有良好的不变性、计算简单、表示直观等特点，在进行后续计算时，需要分别提取目标物体在当前时刻的目标点云数据对应的全局特征以及目标物体在前一时刻的目标点云数据对应的全局特征。
步骤S305，根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿优估计网络，获取目标物体在当前时刻的相对位姿。
可选的，由于利用的是目标物体在前一时刻的优化位姿，其表征了目标物体在前一时刻的真实位姿，而目标物体在当前时刻的位姿还没有进过优化，则将目标物体在前一时刻的目标点云数据对应的全局特征与目标物体在当前时刻的目标点云数据对应的全局特征进行拼接处理，拼接后反应的差异，表征了目标物体在当前时刻的估计姿态和真实姿态的差异，以该差异为位姿优化网络的输入，即可获取目标物体在当前时刻的估计位姿相对于目标物体在当前时刻的真实位姿的相对位姿。
步骤S306，根据相对位姿对估计位姿进行优化，以获得目标物体在当前时刻的优化位姿。
可选的，目标物体在当前时刻的优化位姿即是目标物体在当前时刻的真实位姿，其可通过下式计算获得：
p′t＝pt*Δpt
其中，p′t表征目标物体在当前时刻的优化位姿；pt表征目标物体在当前时刻的估计位姿；Δpt表征目标物体在当前时刻的相对位姿。
需要说明的是，目标物体在初始时刻，即t＝0时刻的位姿和点云数据为已知的，由相关人员进行设置并存储，当需要优化目标物体在t＝1时刻的位姿时，即可直接获取目标物体在t＝0时刻的相关数据，以对目标物体在t＝1时刻的位姿进行优化，并存储目标物体在t＝1时刻的优化位姿和点云数据；当需要对目标物体在t＝2时刻的位姿进行优化时，即可直接获取目标物体在t＝1时刻的相关数据，以对目标物体在t＝2时刻的位姿进行优化，并存储目标物体在t＝2时刻的优化位姿和点云数据，以此类推，即可实现对目标物体在当前时刻的位姿进行优化。可以理解的是，在对目标物体在当前时刻的位姿进行优化时，目标物体在前一时刻的优化位姿和点云数据对于当前时刻是已知的。
本发明实施例提供的位姿优化方法，根据目标物体在当前时刻的图像，获取目标物体在当前时刻的估计位姿和点云数据，根据估计位姿对目标物体在当前时刻的点云数据进行变换，得到目标物体在当前时刻的目标点云数据；并根据目标物体在前一时刻的优化位姿对目标物体在前一时刻的点云数据进行变换，得到目标物体在前一时刻的目标点云数据；再提取目标物体在当前时刻的目标点云数据对应的全局特征，以及提取目标物体在前一时刻的目标点云数据对应的全局特征，进而根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿优化网络，获取目标物体在当前时刻的相对位姿，根据相对位姿对估计位姿进行优化，以获得目标物体在当前时刻的优化位姿。由于利用了目标物体在前一时刻的优化位姿，则将目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征输入预设的位姿优化网络时，即可获取目标物体在当前时刻和估计位姿相对于真实位姿的相对位姿，进而根据相对位姿进行优化，获得目标物体在当前时刻的优化位姿，即目标物体在当前时刻的真实位姿，避免了初始化位姿精度不高时，引起的局部最优的问题，并且对位姿进行优化时，仅使用了点云数据，相对于现有技术，推理速度更快。
为了将相机坐标系的点云数据进行变换，以得到物体坐标系中的目标点云数据，上述步骤S302，包括：
根据估计位姿的旋转矩阵和偏移矩阵，对目标物体在当前时刻的点云数据进行旋转和平移变化，以得到目标物体在当前时刻的目标点云数据。
可选的，通过上述步骤S301获取的估计位姿中包括旋转矩阵和偏移矩阵，其表示为：
pt＝[Rt|Tt]
其中，pt表征目标物体在当前时刻的估计位姿；Rt表征目标物体在当前时刻的旋转矩阵；Tt表征目标物体在当前时刻的偏移矩阵。
在本实施例中，可以通过以下公式对目标物体在当前时刻的点云数据进行旋转和平移变化，以得到目标物体在当前时刻的目标点云数据：
pc′t＝pct*Rt+Tt
其中，pc′t表征目标物体在当前时刻的目标点云数据；pct表征目标物体在当前时刻的点云数据。
上述步骤S303，包括：
根据目标物体在前一时刻的优化位姿的旋转矩阵和偏移矩阵，对目标物体在前一时刻的点云数据进行旋转和平移变化，以得到目标物体在前一时刻的目标点云数据。
可选的，目标物体在前一时刻的优化位姿中也包括旋转矩阵和偏移矩阵，其表示为：
pt-1＝[Rt-1|Tt-1]
其中，pt-1表征目标物体在前一时刻的估计位姿；Rt-1表征目标物体在前一时刻的旋转矩阵；Tt-1表征目标物体在前一时刻的偏移矩阵。
在本实施例中，可以通过以下公式对目标物体在前一时刻的点云数据进行旋转和平移变化，以得到目标物体在前一时刻的目标点云数据：
pct′t-1＝pct-1*Rt-1+Tt-1
其中，pc′t-1表征目标物体在前一时刻的目标点云数据；pct-1表征目标物体在当前时刻的点云数据。
需要说明的是，上述步骤S302与步骤S303之间的执行先后顺序仅为一种示例，在实际应用中，步骤S302可能在步骤S303之前执行，也可能在步骤S303之后执行，本实施例对此不进行限制。
为了更简单、直观地进行后续计算，需要获取目标物体在当前时刻的目标点云数据的全局特征，请参阅图4，图4为图3中步骤S304的子步骤的一种流程示意图。上述步骤S304，包括：
步骤S3041，根据预设的近邻算法，获取目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点。
可选的，在本实施例中，该近邻算法为K最近邻(K-Nearest Neighbor，KNN)分类算法，对于每个点，通过将每个点的目标点云数据输入到该近邻算法中，即可获取目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点，其中，预设数量的设置可以不受限制，根据实际需要进行设置，例如可以是8、16、32个。
在本实施例中，可以通过计算欧氏距离的方式获得距离每个点最近的预设数量个点，也可以是通过计算余弦距离的方式获得距离每个点最近的预设数量个点，本发明实施例中对此不予以限定。
步骤S3042，针对目标物体在当前时刻的目标点云数据中的每个点，计算预设数量个点分别到每个点的边向量和法向量。
可选的，边向量由两个点之间的坐标相减获得，法向量由上述边向量除以该边向量的模获得。
例如，两个点分别表示为A点和B点，边向量表示为：
法向量表示为：
步骤S3043，根据目标物体在当前时刻的目标点云数据中的每个点、每个点对应的预设数量个点以及每个点对应的边向量和法向量，计算目标物体在当前时刻的目标点云数据中的每个点的初始点云特征。
可选的，目标物体在当前时刻的目标点云数据中的每个点的初始点云特征，可以由以下表达式获取得到：
其中，θ表征要学习的参数；xi表征每个点对应的目标点云数据；xjk表征目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点。
在本实施例中，上述表达式的左边为输入，输入的数据为3维；右边为输出，输入的数据为9维。将上述9维的数据进行扩充，以获得256维的目标物体在当前时刻的目标点云数据中的每个点的初始点云特征。
步骤S3044，对目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得目标物体在当前时刻的目标点云数据中的每个点的点云特征。
可选的，目标物体在当前时刻的目标点云数据中的每个点的初始点云特征可以表示为：
Kt*Ct
其中，Kt表征目标物体在当前时刻的目标点云数据中的每个点的初始点云特征的第一维度，与上述预设数量相对应；Ct表征目标物体在当前时刻的目标点云数据中的每个点的初始点云特征的第二维度，与上述256维相对应；
目标物体在当前时刻的目标点云数据中的每个点的点云特征分别可以表示为：
1*Ct
在本实施例中，需要通过池化处理，对目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行降维处理，以获取目标物体在当前时刻的目标点云数据中的每个点的点云特征，进而可以得到目标物体在当前时刻的目标点云数据中的每个点的点云特征的整体情况。其中，该池化处理的选择不受限制，例如，在本实施例中，选择最大值池化进行降维处理，在另外的一些实施例中，还可以选择平均值池化进行降维处理。
步骤S3045，对目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到目标物体在当前时刻的目标点云数据对应的全局特征。
可选的，通过加权处理，可以精准地反应出目标物体在当前时刻的目标点云数据中的每个点的点云特征，即目标物体在当前时刻的目标点云数据对应的全局特征。
同样的，还需要获取目标物体在前一时刻的目标点云数据的全局特征，请继续参阅图4，上述步骤S304，还包括：
步骤S3046，根据预设的近邻算法，获取目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点。
可选的，在本实施例中，该近邻算法为K最近邻(k-Nearest Neighbor，KNN)分类算法，对于每个点，通过将每个点的目标点云数据输入到该近邻算法中，即可获取目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点，其中，预设数量的设置可以不受限制，根据实际需要进行设置，例如可以是15、20、25个；并且，该距离可以是欧氏距离，也可以是余弦距离等，在本发明实施例中对此不予以限定。
步骤S3047，针对目标物体在前一时刻的目标点云数据中的每个点，计算预设数量个点分别到每个点的边向量和法向量。
可选的，边向量由两个点之间的坐标相减获得，法向量有上述边向量除以该边向量的模获得。
例如，两个点分别表示为A′点和B′点，边向量表示为：
法向量表示为：
步骤S3048，根据目标物体在前一时刻的目标点云数据中的每个点、每个点对应的预设数量个点以及每个点对应的边向量和法向量，计算目标物体在前一时刻的目标点云数据中的每个点的初始点云特征。
可选的，目标物体在前一时刻的目标点云数据中的每个点的点云特征，可以由以下表达式获取得到：
其中，θ表征要学习的参数；xi表征每个点对应的目标点云数据；xjk表征目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点。
在本实施例中，上述表达式的左边为输入，输入的数据为3维；右边为输出，输入的数据为9维。将上述9维的数据进行扩充，以获得256维的目标物体在前一时刻的目标点云数据中的每个点的初始点云特征。
步骤S3049，对目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得目标物体在前一时刻的目标点云数据中的每个点的点云特征。
可选的，目标物体在前一时刻的目标点云数据中的每个点的初始点云特征可以表示为：
Kt-1*Ct-1
其中，Kt-1表征目标物体在前一时刻的目标点云数据中的每个点的初始点云特征的第一维度，与上述预设数量相对应；Ct-1表征目标物体在前一时刻的目标点云数据中的每个点的初始点云特征的第二维度，与上述256维相对应；
目标物体在前一时刻的目标点云数据中的每个点的点云特征分别可以表示为：
1*Ct-1
在本实施例中，需要通过池化处理，对目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行降维处理，以获取目标物体在前一时刻的目标点云数据中的每个点的点云特征，进而可以得到目标物体在前一时刻的目标点云数据中的每个点的点云特征的整体情况。其中，该池化处理的选择不受限制，例如，在本实施例中，选择最大值池化进行降维处理，在另外的一些实施例中，还可以选择平均值池化进行降维处理。
步骤S30410，对目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到目标物体在前一时刻的目标点云数据对应的全局特征。
可选的，通过加权处理，可以精准地反应出目标物体在前一时刻的目标点云数据中的每个点的点云特征，即目标物体在前一时刻的目标点云数据对应的全局特征。
需要说明的是，图4所示的步骤S3041-S3045与步骤S3046-S30410之间的执行先后顺序仅为一种示例，在实际应用中，步骤S3041-S3045可能在步骤S3046-S30410之前执行，也可能在步骤S3046-S30410之后执行，本实施例对此不进行限制。
可选的，在步骤S3044的基础上，为了实现目标物体在当前时刻的目标点云数据中的每个点的点云特征的加权处理，请参阅图5，图5为图4中步骤S3044的子步骤的一种流程示意图。上述步骤S3044，包括：
步骤S30441，计算目标物体在当前时刻的目标点云数据中的每个点的点云特征的平均值，获得目标物体在当前时刻的目标点云数据中的每个点的平均特征。
可选的，在本实施例中，对上述目标物体在当前时刻的目标点云数据中的每个点的点云特征：1*Ct，计算平均值，进而可以反应目标物体在当前时刻的目标点云数据中的每个点的点云特征的整体情况。
步骤S30442，利用预设的第一卷积神经网络对目标物体在当前时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得目标物体在当前时刻的目标点云数据中的每个点的非线性特征。
可选的，在本实施例中，选择线性整流函数(Rectified Linear Unit,ReLU)作为该第一卷积神经网络的激活函数，以每个点的平均特征作为第一卷积神经网络输入，即可获得目标物体在当前时刻的目标点云数据中的每个点的非线性特征，通过对平均特征非线性化，可以保证第一卷积神经网络的正常运行；其中，该激活函数还可以是其他的可以使平均特征进行非线性化的函数，例如，双曲正切(tanh)函数，在本发明实施中不予以限定。
步骤S30443，根据预设的第二卷积神经网络和目标物体在当前时刻的目标点云数据中的每个点的非线性特征，计算目标物体在当前时刻的目标点云数据中的每个点的点云特征的权重。
可选的，在本实施例中，选择S型函数(Sigmoid function,S)作为该第二卷积神经网络的激活函数，以每个点的非线性特征作为第二卷积神经网络输入，其原理是给每个点的非线性特征计算了一个0-1的数，然后把这个数字乘到每个点的非线性特征上，以计算每个点的点云特征的占比，进而通过第二卷积神经网络输出目标物体在当前时刻的目标点云数据中的每个点的点云特征的权重。通过计算权重，可以获取每个点的点云特征在整体中的占比情况；其中，该激活函数还可以是其他的可以计算权重的函数，在本发明实施中不予以限定。
步骤S30444，将目标物体在当前时刻的点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得目标物体在当前时刻的目标点云数据对应的全局特征。
可选的，通过将目标物体在当前时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，以反映目标物体在当前时刻的目标点云数据中的每个点的点云特征各自的实际情况，由于此时加权后的目标物体在当前时刻的目标点云数据对应的点云特征是N维，还需要通过池化处理，对加权后的目标物体在当前时刻的目标点云数据对应的点云特征进行降维处理，以减少计算量，以及提高卷积神经网络的容错能力，进而获得目标物体在当前时刻的目标点云数据对应的全局特征，其中N表征目标物体在当前时刻的目标点云数据的数量。
其中，该池化处理的选择不受限制，例如，在本实施例中，选择最大值池化进行降维处理，在另外的一些实施例中，还可以选择平均值池化进行降维处理。
可选的，步骤S3048的基础上，为了实现目标物体在当前时刻的目标点云数据中的每个点的点云特征的加权处理，请参阅图6，图6为图4中步骤S3048的子步骤的一种流程示意图。上述步骤S3048，包括：
步骤S30481，计算目标物体在前一时刻的目标点云数据中的每个点的点云特征的平均值，获得目标物体在前一时刻的目标点云数据中的每个点的平均特征。
可选的，在本实施例中，对上述目标物体在前一时刻的目标点云数据中的每个点的点云特征：1*Ct-1，计算平均值，进而可以反应目标物体在前一时刻的目标点云数据中的每个点的点云特征的整体情况。
步骤S30482，利用预设的第一卷积神经网络对目标物体在前一时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得目标物体在前一时刻的目标点云数据中的每个点的非线性特征。
可选的，在本实施例中，选择线性整流函数(Rectified Linear Unit,ReLU)作为该第一卷积神经网络的激活函数，以每个点的平均特征作为第一卷积神经网络输入，即可获得目标物体在前一时刻的目标点云数据中的每个点的非线性特征，通过对平均特征非线性化，可以保证第一卷积神经网络的正常运行；其中，该激活函数还可以是其他的可以使平均特征进行非线性化的函数，例如，双曲正切(tanh)函数，在本发明实施中不予以限定。
可以理解的是，步骤S30483中使用到第一卷积神经网络和步骤S30443使用到第一卷积神经网络一致。
步骤S30483，根据预设的第二卷积神经网络和目标物体在前一时刻的目标点云数据中的每个点的非线性特征，计算目标物体在前一时刻的目标点云数据中的每个点的点云特征的权重。
可选的，在本实施例中，选择S型函数(Sigmoid function,S)作为该第二卷积神经网络的激活函数，以每个点的非线性特征作为第二卷积神经网络输入，其原理是给每个点的非线性特征计算了一个0-1的数，然后把这个数字乘到每个点的非线性特征上，以计算每个点的点云特征的占比，进而通过第二卷积神经网络输出目标物体在前一时刻的目标点云数据中的每个点的点云特征的权重。通过计算权重，可以获取每个点的点云特征在整体中的占比情况；其中，该激活函数还可以是其他的可以计算权重的函数，在本发明实施中不予以限定。
可以理解的是，步骤S30484中使用到第二卷积神经网络和步骤S30444使用到第二卷积神经网络一致。
步骤S30484，将目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得目标物体在前一时刻的目标点云数据对应的全局特征。
可选的，通过将目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，以反映目标物体在前一时刻的目标点云数据中的每个点的点云特征各自的实际情况，由于此时加权后的目标物体在前一时刻的目标点云数据对应的点云特征是N维，还需要通过池化处理，对加权后的目标物体在前一时刻的目标点云数据对应的点云特征进行降维处理，以减少计算量，以及提高卷积神经网络的容错能力，进而获得目标物体在前一时刻的目标点云数据对应的全局特征，其中N表征目标物体在前一时刻的目标点云数据的数量。
其中，该池化处理的选择不受限制，例如，在本实施例中，选择最大值池化进行降维处理，在另外的一些实施例中，还可以选择平均值池化进行降维处理。
可选的，在本实施例中，预设的位姿估计网络包括位置估计网络和姿态估计网络，为了准确地获取目标物体在当前时刻的相对位置，请参阅图7，图7为图3中步骤S305的子步骤的一种流程示意图。上述步骤S305，包括：
步骤S3051，根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及位置估计网络，获取目标物体在当前时刻的相对位置。
可选的，将目标物体在当前时刻的目标点云数据对应的全局特征与目标物体在前一时刻的目标点云数据对应的全局特征进行拼接，即可获得目标物体在当前时刻的估计位姿和真实位姿的差异，将该差异作为位置估计网络的输入，即可获取目标物体在当前时刻的相对位置。
在本实施例中，位置估计网络在训练时的损失函数如下：
其中，lt表征位置的损失函数；T′表征优化后的当前时刻的目标物体的位置；表征当前时刻的目标物体的真实位置。
步骤S3052，根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及姿态估计网络，获取目标物体在当前时刻的相对姿态。
可选的，将目标物体在当前时刻的目标点云数据对应的全局特征与目标物体在前一时刻的目标点云数据对应的全局特征进行拼接，即可获得目标物体在当前时刻的估计位姿和真实位姿的差异，将该差异作为姿态估计网络的输入，即可获取目标物体在当前时刻的相对姿态。
在本实施例中，姿态估计网络在训练时的损失函数如下：
其中，lr表征姿态的损失函数；R′表征优化后的当前时刻的目标物体的姿态；表征当前时刻的目标物体的真实姿态。
在本实施例中，还需要根据对上述lt损失函数和lr损失函数进行加权求和，以获得位姿的目标函数，其中，目标函数可以由下式表示：
l＝lr+Δlt
其中，l表征位姿的目标函数；λ表征lt损失函数的权重。
需要说明的是，位姿由位置和姿态组成，则相对位姿包括相对位置和相对姿态。为了执行上述实施例及各个可能的方式中的相应步骤，下面给出一种位姿优化装置的实现方式。进一步地，请参阅图8，图8为本发明实施例提供的位姿优化装置的一种功能模块图。需要说明的是，本实施例所提供的位姿优化装置，其基本原理及产生的技术效果和上述实施例相同，为简要描述，本实施例部分未提及之处，可参考上述的实施例中相应内容。该位姿优化装置包括：获取模块401、变换模块402、提取模块403和优化模块404。
可选地，上述模块可以软件或固件(Firmware)的形式存储于图2所示的存储器中或固化于该上位机10的操作系统(Operating System，OS)中，并可由图2中的处理器执行。同时，执行上述模块所需的数据、程序的代码等可以存储在存储器中。
获取模块401，用于根据目标物体在当前时刻的图像，获取目标物体在当前时刻的估计位姿和点云数据。
可以理解，该获取模块401可以执行上述步骤S301。
变换模块402，用于根据估计位姿对目标物体在当前时刻的点云数据进行变换，得到目标物体在当前时刻的目标点云数据。
可选的，上述变换模块402，具体用于根据估计位姿的旋转矩阵和偏移矩阵，对目标物体在当前时刻的点云数据进行旋转和平移变化，以得到目标物体在当前时刻的目标点云数据。
可以理解，该变换模块402可以执行上述步骤S302。
变换模块402，还用于根据目标物体在前一时刻的优化位姿对目标物体在前一时刻的点云数据进行变换，得到目标物体在前一时刻的目标点云数据。
可选的，上述变换模块402，还具体用于根据目标物体在前一时刻的优化位姿的旋转矩阵和偏移矩阵，对目标物体在前一时刻的点云数据进行旋转和平移变化，以得到目标物体在前一时刻的目标点云数据。
可以理解，该变换模块402还可以执行上述步骤S303。
提取模块403，用于提取目标物体在当前时刻的目标点云数据对应的全局特征，以及提取目标物体在前一时刻的目标点云数据对应的全局特征。
可以理解，该提取模块403可以执行上述步骤S304。
优化模块404，用于根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取目标物体在当前时刻的相对位姿。
可以理解，该优化模块404可以执行上述步骤S305。
优化模块404，还用于根据相对位姿对估计位姿进行优化，以获得目标物体在当前时刻的优化位姿。
可以理解，该优化模块404还可以执行上述步骤S306。
可选的，该提取模块403，还用于根据预设的近邻算法，获取目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点；针对目标物体在当前时刻的目标点云数据中的每个点，计算预设数量个点分别到每个点的边向量和法向量；根据目标物体在当前时刻的目标点云数据中的每个点、每个点对应的预设数量个点以及每个点对应的边向量和法向量，计算目标物体在当前时刻的目标点云数据中的每个点的初始点云特征；对目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得目标物体在当前时刻的目标点云数据中的每个点的点云特征；对目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到目标物体在当前时刻的目标点云数据对应的全局特征。
可以理解，该提取模块403还可以执行上述步骤S3041-S3045。
可选的，该提取模块403，还用于根据预设的近邻算法，获取目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点；针对目标物体在前一时刻的目标点云数据中的每个点，计算预设数量个点分别到每个点的边向量和法向量；根据目标物体在前一时刻的目标点云数据中的每个点、每个点对应的预设数量个点以及每个点对应的边向量和法向量，计算目标物体在前一时刻的目标点云数据中的每个点的初始点云特征；对目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得目标物体在前一时刻的目标点云数据中的每个点的点云特征；对目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到目标物体在前一时刻的目标点云数据对应的全局特征。
可以理解，该提取模块403还可以执行上述步骤S3046-S30410。
可选的，该提取模块403，还用于计算目标物体在当前时刻的目标点云数据中的每个点的点云特征的平均值，获得目标物体在当前时刻的目标点云数据中的每个点的平均特征；利用预设的第一卷积神经网络对目标物体在当前时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得目标物体在当前时刻的目标点云数据中的每个点的非线性特征；根据预设的第二卷积神经网络和目标物体在当前时刻的目标点云数据中的每个点的非线性特征，计算目标物体在当前时刻的目标点云数据中的每个点的点云特征的权重；将目标物体在当前时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得目标物体在当前时刻的目标点云数据对应的全局特征。
可以理解，该提取模块403还可以执行上述步骤S30441-S30444。
可选的，该提取模块403，还用于计算目标物体在前一时刻的目标点云数据中的每个点的点云特征的平均值，获得目标物体在前一时刻的目标点云数据中的每个点的平均特征；利用预设的第一卷积神经网络对目标物体在前一时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得目标物体在前一时刻的目标点云数据中的每个点的非线性特征；根据预设的第二卷积神经网络和目标物体在前一时刻的目标点云数据中的每个点的非线性特征，计算目标物体在前一时刻的目标点云数据中的每个点的点云特征的权重；将目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得目标物体在前一时刻的目标点云数据对应的全局特征。
可以理解，该提取模块403还可以执行上述步骤S30481-S30484。
可选的，该优化模块404，还用于根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及位置估计网络，获取目标物体在当前时刻的相对位置；根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及姿态估计网络，获取目标物体在当前时刻的相对姿态。
可以理解，该优化模块404还可以执行上述步骤S3051-S3052。
本发明实施例提供的位姿优化装置，获取模块根据目标物体在当前时刻的图像，获取目标物体在当前时刻的估计位姿和点云数据，以使变换模块根据估计位姿对目标物体在当前时刻的点云数据进行变换，得到目标物体在当前时刻的目标点云数据；变换模块还根据目标物体在前一时刻的优化位姿对目标物体在前一时刻的点云数据进行变换，得到目标物体在前一时刻的目标点云数据；提取模块再提取目标物体在当前时刻的目标点云数据对应的全局特征，以及提取目标物体在前一时刻的目标点云数据对应的全局特征，进而优化模块根据目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取目标物体在当前时刻的相对位姿，优化模块再根据相对位姿对估计位姿进行优化，以获得目标物体在当前时刻的优化位姿。由于利用了目标物体在前一时刻的优化位姿，则将目标物体在当前时刻的目标点云数据对应的全局特征、目标物体在前一时刻的目标点云数据对应的全局特征输入预设的位姿估计网络时，即可获取目标物体在当前时刻和估计位姿相对于真实位姿的相对位姿，进而根据相对位姿进行优化，获得目标物体在当前时刻的优化位姿，即目标物体在当前时刻的真实位姿，避免了初始化位姿精度不高时，引起的局部最优的问题，并且对位姿进行优化时，仅使用了点云数据，相对于现有技术，推理速度更快。
在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (5)
1.一种位姿优化方法，其特征在于，所述方法包括：
根据目标物体在当前时刻的图像，获取所述目标物体在当前时刻的估计位姿和点云数据；所述估计位姿包括旋转矩阵和偏移矩阵；
根据所述估计位姿对所述目标物体在当前时刻的点云数据进行变换，得到所述目标物体在当前时刻的目标点云数据，包括：根据所述估计位姿的旋转矩阵和偏移矩阵，对所述目标物体在当前时刻的点云数据进行旋转和平移变化，以得到所述目标物体在当前时刻的目标点云数据；
根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进行变换，得到所述目标物体在前一时刻的目标点云数据，包括：根据所述目标物体在前一时刻的优化位姿的旋转矩阵和偏移矩阵，对所述目标物体在前一时刻的点云数据进行旋转和平移变化，以得到所述目标物体在前一时刻的目标点云数据；
提取所述目标物体在当前时刻的目标点云数据对应的全局特征，包括：根据预设的近邻算法，获取所述目标物体在当前时刻的目标点云数据中距离每个点最近的预设数量个点；针对所述目标物体在当前时刻的目标点云数据中的每个点，计算所述预设数量个点分别到所述每个点的边向量和法向量；根据所述目标物体在当前时刻的目标点云数据中的每个点、所述每个点对应的预设数量个点以及所述每个点对应的边向量和法向量，计算所述目标物体在当前时刻的目标点云数据中的每个点的初始点云特征；对所述目标物体在当前时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得所述目标物体在当前时刻的目标点云数据中的每个点的点云特征；对所述目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在当前时刻的目标点云数据对应的全局特征；以及提取所述目标物体在前一时刻的目标点云数据对应的全局特征，包括：根据预设的近邻算法，获取所述目标物体在前一时刻的目标点云数据中距离每个点最近的预设数量个点；针对所述目标物体在前一时刻的目标点云数据中的每个点，计算所述预设数量个点分别到所述每个点的边向量和法向量；根据所述目标物体在前一时刻的目标点云数据中的每个点、所述每个点对应的预设数量个点以及所述每个点对应的边向量和法向量，计算所述目标物体在前一时刻的目标点云数据中的每个点的初始点云特征；对所述目标物体在前一时刻的目标点云数据中的每个点的初始点云特征进行池化处理，以获得所述目标物体在前一时刻的目标点云数据中的每个点的点云特征；对所述目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在前一时刻的目标点云数据对应的全局特征；
所述对所述目标物体在当前时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在当前时刻的目标点云数据对应的全局特征，包括：计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的平均值，获得所述目标物体在当前时刻的目标点云数据中的每个点的平均特征；利用预设的第一卷积神经网络对所述目标物体在当前时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得所述目标物体在当前时刻的目标点云数据中的每个点的非线性特征；根据预设的第二卷积神经网络和所述目标物体在当前时刻的目标点云数据中的每个点的非线性特征，计算所述目标物体在当前时刻的目标点云数据中的每个点的点云特征的权重；将所述目标物体在当前时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得所述目标物体在当前时刻的目标点云数据对应的全局特征；
所述对所述目标物体在前一时刻的目标点云数据中的每个点的点云特征进行加权处理，得到所述目标物体在前一时刻的目标点云数据对应的全局特征，包括：计算所述目标物体在前一时刻的目标点云数据中的每个点的点云特征的平均值，获得所述目标物体在前一时刻的目标点云数据中的每个点的平均特征；利用预设的第一卷积神经网络对所述目标物体在前一时刻的目标点云数据中的每个点的平均特征进行非线性变化，获得所述目标物体在前一时刻的目标点云数据中的每个点的非线性特征；根据预设的第二卷积神经网络和所述目标物体在前一时刻的目标点云数据中的每个点的非线性特征，计算所述目标物体在前一时刻的目标点云数据中的每个点的点云特征的权重；将所述目标物体在前一时刻的目标点云数据中的每个点的点云特征与对应的权重相乘，并对乘积结果进行池化处理，以获得所述目标物体在前一时刻的目标点云数据对应的全局特征；
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取所述目标物体在当前时刻的相对位姿；
根据所述相对位姿对所述估计位姿进行优化，以获得所述目标物体在当前时刻的优化位姿。
2.根据权利要求1所述的方法，其特征在于，所述预设的位姿估计网络包括位置估计网络和姿态估计网络，所述根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取所述目标物体在当前时刻的相对位姿包括：
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及所述位置估计网络，获取所述目标物体在当前时刻的相对位置；
根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及所述姿态估计网络，获取所述目标物体在当前时刻的相对姿态；
其中，所述相对位姿包括所述相对位置和所述相对姿态。
3.根据权利要求1所述的方法，其特征在于，所述方法还包括位姿优化装置，包括：
获取模块，用于根据目标物体在当前时刻的图像，获取所述目标物体在当前时刻的估计位姿和点云数据；
变换模块，用于根据所述估计位姿对所述目标物体在当前时刻的点云数据进行变换，得到所述目标物体在当前时刻的目标点云数据；
所述变换模块，还用于根据所述目标物体在前一时刻的优化位姿对所述目标物体在前一时刻的点云数据进行变换，得到所述目标物体在前一时刻的目标点云数据；
提取模块，用于提取所述目标物体在当前时刻的目标点云数据对应的全局特征，以及提取所述目标物体在前一时刻的目标点云数据对应的全局特征；
优化模块，用于根据所述目标物体在当前时刻的目标点云数据对应的全局特征、所述目标物体在前一时刻的目标点云数据对应的全局特征以及预设的位姿估计网络，获取所述目标物体在当前时刻的相对位姿；
所述优化模块，还用于根据所述相对位姿对所述估计位姿进行优化，以获得所述目标物体在当前时刻的优化位姿。
4.一种抓取设备，其特征在于，所述抓取设备包括：上位机、相机、机械臂和机械爪；所述上位机与相机和机械臂通信连接；
所述上位机包括处理器和存储器，所述存储器存储有计算机程序，所述处理器可执行所述计算机程序以实现权利要求1或2所述的方法；
所述相机用于获取目标物体在当前时刻的图像，并发送至所述上位机；
所述机械臂和所述机械爪用于接收所述上位机发送的抓取指令，并根据所述抓取指令对所述目标物体进行抓取。
5.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1或2所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111659480.9A CN114310951B (zh) | 2021-12-31 | 2021-12-31 | 位姿优化方法、装置、抓取设备和计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111659480.9A CN114310951B (zh) | 2021-12-31 | 2021-12-31 | 位姿优化方法、装置、抓取设备和计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114310951A CN114310951A (zh) | 2022-04-12 |
| CN114310951B true CN114310951B (zh) | 2024-04-26 |
Family
ID=81019199
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111659480.9A Active CN114310951B (zh) | 2021-12-31 | 2021-12-31 | 位姿优化方法、装置、抓取设备和计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114310951B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201111871D0 (en) * | 2011-07-11 | 2011-08-24 | Toshiba Res Europ Ltd | An image processing method and system |
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| US10024664B1 (en) * | 2014-09-30 | 2018-07-17 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Range and intensity image-based terrain and vehicle relative pose estimation system |
| CN111046948A (zh) * | 2019-12-10 | 2020-04-21 | 浙江大学 | 点云仿真和深度学习的工件位姿识别及机器人上料方法 |
| CN111442722A (zh) * | 2020-03-26 | 2020-07-24 | 达闼科技成都有限公司 | 定位方法、装置、存储介质及电子设备 |
| CN111540015A (zh) * | 2020-04-27 | 2020-08-14 | 深圳南方德尔汽车电子有限公司 | 基于点云匹配的位姿计算方法、装置、计算机设备及存储介质 |
| CN112509050A (zh) * | 2020-12-18 | 2021-03-16 | 武汉库柏特科技有限公司 | 一种位姿估计方法、防碰撞抓物方法和装置 |
| CN112651944A (zh) * | 2020-12-28 | 2021-04-13 | 哈尔滨工业大学（深圳） | 基于cad模型的3c部件高精度六维位姿估计方法及系统 |
| WO2021125578A1 (ko) * | 2019-12-16 | 2021-06-24 | 네이버랩스 주식회사 | 시각 정보 처리 기반의 위치 인식 방법 및 시스템 |
| CN113126115A (zh) * | 2021-04-06 | 2021-07-16 | 北京航空航天大学杭州创新研究院 | 基于点云的语义slam方法、装置、电子设备和存储介质 |
| CN113239726A (zh) * | 2021-04-06 | 2021-08-10 | 北京航空航天大学杭州创新研究院 | 基于着色点云的目标检测方法、装置和电子设备 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10962355B2 (en) * | 2017-12-25 | 2021-03-30 | Htc Corporation | 3D model reconstruction method, electronic device, and non-transitory computer readable storage medium thereof |
-
2021
- 2021-12-31 CN CN202111659480.9A patent/CN114310951B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201111871D0 (en) * | 2011-07-11 | 2011-08-24 | Toshiba Res Europ Ltd | An image processing method and system |
| US10024664B1 (en) * | 2014-09-30 | 2018-07-17 | The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration | Range and intensity image-based terrain and vehicle relative pose estimation system |
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| CN111046948A (zh) * | 2019-12-10 | 2020-04-21 | 浙江大学 | 点云仿真和深度学习的工件位姿识别及机器人上料方法 |
| WO2021125578A1 (ko) * | 2019-12-16 | 2021-06-24 | 네이버랩스 주식회사 | 시각 정보 처리 기반의 위치 인식 방법 및 시스템 |
| CN111442722A (zh) * | 2020-03-26 | 2020-07-24 | 达闼科技成都有限公司 | 定位方法、装置、存储介质及电子设备 |
| CN111540015A (zh) * | 2020-04-27 | 2020-08-14 | 深圳南方德尔汽车电子有限公司 | 基于点云匹配的位姿计算方法、装置、计算机设备及存储介质 |
| CN112509050A (zh) * | 2020-12-18 | 2021-03-16 | 武汉库柏特科技有限公司 | 一种位姿估计方法、防碰撞抓物方法和装置 |
| CN112651944A (zh) * | 2020-12-28 | 2021-04-13 | 哈尔滨工业大学（深圳） | 基于cad模型的3c部件高精度六维位姿估计方法及系统 |
| CN113126115A (zh) * | 2021-04-06 | 2021-07-16 | 北京航空航天大学杭州创新研究院 | 基于点云的语义slam方法、装置、电子设备和存储介质 |
| CN113239726A (zh) * | 2021-04-06 | 2021-08-10 | 北京航空航天大学杭州创新研究院 | 基于着色点云的目标检测方法、装置和电子设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114310951A (zh) | 2022-04-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11331806B2 (en) | Robot control method and apparatus and robot using the same | |
| EP3304492B1 (en) | Modelling a three-dimensional space | |
| CN110782483B (zh) | 基于分布式相机网络的多视图多目标跟踪方法及系统 | |
| Li et al. | Unscented Kalman filter for visual curve tracking | |
| US11960259B2 (en) | Control system using autoencoder | |
| EP2352128B1 (en) | Mobile body detection method and mobile body detection apparatus | |
| US20220262093A1 (en) | Object detection method and system, and non-transitory computer-readable medium | |
| Zhou et al. | Divide and conquer: Efficient density-based tracking of 3D sensors in Manhattan worlds | |
| CN112085789A (zh) | 位姿估计方法、装置、设备及介质 | |
| Zhu et al. | 6-dof contrastive grasp proposal network | |
| US20220277581A1 (en) | Hand pose estimation method, device and storage medium | |
| CN113793370B (zh) | 三维点云配准方法、装置、电子设备及可读介质 | |
| CN114565668A (zh) | 即时定位与建图方法及装置 | |
| CN114310951B (zh) | 位姿优化方法、装置、抓取设备和计算机可读存储介质 | |
| Lin et al. | A novel robust algorithm for position and orientation detection based on cascaded deep neural network | |
| CN111665490B (zh) | 目标跟踪方法和装置、存储介质及电子装置 | |
| CN114119999B (zh) | 基于深度学习的迭代6d位姿估计方法及装置 | |
| Krüger et al. | Accumulation of object representations utilising interaction of robot action and perception | |
| CN112184766B (zh) | 一种对象的跟踪方法、装置、计算机设备和存储介质 | |
| Fei et al. | Robust visual tracking based on improved perceptual hashing for robot vision | |
| CN113409373B (zh) | 图像处理方法及相关终端、设备和存储介质 | |
| Yong et al. | A Robust CoS-PVNet Pose Estimation Network in Complex Scenarios | |
| Dhassi et al. | Robust visual tracking based on adaptive gradient descent optimization of a cost function with parametric models of appearance and geometry | |
| US8818036B1 (en) | Contextual behavior state filter for sensor registration | |
| CN113409365B (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 |