一种面向6D位姿估计的3D关键点选择方法

本发明涉及位姿估计，尤其涉及一种面向6d位姿估计的3d关键点选择方法。

背景技术：

1、物体的6d位姿估计(6d pose estimation)是计算机视觉领域的一个关键任务，它和许多快速发展的技术领域密切相关，如自动驾驶、机器人抓取和混合现实技术。6d位姿估计是物体坐标系和与视觉传感器(相机)坐标系之间的刚体坐标变换，由3-doftranslation与3-dof rotation组成，简称6-dof(degree of freedom)位姿。

2、解决6d位姿估计问题的方法目前主要分为三类方法，分别是基于模板的方法、基于坐标的方法和基于关键点的方法。在基于模板的刚体位姿估计方法中，通过建立目标刚体特征数据库，对输入数据进行特征提取，最后与特征库中的特征进行比对，选取匹配度最高的特征作为最优特征来恢复目标刚体的位姿信息。基于坐标的刚体位姿估计方法通过对目标刚体三维模型坐标进行标注，然后训练网络对处理后的图像像素进行坐标预测，获取2d-3d的空间映射，最后使用ransac和pnp(perspective-n-point)算法或训练网络来恢复目标刚体位姿。基于关键点的刚体位姿估计方法通过对已知目标刚体的3d模型进行2d投影并选取关键点进行标注，训练网络对输入图像进行关键点预测，从而获取3d-2d的空间映射，最后使用pnp基本算法或训练网络来恢复目标刚体位姿。其中基于关键点的方法已经被证明了是6d位姿估计解决方案中性能最好的一种。

3、目前6d位姿估计中常用的3d关键点选择算法包括bbox(bounding box)和最远点采样fps(farthest point sampling)。bbox的方法通常用于目标检测和物体位姿估计任务中，其基本思想是用一个矩形框(bounding box)来框住目标物体，并且用矩形框的位置和大小来描述物体的位置和边界。bbox方法的优点是简单直观，易于理解和实现。但是bbox方法也有局限性：它可能不够精确地描述物体的形状，特别是对于非矩形物体或有遮挡的物体。fps是一种常用的关键点选择方法，fps方法的基本思想是从点云或曲面上选择一组关键点，以确保它们在空间上尽可能分散，从而能够较好地表示原始数据。fps的流程是通过对点云进行启发式搜索，不断地将关键点添加到维护的数组当中。例如在pvnet这篇论文中，作者通过添加对象中心来初始化关键点集，然后反复找到物体表面上距离当前关键点集合最远的点，并将其添加到集合中，直到集合的大小达到k。pvnet的作者还通过实验证明该策略产生的结果比使用bbox的方法更好。fps方法的优点是简单高效，并且能够在保持数据表示能力的同时，减少关键点的数量。fps方法也存在一些局限性：当数据集具有较高的维度时，计算距离矩阵的成本可能会很高。此外，fps方法可能会导致关键点集合中的点之间存在较大的空白区域，这可能会导致在某些情况下丢失细节信息。

4、近年来，有部分工作尝试采用数据驱动的方法来进行关键点选择。这些方法利用深度神经网络来学习从图像中提取关键点的特征表示，并根据这些特征来选择关键点。这种方法通常能够更好地适应不同类型的物体和场景，使得运用该关键点检测算法的6d位姿估计模型的准确性和鲁棒性上有所提升。

5、过去的关键点选取方法通常采用启发式搜索，例如fps算法或者基于bbox的几个角点的方法，它们仅基于对象的3d表面几何形状，并忽略其他外观信息(例如颜色)。并且这两种方式存在一定的缺点，对于fps算法来说，当点云的数量过多的时候，大量的搜索空间也会限制关键点选择的性能。对于bbox的方法来说，往往得到的边界框不能很好的描述物体的形状。这种启发式方法可能不一定能够捕获最具辨别力的关键点以进行准确的姿态估计。另外这两种方法在应对形状变化(如遮挡、对称等)的时候，由于适应性较差可能会导致对象的关键点选择不理想，进而影响6d位姿估计的准确性。

技术实现思路

1、针对现有技术的不足，本发明提供一种面向6d位姿估计的3d关键点选择方法，用于从.ply格式的3d模型中提取关键点。

2、一种面向6d位姿估计的3d关键点选择方法，具体步骤如下：

3、步骤1：使用.ply格式的3d模型生成渲染若干个角度的图像并使用sift算法提取2d关键点；

4、步骤1.1：定义load_mesh函数，加载.ply格式的3d模型并准备渲染多视角的rgbd图片；所述load_mesh函数的输出是一个包含模型数据的字典meshdic，其中模型数据包含模型的顶点数据、模型面数据、颜色信息和模型缩放比例，即scale2m参数；

5、步骤1.2：生成相机到对象的多个变换矩阵，模拟相机多角度拍摄物体。定义函数util.camerapositon，计算相机在球面上的位置，并使用函数util.getcamerapose函数计算得到相机的位姿，输出相机的平移和旋转矩阵，最后使用util.get_o2c_pose函数得到相机到3d对象的变换矩阵；给定相机的旋转平移矩阵rt，使用仿射变换函数np.dot函数，改变观察物体的视角，模拟相机拍摄物体时的情况；执行过程中，先使用步骤1.1中的load_mesh函数得到模型的xyz顶点坐标，接着使用np.dot(xyz,r.t)+t，将模型的顶点坐标从模型坐标系或世界坐标系变换到相机坐标系，得到新的顶点坐标xyz_camera。

6、步骤1.3：利用物体位姿生成2d投影信息和深度信息，并使用自定义渲染器生成渲染图像和深度缓冲区；使用相机的内参矩阵k将变换后的3d顶点坐标xyz_camera投影到2d图像平面上，使用np.dot函数，得到每个顶点的2d像素坐标；同时，保留这些顶点在相机坐标系中的z值，即深度信息zs；最后通过调用renderer.rgbzbuffer函数，传入上述的2d像素坐标、深度信息、颜色信息以及模型面信息，来生成rgb图像和深度缓冲区。

7、步骤1.4：设置generate_sift_feature方法提取渲染图像中的2d关键点；使用cv2.imread()函数加载图像img，加载的时候以灰度模式cv2.imread_grayscale加载，调用cv2.xfeatures2d.sift_create()创建一个sift特征提取器，命名为siftextractor，使用siftextractor内置的方法detectandcompute(bgr,none)方法检测图像中的关键点，最后返回一个关键点的列表kps和关键点对应的描述符des，kps中包含了每个关键点的像素坐标、尺度、方向、强度和图像金字塔层级信息，des是一个二维numpy数组，其中每行代表一个关键点的描述符。

8、步骤2：基于针孔成像原理，整合每个.ply格式的3d模型的多视角2d关键点，获取纹理化的3d点云；

9、步骤2.1：声明两个数组varray和uarray，存储2d关键点的像素坐标，声明zarray存储像素对应的深度值；

10、步骤2.2：基于针孔成像原理，根据相机的内参k计算像素对应的空间坐标；使用相机的内参k，结合像素坐标和深度值，利用相机模型的几何关系，计算每个像素点在三维空间中的坐标；

11、所述针孔成像原理如下所示；设u、v为图像坐标系下的任意坐标点；u0、v0为图像坐标系中的主点坐标；xw、yw、zw表示世界坐标系下的三维坐标点；zc表示相机坐标的z轴值，即目标到相机的距离；r、t分别为外参矩阵的3×3旋转矩阵和3×1平移矩阵；f为相机的焦距；dx、dy为图像坐标系中的像素尺寸：

12、

13、默认相机远点和世界坐标远点重合，即没有平移和旋转，相机坐标和世界坐标下的同一个物体具有相同的深度，得到点云的三维坐标：

14、

15、步骤2.3：整合点云数据；

16、将计算得到的三维坐标(xw,yw,zw)和rgb图像中对应的颜色值整合成点云数据；(xw,yw,zw)分别代表点云中的x、y、z坐标，而rgb[vs,us,0]、rgb[vs,us,1]、rgb[vs,us,2]分别代表对应点的r、g、b值，其中vs、us代表图像坐标系中水平和竖直方向的像素坐标。

17、步骤3：设计关键点提取网络keypointnet提取关键点的信息，实现3d关键点提取网络提取点云中的关键点。

18、步骤3.1：构建get_point_graph_feature函数，处理输入的点云数据，生成具有图形结构的特征表示，用于表示点云数据中每个点与其最近邻点之间的关系；

19、具体为，根据输入的点云数据，计算每个点的最近邻点的索引；接着利用最近邻点索引，从原始的点云数据中提取特征，最后将提取的特征与原始点云数据进行组合，构建出具有图形结构的特征表示。

20、步骤3.1.1定义方法参数；具体为：x表示输入的点云数据，是一个大小为(batch_size,num_dims,num_points)的张量，其中batch_size表示批量大小，num_dims表示每个点的维度，num_points表示点的数量；k用于计算每个点的最近邻的数量；idx是最近邻点的索引。

21、步骤3.1.2：动态计算最近邻点的索引；

22、首先，获取输入数据的批量大小和点的数量，并将输入数据重新排列成(batch_size,num_dims,num_points)的形状；如果没有提供最近邻点的索引idx，则调用knn函数计算每个点的k个最近邻点的索引，knn用于计算每个点的最近邻点的索引，具体为：对于每个点，计算其与所有其他点之间的欧几里得距离，然后根据距离找到每个点的k个最近邻点的索引；

23、所述欧几里得距离的计算如下：假设有两个点a和b，它们的坐标分别是(x1,y1,z1)和(x2,y2,z2)，则他们之间的欧几里得距离distance表示如下：

24、distance2＝(x1-x2)2+(y1-y2)2+(z1-z2)2

25、寻找每个点最邻近k个点的方法是pytorch提供的def topk(self,…)方法，该方法是pytorch中的一个张量操作，用于获取张量沿指定维度的最大的k个元素及其对应的索引；具体来说，对于输入张量，它会返回两个张量，分别是最大的k个元素的值和对应的索引；

26、步骤3.1.3：根据计算出的最近邻点的索引，将其转换为在批处理中的全局索引；

27、将输入的点云数据x转置为(batch_size,num_points,num_dims)的形状，接下来根据最近邻点的索引从原始输入数据中提取特征，通过将原始数据重复k次，并与最近邻索引相乘，实现特征提取；最后将提取的特征与原始数据通过torch.cat方法拼接起来，得到最终的图形特征；所述特征为点与其最近邻点的差值，所述图形特征feature，其形状为(batch_size,2*num_dims,num_points,k)。

28、步骤3.2：定义关键点提取网络keypointnet的超参数；

29、其中，self.output_channels存储了输出通道数，根据关键点的数量确定；self.keypointsnum存储了关键点的数量。

30、步骤3.3：定义关键点提取网络keypointnet；在__init__方法中，调用了super(keygnet,self).__init__()来初始化父类；关键点提取网络keypointnet首先定义了5个归一化层，输入特征图的大小分别是32、64、128、256和相应的嵌入维度args.emb_dims，接着定义5个卷积层self.conv1、self.conv2、self.conv3、self.conv4、self.conv5，每个卷积层使用nn.sequential进行封装，封装模块为一个卷积层、一个批归一化层和一个leakyrelu激活函数，第一个卷积层的输入是12*32，核大小为1；最后一个卷积层的输入是480*emb_dims，核大小为1；用于从输入数据中提取特征；然后定义三个线性层，从提取的特征中进行进一步的转换和预测，其中前两个线性层后面均连接正则化层和dropout层，分别用于稳定模型训练和防止过拟合；第一个线性层的输入大小为args.emb_dims*2，输出大小为512；第二个线性层的输入大小为512，输出大小为256；最后一个线性层的输入大小为256，输出大小为self.output_channels。

31、步骤3.4：定义网络的前向传播过程forward方法，即关键点提取网络keypointnet的架构，通过构建动态图获取特征，然后进行卷积操作，最终输出关键点的坐标；

32、所述forward方法接收一个张量x_input作为输入，并获取该输入的批量大小batch size。然后调用get_point_graph_feature函数，用于生成图形特征，得到的图形特征输入卷积层self.conv1进行处理，得到特征x1，然后，将x1作为输入，再次调用get_point_graph_feature函数生成新的图形特征，并经过self.conv2处理得到x2，通过self.conv3和self.conv4重复了两次，生成x3和x4，得到四个不同的特征；接下来使用torch.cat方法将x1-x4四个特征进行拼接，得到一个更加丰富的特征x，最后经过一个卷积层self.conv5进行处理，然后，通过自适应最大池化和自适应平均池化，对特征进行池化操作，并将结果展平成一维张量x1和x2，将其拼接起来，得到最终的特征表示x_output；关键点提取网络keypointnet的最后通过线性层和激活函数进一步处理x_output，并进行reshape操作，将其形状调整为(batch_size,self.keypointsno,3)，即每个批次中包含self.keypointsno个关键点，每个关键点由三个坐标值表示；最终的结果返回，即为关键点的预测结果。

33、步骤4：对关键点提取网络keypointnet进行训练。

34、步骤4.1：定义关键点的距离损失wassersteinloss；基于改进的wassgan-gp，具体使用现有的梯度惩罚思想，并增加了距离损失wasserstein，进行关键点投票：将点云的每个表面点向关键点发出的投票视为一个分布，投票则根据这两者之间的距离进行编码，距离的计算方式根据模型的投票方式决定；最后使用他们的投票分布计算wasserstein距离；通过最小化wasserstein距离，最小化网络预测的关键点投票分布与其他关键点分布之间的差异；

35、步骤4.1.1：定义wassersteinloss的参数；具体包括：wassersteinloss损失函数输入是关键点张量、点云数据、投票类型和梯度惩罚的权重系数，输出为wasserstein距离损失；其中关键点张量input由关键点提取网络keypointnet预测得到，形状为[b,n,3]，表示每个批次中的关键点的坐标，b表示批次大小，n表示每个样本中的关键点数量；点云数据texturepointcloud的形状为[b,n,3]，表示每个批次中的分割点云的坐标，b表示批次大小，n表示每个点云样本中包含的n个点；基于关键点投票进行6d位姿估计，通过检测和匹配关键点，并利用投票机制确定物体在三维空间中的位置和方向；由于对关键点投票的方式不一样，所以定义了vote_type用于判断投票的类型，如果投票类型是径向投票，则计算关键点与点云之间的欧几里得距离；如果投票类型是向量投票，则计算关键点与点云之间的向量；如果投票类型是偏移投票，则计算关键点与点云之间的偏移，定义λ为梯度惩罚的权重系数，最后定义istraining判断关键点提取网络keypointnet是否处于训练过程，如果处于训练过程则使用梯度惩罚；

36、步骤4.1.2：wassersteinloss的计算流程；

37、具体为：距离损失wassersteinloss在接收到输入张量后，初始化批次大小b，关键点数量n和点云的数量n。迭代每个批次的关键点，根据关键点投票的类型vote_type计算其与点云之间的距离，得到一个n*n的矩阵normals，它存储了每个关键点与点云中每个点之间的距离；计算所有关键点对之间的距离后，对矩阵normals逐行按照升序将这些距离进行排序，最终矩阵normals的每一行代表一个关键点与点云中所有点之间的距离，并且已经被排序；接下来，循环遍历从第一个关键点到倒数第二个关键点的每一个关键点，j是当前关键点的索引，对于每一个j，循环遍历从j之后的关键点到最后一个关键点的每一个关键点m，使用torch.mean(torch.abs(normal[j]-normal[k]))计算每对关键点j和m之间的平均绝对差值，这个值作为关键点对之间wasserstein距离的近似值，累加到wassersteinsum中；在所有的关键点对之间的wasserstein距离计算并累加之后，将wassersteinsum除以关键点对的数量iter，得到所有关键点对之间的平均wasserstein距离；如果关键点提取网络keypointnet处于训练过程中，wassersteinloss则会将对点云进行随机重排序，并计算梯度惩罚；具体方式是打乱点云的排序，让keypointnet对打乱后的点云进行前向传播，并计算梯度的范数减1的平方的平均值乘以再乘以λ；最后，将梯度惩罚gradient penalty添加到wasserstein距离中；其中，lwass指的是wassersteinloss的数值，即wasserstein损失；iter是迭代次数；|normals[j]和normals[k]是计算得到的每个关键点到其他关键点的距离，指示归一化距离向量；是网络的输出相对于样本的梯度；表示其欧几里得范数：

38、

39、步骤4.2：定义关键点的分散度损失dispersionloss；添加一个分散损失，dispersionloss的公式如下所示，ldispersion指的是关键点分散度损失dispersionloss的值，表示关键点在3d空间中的分散度；iter指迭代次数；γ是距离的缩放因子，γ较大时会对距离较近的关键点对施加更大的惩罚；pi、pj分别代表第i个和第j个关键点的坐标。

40、

41、具体实现中，首先遍历输入的每个关键点，对于每个关键点i，通过双重循环遍历关键点j，得到关键点对，确保每对关键点只计算一次距离，对于每对关键点(i,j)，计算它们之间的欧氏距离；对于每对关键点之间的距离，应用指数函数和gamma系数进行调整，即torch.exp(gamma*distance)；最后将所有计算出的调整后的距离值累加，并通过总计算次数iter求平均，得到最终的分散损失并返回。

42、步骤4.3：计算关键点提取网络的总损失ltotal；

43、最终关键点提取网络的损失为，ltotal表示wasserstein损失和分散度损失的加权和，lwass表示步骤4.1中的wassersteinloss，ldispersion表示步骤4.2中的分散度损失dispersionloss，α表示wassersteinloss损失的权重系数，β表示分散度损失的权重系数。

44、ltotal＝αlwass+βldispersion

45、步骤4.4：训练流程：

46、建立测试数据集，测试数据集包含多物体6d姿态估计数据；

47、关键点提取网络keypointnet使用的优化器是sgd，学习率设置为0.001；在前50个周期中，设置α和β的值为0.7和0.3，之后的周期设置为0.3和0.7；训练时，每个完整训练过程epoch都执行以下步骤：

48、步骤s1：遍历训练数据加载器，获取批次数据；

49、步骤s2：将批次数据送入关键点提取网络keypointnet进行前向传播，计算损失；

50、步骤s3：根据损失进行反向传播和优化器的参数更新；

51、步骤s4：将损失记录到可视化工具tensorboard中；

52、步骤s5：保存模型的检查点；

53、所述训练过程中，每隔5个训练周期进行一次测试，测试的时候执行以下步骤：

54、步骤d1：测试过程遍历测试数据加载器，获取批次数据，但是在测试过程中，模型的参数不会被更新；此时初始化最佳测试损失，将其设置为0。

55、步骤d2：计算测试数据集上的损失，并将其记录到tensorboard中。

56、步骤d3：根据测试损失调整学习率；如果测试损失优于当前最佳测试损失，更新最佳测试损失，并保存当前模型。

57、步骤4.5：关键点选择结果；在训练结束后，加载最佳模型；将步骤2中得到的纹理化的3d点云数据输入最佳模型中，得到关键点的预测结果。最后将关键点结果反归一化，并保存到文件中。

58、采用上述技术方案所产生的有益效果在于：

59、本发明提供一种面向6d位姿估计的3d关键点选择方法，以解决传统方法中关键点选择的不足之处。本方法结合了深度学习网络和传统的2d关键点选择方法，设计了一个名为keypointnet的神经网络模型，用于从点云数据中提取关键点信息。本发明结合了wasserstein距离和分散损失，以便更好地优化关键点的选择，从而提高位姿估计模型的准确性。本方法能有效克服以往关键点选择算法中依赖几何信息而忽略纹理信息的问题；另外，设计的3d关键点提取网络keypointnet还引入了wasserstein距离和分散度损失，极大地优化了关键点的选择过程，提高了位姿估计的准确性和鲁棒性；最后本发明还通过梯度惩罚策略加速了模型训练，保证了训练过程的稳定性。通过本发明选择的3d关键点，能够更好的保留物体的几何特征，最终提高了位姿估计的性能。综上，本发明在基于关键点的6d位姿估计任务中具有显著的效果和实用价值。

技术特征：

1.一种面向6d位姿估计的3d关键点选择方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤1包括以下步骤：

3.根据权利要求1所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤2包括以下步骤：

4.根据权利要求1所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤3包括以下步骤：

5.根据权利要求4所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤3.1包括以下步骤：

6.根据权利要求1所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤4包括以下步骤：

7.根据权利要求6所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述步骤4.1包括以下步骤：

8.根据权利要求6所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，步骤4.4所述训练流程中，每个完整训练过程epoch都执行以下步骤：

9.根据权利要求8所述的一种面向6d位姿估计的3d关键点选择方法，其特征在于，所述训练过程中，每隔5个训练周期进行一次测试，测试的时候执行以下步骤：

技术总结
本发明提供一种面向6D位姿估计的3D关键点选择方法，涉及位姿估计技术领域，首先，该方法加载并渲染.ply格式的3D模型，使用SIFT算法从渲染的图像中提取2D关键点，这种方案解决了以往关键点选择算法中仅依赖几何信息而忽略纹理信息的问题。然后，将2D关键点提升到3D空间中，得到纹理化的3D点云。最后，设计了一个名为KeyPointNet的神经网络模型，该模型结合了2D关键点选择方法与深度学习的优势，用于从点云数据中提取关键点信息。本发明还引入了Wasserstein损失和分散度损失，综合考虑了关键点之间的几何分散性和投票分布，以改进关键点的选择的准确性和鲁棒性，进而提高物体6D位姿估计的准确率。

技术研发人员：高天寒,王富新,江欣蓓,朱子辰,郭艳
受保护的技术使用者：东北大学
技术研发日：
技术公布日：2024/9/23