CN118212268A - 一种基于软对应估计的类级别三维物体六维姿态跟踪方法 - Google Patents

Abstract

本发明涉及一种基于软对应估计的类级别三维物体六维姿态跟踪方法，属于计算机视觉领域，在数据预处理模块，对视频流的第一帧中的目标物体进行位姿估计，得到目标物体的初始位姿，初始化后，对深度视频流的物体进行连续的六维姿态跟踪。在对应估计模块中，提取来自不同帧的点云数据的几何特征并估计不同点云之间点与点的对应关系。在对应扩展模块中，针对估计得到的对应关系进行对应扩张，使每一个点所关联的点的数量尽可能少。在基准帧模块中，随着时间的推进根据标准更新基准帧，以减小跟踪过程中的累计误差。本发明解决了在仅有深度视频流和初始帧检测和分割结果作为输入的情况下，对同一类别但此前从未见过的物体进行六维姿态跟踪。

Description

一种基于软对应估计的类级别三维物体六维姿态跟踪方法

技术领域

本发明涉及一种基于软对应估计的类级别三维物体六维姿态跟踪方法，属于计算机视觉技术领域。

背景技术

在增强现实/混合现实应用中，三维物***姿跟踪模型的训练时间、跟踪的精确性以及流畅性是影响应用程序便利性的主要因素。基于深度神经网络的位姿跟踪方法在性能上表现出了优秀的性能，但是其需要针对特定的物体模型进行预先训练这一特点增加了其应用成本。

现有的大多数方法使用形状先验来替换传统跟踪算法中的实例级别模型，并试图在匹配过程中使形状先验变形以近似当前实例对象的形状。基于先验的方法可以在一定程度上部分捕捉类别内的变化，但它们在训练阶段需要大量的3D模型，并且可能难以完全考虑所有类别内的形状变化，由于形状先验无法完全达到实例级别模型的精度，导致其在应用中无法得到与实例级别跟踪同样的准确度。

发明内容

为了解决现有技术的不足，本发明提供一种基于软对应估计的类级别三维物体六维姿态跟踪方法，解决了在仅有深度视频流和初始帧检测和分割结果作为输入的情况下，对同一类别但此前从未见过的物体进行六维姿态跟踪。不需要针对特定的实例进行数据收集和模型参数的重新训练。

本发明的技术方案如下：

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，包括如下步骤：

S1：在数据预处理模块，通过现有相机标定方法预先获得相机内参，通过相机内参和深度视频流作为输入，反投影计算每一帧画面对应的三维点云数据，将来自两个不同输入点云分别进行均值化处理，以保证在训练阶段网络能够有效地估计视频帧之间的相对位置和姿态变化；

同时，选择深度视频流的第一帧图像，使用现有的目标检测方法检测目标物体所在区域，得到覆盖该区域的分割掩膜M_init(也被称为mask)，并使用现有的类级别单帧位姿估计方法估计目标物体的六维姿态作为初始位姿，初始化过程结束；

S2：在帧间姿态估计模块中，使用三层感知机(MLP)提取S1获得的三维点云数据的几何特征，来自两帧的点云经过多层感知机得到两个特征向量，两个特征向量通过叉乘以计算不同特征向量之间的相似度，该相似度的表示形式为矩阵，称为对应矩阵，表示不同点之间的对应关系；

在训练过程中设计了软对应约束：使用已知的对应关系生成真实的对应矩阵，在真实对应矩阵的约束下训练特征提取网络的参数以估计正确的对应矩阵；

S3：在对应扩张模块中，使用k双向对应来控制对应矩阵中对应关系的分散程度，随着迭代次数增加逐渐缩小k的值，使对应矩阵映射后的原点云逐渐接近目标点云的形状。

优选的，在均值化时需要点云的中心点才能进行，如果简单使用点云的均值点作为中心点，当分割结果存在误差时，可能会导致背景点错误地包含在观测点云中，进而影响点云的标准化过程和中心位置的确定，从而影响网络在学习特征过程中的效果。为了解决这一问题，S1中均值化处理时，将来自两帧点云的所有点作为整体共同计算均值点作为中心点，从而有效避免背景点对点云中心定位的干扰；

S1中，初始位姿初始化结束后，从输入的深度视频流中选择当前需要匹配的两帧深度图像，通过深度图像与分割掩膜可以得到每个目标像素点的深度值z，由于相机是预先标定过的，相机的内部参数矩阵K为已知，能够获得目标二维像素点(u,v)在相机坐标系下反投影得到的三维点(x,y,z)，如下公式所示：

优选的，S2中每层感知机由两个线性层与一个ReLU层组成，两个特征向量叉乘后，使用现有的sinkhorn算法迭代进行行列归一化，sinkhorn算法优选迭代5次，每次按顺序分别对矩阵的行和列进行归一化操作，最终得到表示对应关系的对应矩阵，反映不同点之间的特征的相似度。

通过利用前一视频帧中物体的已知位置数据，以及相机的运动数据，本发明创新性地提出一种方法，用于实现对当前视频帧的精确深度图像分割，由于物体和相机的运动是连续的，S1中假设在连续的视频帧之间物体相对于相机的位置变化是微小的，忽略不计，首先，使用前一帧，即第i帧的分割掩膜M_i(在初始情况下为初始分割掩膜M_init)，获取目标物体在前一视频帧深度图像D_t-1上的像素坐标；在当前帧，即第j帧的深度图像上生成一系列不同大小的二维矩形包围盒B_j,k；随后，通过预设的步长(一般为1个像素)逐步扩大矩形包围盒B_i,k，并将其应用于当前帧的图像处理中，通过综合所有生成的分割结果，并选取其中置信度评分最高的分割结果作为当前帧j上最优的分割结果B_j，确保这些包围盒能够准确覆盖到目标物体；最后，矩形包围盒B_i,k作为输入数据，配合目标检测方法获得最终用于当前帧的分割掩膜M_j。

优选的，S2中的几何特征通过特征提取模块实现，使用点对特征(Point-PairFeature,PPF)来描述点云中每个点周围的邻域，以便以旋转不变的方式表示每个点的局部几何信息；特征提取模块立在PointNet++之上，以点对特征作为输入，并输出每个点云的局部几何特征F_i和F_j，局部几何特征F_i和F_j随后被送入特征匹配模块以估计点云之间的软对应关系。

优选的，S2中，使用三层感知机提取不同帧的相机坐标系下观测点云P_i和P_j的特征F_i和F_j，通过计算两特征向量的叉乘并行列归一化来得到两组特征之间的相似度得分，最终表示为对应矩阵A，通过对应矩阵A_ij建立第j帧的点云P_j的点通与第i帧的点云P_i的点的软映射关系，反之亦然；A_i ^T _j为对应矩阵A_ij的转置，如公式所示：

P_j＝A_ijP_j

在训练阶段，归一化对应矩阵A的行列，建立三维空间点之间的一对一、一对多、多对一的“软对应”关系，而非严格的一对一的“硬对应”关系；

在训练阶段，已知的信息包括：每一帧的点云的在标准物体坐标空间(NormalizedObject Coordinate Space,NOCS)下的对应点云N_i和N_j，标准物体坐标空间变换到相机坐标空间的相对位姿变换T_i和T_j，则：

P_i＝T_iN_i

P_j＝T_jN_j

通过位姿变换T_i和T_j可以建立标准物体坐标空间与相机坐标空间的联系，标准物体坐标空间下的点云N_i和N_j与相机坐标空间下的观测点云P_i和P_j中的点是一一对应的，在标准坐标空间下，点云N_i和N_j的对应来自同一物体的点，且朝向和位置是相同的，能够获得N_i和N_j之间点的对应关系，进而获得观测点云P_i和P_j点的对应关系。

当估计得到对应矩阵后，其软对应性质会使得一个点云中的每个点在另一个点云中的权重一般不会集中在单个点上，而是分散在多个点上。据此，对应矩阵A_ij能够在点云P_j的坐标空间中相对于P_i生成一个对应点云P_c。对应矩阵中的每一个元素都代表一个权重，这些权重的取值范围在0到1之间，且其总和为1，表明所有可能的权重组合生成的对应点云P_c都包含在点云P的空间内，并基于点云P中的点以加权方式组合而成，该空间可以被视为点云P中点的形状的多种变化。

基于软对应的特性导致权重在多点之间分散的情况，本发明提出了一种新策略“对应扩展”，该策略旨在逐步减小每一个点在另一点云中相关的区域。通过减少计算对应点时涉及的点数，来集中关联点的分布，该过程同样导致由矩阵A_ij和点云P表示的空间的逐渐缩小，使得生成的对应点云更加贴近点云P的形状，从而有效地解决了“点云收缩”的问题。

优选的，S3中采用k双向对应控制对应矩阵A中对应关系的规模，利用点云N_i和N_j处于同一坐标空间下且属于同一物体的不同部分这一特性，使用最近邻(knn)算法为每一个点p_a寻找半径r内的k个点p_a,k,k＝1,2,…,k作为其关联点；当来自点云N_i的点p_a是来自点云N_j的点p_b的关联点，同时p_b也是点p_a的关联点时，这组点(p_a,p_b)被视为相互关联的点对；

生成与对应矩阵A同样大小的矩阵A_gt，将其中相互关联的点对对应的位置设置为1，其他位置设置为0；使用KL散度约束对应矩阵A_gt与估计的对应矩阵A_ij，使帧间位姿估计模块能够估计出表示帧间点云软对应关系的软对应矩阵。

优选的，使用双向对应距离(Bidirectional Correspondence Distance，BCD)进一步估计对应的置信度分数。通过对两组点云的特征相似性矩阵S进行转置，能够估计表示为对应矩阵A_ij和A_ji的双向对应关系，从而实现两组点云间相对姿态的估计。当对应关系为硬对应时，A_ij和A_ji是互逆的。然而，当对应关系为软对应并表示为软对应矩阵时，由于权重分散和行列归一化的影响，A_ij和A_ji通常不形成互逆关系。

通过利用双向对应关系，可以识别出权重分布过度分散的点。如上所述，点云P_i中的一个点P_c对应于点云P_j中的多个点P_c,k，并且当P_c,k在空间上分散时，在对应矩阵D中A_ij和A_ji中相应的点在P_j点云中将会非常远离那个点。基于这一现象，当对对应矩阵A_ij和A_ji进行逐点加权计算时，在空间距离上，生成的点p^′ _c在P_i中也会远离P_c的原始位置。

BCD_i＝P_i-A_ijA_jiP_i

BCD_i表示第i帧点云的双向对应距离值。

所有的关联点对都对应着权重集中在特定区域的点，它们的双向对应距离BCD较小。这是因为这样的点的对应关系接近于硬对应，它们的对应矩阵线性组合受到的点数很少的影响。因此两个对应矩阵的组合本质上不改变它们的空间位置。

借助软对应矩阵估计可以准确地估计连续帧之间的相对姿态，在持续匹配过程中错误的累积是不可避免的。这些累积误差会逐渐将跟踪结果引向错误的方向，最终导致跟踪失败。这也是无法将点云注册方法直接应用于跟踪任务的原因之一。为了克服累积误差问题，本发明提出了一种参考帧策略，在跟踪开始时，初始帧的数据被保存为参考帧，在计算连续两帧之间的相对姿态，相邻帧之间的相对变换被应用于参考帧；然后，这一变换被应用于参考帧的点云；随后，参考帧被变换到当前帧的坐标***中；参考帧和当前帧的点云被视为相邻帧，并估计软对应矩阵来确定相对姿态；最后，多个阶段的姿态估计结果被累积，得出当前图像帧中目标物体的最终估计。参考帧策略用于在一定时间间隔内匹配帧，从而减少误差累积并提供更准确的姿态跟踪结果。

优选的，S2的训练过程中，通过标准物体坐标空间与相机坐标空间的标注信息，来获得不同点云的点之间的对应关系，并生成真实对应矩阵A_gt，使用对应损失函数L_corr约束网络的训练过程：

L_corr＝KL(A_gt,A_pred)

其中，KL()表示KL散度计算函数，用于衡量两个分布的相似程度，其值越大说明两个输入的矩阵表示的分布的差异越大；A_pred为估计得到的对应矩阵；

同时，针对两帧点云通过已知的对应关系(可通过对应估计模块实现)估计得到的对应矩阵A_ij和A_ji，计算其双向损失，鼓励网络在估计的响应矩阵中聚合权重，在训练阶段，利用了双向对应特性；

式中，P_i和P_j分别为第i帧和第j帧的点云，A_ij为表示从第j帧到第i帧对应关系的对应矩阵，A_ji为表示从第i帧到第j帧对应关系的对应矩阵，L(·)表示L1损失函数；

最终阶段，将位姿变换损失函数L_transform分为旋转损失L_rot和平移损失L_t，分别评估网络在估计旋转和平移时的性能，而不是将它们组合成单个统一的损耗函数。变换损失计算l¹，使用预测变换[R_pred,t_pred]变换的点云与地面实况变换[R_gt,t_gt]之间的距离，如下列公式所示：

L_transform＝λ_rotL_rot+λ_tL_t

＝λ_rot(R_gtP_i-R_predP_i)+λ_t(t_gt-t_pred)

式中，λ_rot和λ_t分别表示旋转损失L_rot和平移损失L_t在变换损失中所占的比重；T_pred为最终估计得到的六维位姿，其包括三维的旋转和三维的位移；T_gt为目标物体真实的六维位姿；R_pred是位姿T_pred的旋转矩阵，而t_pred是位姿T_pred的平移向量；R_gt是位姿T_gt的旋转矩阵，而t_gt是位姿T_gt的平移向量。

本发明将深度视频流作为输出，在数据预处理模块，对视频流的第一帧中的目标物体进行位姿估计，得到目标物体的初始位姿。初始化完成后，开始对深度视频流的物体进行连续的六维姿态跟踪。在对应估计模块中，提取来自不同帧的点云数据的几何特征，并进行特征匹配，估计不同点云之间点与点的对应关系。在对应扩展模块中，针对估计得到的对应关系，进行对应扩张，使每一个点所关联的点的数量尽可能少，在经过对应矩阵映射后能够得到更准确的形状。在基准帧模块中，随着时间的推进根据标准更新基准帧，以减小跟踪过程中的累计误差。本发明解决了类级别物体难以使用深度视频流输出进行六维姿态跟踪的问题，针对具有相同几何特征的同类物体，即便模型在此前没有见过该物体，也能够跟踪其六维姿态。

本发明未详尽之处，均可参见现有技术。

本发明的有益效果为：

1、本发明在类级别三维物体六维姿态跟踪过程中，加入了三种约束：软对应约束，变换约束(现有同类方法中普遍使用，当整个跟踪流程结束后，将估计得到的位姿与真实位姿进行比较，检查是否相同)和双向对应约束，尤其是针对基于深度视频流的三维跟踪，为了解决深度图像上难以估计准确的关键点和关键点估计偏移，导致跟踪定位不准确的问题，本发明所设计的软对应约束有效解决了密集点云匹配时对应点分散的问题，能够将点云映射到更加接近目标点云的正确形状，从而使得三维物体六维姿态跟踪结果更加精确。

2、本方法深度视频流和第一帧画面中目标物体的分割结果和六维姿态，即使该同类物体在此前的训练数据中从来没有出现过，也可完成对该物体的六维姿态跟踪。

附图说明

构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。

图1为本发明的基于软对应估计的类级别三维物体六维姿态跟踪方法各个模块协作的流程图；

图2为本发明在标准物体坐标空间数据集(NOCS-REAL275)在同一个场景下针对不同类别不同角度的跟踪结果图；其中(a)为角度一的跟踪结果图；(b)为角度二的跟踪结果图；(c)为角度三的跟踪结果图；(d)为角度四的跟踪结果图；(e)为角度五的跟踪结果图；(f)为角度六的跟踪结果图；

图3为本发明中软对应关系的估计流程、点云收缩问题和点云扩张后的效果图。

具体实施方式

为了使本技术领域的人员更好的理解本说明书中的技术方案，下面结合本说明书实施中的附图，对本发明书实施例中的技术方案进行清楚、完整的描述，但不仅限于此，本发明未详尽说明的，均按本领域常规技术。

实施例1

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如图1所示，包括如下步骤：

S1：在数据预处理模块，通过现有相机标定方法预先获得相机内参，通过相机内参和深度视频流作为输入，反投影计算每一帧画面对应的三维点云数据，将来自两个不同输入点云分别进行均值化处理，以保证在训练阶段网络能够有效地估计视频帧之间的相对位置和姿态变化；

同时，选择深度视频流的第一帧图像，使用现有的目标检测方法yolov8检测目标物体所在区域，然后使用FAST-SAM模型在检测得到的二维包围盒中分割出目标物体，得到覆盖该区域的分割掩膜M_init(也被称为mask)，并使用现有的类级别单帧位姿估计方法(如GenPose、IST-Net)估计目标物体的六维姿态作为初始位姿；这一初始化过程结束后，开始对三维物体进行姿态跟踪；

S2：在帧间姿态估计模块中，使用三层感知机(MLP)提取S1获得的三维点云数据的几何特征，来自两帧的点云经过多层感知机得到两个特征向量，两个特征向量通过叉乘以计算不同特征向量之间的相似度，该相似度的表示形式为矩阵，称为对应矩阵，表示不同点之间的对应关系，如图3所示；

在训练过程中设计了软对应约束：使用已知的对应关系生成真实的对应矩阵，在真实对应矩阵的约束下训练特征提取网络的参数以估计正确的对应矩阵；

S3：在对应扩张模块中，使用k双向对应来控制对应矩阵中对应关系的分散程度，在S2过程中得到的对应矩阵中，每个点可能在另一个点云中与多个点关联，这会导致关联点加权得到的对应点在空间上趋向接近所有关联点的平均点，导致最终加权得到的整个点云形状错误，如图3点云收缩问题所示。k双向对应通过将对应矩阵中权重最大的k个点保留，来限制可以关联的点的数量，其他点权重置为0，并重新进行行列归一化。被排除的点不会继续参加归一化，无法被分配新的权重，也无法参与之后的加权过程。每个点在另一个点云中能够关联的点的数量受到数值k的限制，即最大不能超过k个。随着迭代次数增加，按预设的步长逐渐缩小k的值，k的值至小为3，使对应矩阵映射后的原点云逐渐接近目标点云的形状。

实施例2

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例1所述，所不同的是，在均值化时需要点云的中心点才能进行，如果简单使用点云的均值点作为中心点，当分割结果存在误差时，可能会导致背景点错误地包含在观测点云中，进而影响点云的标准化过程和中心位置的确定，从而影响网络在学习特征过程中的效果。为了解决这一问题，S1中均值化处理时，将来自两帧点云的所有点作为整体共同计算均值点作为中心点，从而有效避免背景点对点云中心定位的干扰；

S1中，初始位姿初始化结束后，从输入的深度视频流中选择当前需要匹配的两帧深度图像，通过深度图像与分割掩膜可以得到每个目标像素点的深度值z，由于相机是预先标定过的，相机的内部参数矩阵K为已知，能够获得目标二维像素点(u,v)在相机坐标系下反投影得到的三维点(x,y,z)，如下公式所示：

实施例3

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例2所述，所不同的是，S2中每层感知机由两个线性层与一个ReLU层组成，两个特征向量叉乘后，使用现有的sinkhorn算法迭代进行行列归一化，sinkhorn算法优选迭代5次，每次按顺序分别对矩阵的行和列进行归一化操作，最终得到表示对应关系的对应矩阵，反映不同点之间的特征的相似度。

通过利用前一视频帧中物体的已知位置数据，以及相机的运动数据，本发明创新性地提出一种方法，用于实现对当前视频帧的精确深度图像分割，由于物体和相机的运动是连续的，S1中假设在连续的视频帧之间物体相对于相机的位置变化是微小的，忽略不计，首先，使用前一帧，即第i帧的分割掩膜M_i(在初始情况下为初始分割掩膜M_init)，获取目标物体在前一视频帧深度图像D_t-1上的像素坐标；在当前帧，即第j帧的深度图像上生成一系列不同大小的二维矩形包围盒B_j,k；随后，通过预设的步长(一般为1个像素)逐步扩大矩形包围盒B_i,k，并将其应用于当前帧的图像处理中，通过综合所有生成的分割结果，并选取其中置信度评分最高的分割结果作为当前帧j上最优的分割结果B_j，确保这些包围盒能够准确覆盖到目标物体；最后，矩形包围盒B_i,k作为输入数据，配合目标检测方法获得最终用于当前帧的分割掩膜M_j。

S2中的几何特征通过特征提取模块实现，使用点对特征(Point-Pair Feature,PPF)来描述点云中每个点周围的邻域，以便以旋转不变的方式表示每个点的局部几何信息；特征提取模块立在PointNet++之上，以点对特征作为输入，并输出每个点云的局部几何特征F_i和F_j，局部几何特征F_i和F_j随后被送入特征匹配模块以估计点云之间的软对应关系。

实施例4

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例3所述，所不同的是，S2中，使用三层感知机提取不同帧的相机坐标系下观测点云P_i和P_j的特征F_i和F_j，通过计算两特征向量的叉乘并行列归一化来得到两组特征之间的相似度得分，最终表示为对应矩阵A，通过对应矩阵A_ij建立第j帧的点云P_j的点通与第i帧的点云P_i的点的软映射关系，反之亦然；为对应矩阵A_ij的转置，如公式所示：

P_j＝A_ijP_j

在训练阶段，归一化对应矩阵A的行列，建立三维空间点之间的一对一、一对多、多对一的“软对应”关系，而非严格的一对一的“硬对应”关系；

在训练阶段，已知的信息包括：每一帧的点云的在标准物体坐标空间(NormalizedObject Coordinate Space,NOCS)下的对应点云N_i和N_j，标准物体坐标空间变换到相机坐标空间的相对位姿变换T_i和T_j，则：

P_i＝T_iN_i

P_j＝T_jN_j

通过位姿变换T_i和T_j可以建立标准物体坐标空间与相机坐标空间的联系，标准物体坐标空间下的点云N_i和N_j与相机坐标空间下的观测点云P_i和P_j中的点是一一对应的，在标准坐标空间下，点云N_i和N_j的对应来自同一物体的点，且朝向和位置是相同的，能够获得N_i和N_j之间点的对应关系，进而获得观测点云P_i和P_j点的对应关系。

实施例5

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例4所述，所不同的是，当估计得到对应矩阵后，其软对应性质会使得一个点云中的每个点在另一个点云中的权重一般不会集中在单个点上，而是分散在多个点上。据此，对应矩阵A_ij能够在点云P_j的坐标空间中相对于P_i生成一个对应点云P_c。对应矩阵中的每一个元素都代表一个权重，这些权重的取值范围在0到1之间，且其总和为1，表明所有可能的权重组合生成的对应点云P_c都包含在点云P的空间内，并基于点云P中的点以加权方式组合而成，该空间可以被视为点云P中点的形状的多种变化。

基于软对应的特性导致权重在多点之间分散的情况，本发明提出了一种新策略“对应扩展”，该策略旨在逐步减小每一个点在另一点云中相关的区域。通过减少计算对应点时涉及的点数，来集中关联点的分布，该过程同样导致由矩阵A_ij和点云P表示的空间的逐渐缩小，使得生成的对应点云更加贴近点云P的形状，从而有效地解决了“点云收缩”的问题。

S3中采用k双向对应控制对应矩阵A中对应关系的规模，利用点云N_i和N_j处于同一坐标空间下且属于同一物体的不同部分这一特性，使用最近邻(knn)算法为每一个点p_a寻找半径r内的k个点p_a,k,k＝1,2,…,k作为其关联点；当来自点云N_i的点p_a是来自点云N_j的点p_b的关联点，同时p_b也是点p_a的关联点时，这组点(p_a,p_b)被视为相互关联的点对；

生成与对应矩阵A同样大小的矩阵A_gt，将其中相互关联的点对对应的位置设置为1，其他位置设置为0；使用KL散度约束对应矩阵A_gt与估计的对应矩阵A_ij，使帧间位姿估计模块能够估计出表示帧间点云软对应关系的软对应矩阵。

实施例6

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例5所述，所不同的是，使用双向对应距离(Bidirectional Correspondence Distance，BCD)进一步估计对应的置信度分数。通过对两组点云的特征相似性矩阵S进行转置，能够估计表示为对应矩阵A_ij和A_ji的双向对应关系，从而实现两组点云间相对姿态的估计。当对应关系为硬对应时，A_ij和A_ji是互逆的。然而，当对应关系为软对应并表示为软对应矩阵时，由于权重分散和行列归一化的影响，A_ij和A_ji通常不形成互逆关系。

通过利用双向对应关系，可以识别出权重分布过度分散的点。如上所述，点云P_i中的一个点P_c对应于点云P_j中的多个点P_c,k，并且当P_c,k在空间上分散时，在对应矩阵D中A_ij和A_ji中相应的点在P_j点云中将会非常远离那个点。基于这一现象，当对对应矩阵A_ij和A_ji进行逐点加权计算时，在空间距离上，生成的点p^′ _c在P_i中也会远离P_c的原始位置。

BCD_i＝P_i-A_ijA_jiP_i

BCD_i表示第i帧点云的双向对应距离值。

所有的关联点对都对应着权重集中在特定区域的点，它们的双向对应距离BCD较小。这是因为这样的点的对应关系接近于硬对应，它们的对应矩阵线性组合受到的点数很少的影响。因此两个对应矩阵的组合本质上不改变它们的空间位置。

借助软对应矩阵估计可以准确地估计连续帧之间的相对姿态，在持续匹配过程中错误的累积是不可避免的。这些累积误差会逐渐将跟踪结果引向错误的方向，最终导致跟踪失败。这也是无法将点云注册方法直接应用于跟踪任务的原因之一。为了克服累积误差问题，本发明提出了一种参考帧策略，在跟踪开始时，初始帧的数据被保存为参考帧，在计算连续两帧之间的相对姿态，相邻帧之间的相对变换被应用于参考帧；然后，这一变换被应用于参考帧的点云；随后，参考帧被变换到当前帧的坐标***中；参考帧和当前帧的点云被视为相邻帧，并估计软对应矩阵来确定相对姿态；最后，多个阶段的姿态估计结果被累积，得出当前图像帧中目标物体的最终估计。参考帧策略用于在一定时间间隔内匹配帧，从而减少误差累积并提供更准确的姿态跟踪结果。

实施例7

一种基于软对应估计的类级别三维物体六维姿态跟踪方法，如实施例6所述，所不同的是，S2的训练过程中，通过标准物体坐标空间与相机坐标空间的标注信息，来获得不同点云的点之间的对应关系，并生成真实对应矩阵A_gt，使用对应损失函数L_corr约束网络的训练过程：

L_corr＝KL(A_gt,A_pred)

其中，KL()表示KL散度计算函数，用于衡量两个分布的相似程度，其值越大说明两个输入的矩阵表示的分布的差异越大；A_pred为估计得到的对应矩阵；

同时，针对两帧点云通过已知的对应关系(可通过对应估计模块实现)估计得到的对应矩阵A_ij和A_ji，计算其双向损失，鼓励网络在估计的响应矩阵中聚合权重，在训练阶段，利用了双向对应特性；

式中，P_i和P_j分别为第i帧和第j帧的点云，A_ij为表示从第j帧到第i帧对应关系的对应矩阵，A_ji为表示从第i帧到第j帧对应关系的对应矩阵，L(·)表示L1损失函数，本发明对点云执行两个对应矩阵变换，并使用平滑的L1损失来约束它们相对于原始点云，有助于将对应矩阵的权重聚集到尽可能少的局部点上。

最终阶段，将位姿变换损失函数L_transform分为旋转损失L_rot和平移损失L_t，分别评估网络在估计旋转和平移时的性能，而不是将它们组合成单个统一的损耗函数。变换损失计算l¹，使用预测变换[R_pred,t_pred]变换的点云与地面实况变换[R_gt,t_gt]之间的距离，如下列公式所示：

L_transform＝λ_rotL_rot+λ_tL_t

＝λ_rot(R_gtP_i-R_predP_i)+λ_t(t_gt-t_pred)

式中，λ_rot和λ_t分别表示旋转损失L_rot和平移损失L_t在变换损失中所占的比重；T_pred为最终估计得到的六维位姿，其包括三维的旋转和三维的位移；T_gt为目标物体真实的六维位姿；R_pred是位姿T_pred的旋转矩阵，而t_pred是位姿T_pred的平移向量；R_gt是位姿T_gt的旋转矩阵，而t_gt是位姿T_gt的平移向量。

对发明与已有的同类方法(6-PACK，CAPTRA)在多种指标下进行对比，结果如表1所示：

表1：真实场景下本文提出的三维物体六维姿态跟踪方法与已有方法的对比精度结果

其中，Metric表示多种不同的度量指标，5°5cm和10°10cm表示旋转和平移两者都在限定范围以内的估计结果所占的百分比，例如5°5cm表示估计结果的旋转误差在5°以内，并且平移误差在5cm以内；R_err表示跟踪的旋转误差，t_err表示跟踪的平移误差，mIoU表示估计的位姿对应的包围盒与真实包围盒的交并比，IoU25和IoU50表示估计的位姿对应的包围盒与真实包围盒的交并比在限定值以上的百分比，例如IoU25表示估计的包围盒与真实的包围盒交并比在25％以上的帧所占序列中总帧数量的百分比；bottle、bowl、camera、can、laptop、mug分别代表六个不同的类别，每个类别对应三个或三个以上序列的评估结果，加粗表示该方法的误差在目前已有的同类算法中达到最好。

表2真实场景Wild6D数据集中本文的类级别三维物体六维姿态跟踪方法的精度结果

表2展示了在真实场景下Wild6D数据集中，本发明提出的方法在不同类别上的测试结果，从表1和表2可以看出，本发明的方法显著提高了类级别三维物体六维姿态跟踪的旋转和平移精度，并获得了最佳的类级别三维物体六维姿态跟踪结果，证明了本发明所提出方法的有效性。

图2展示了在同一个场景下针对瓶子、笔记本、相机、马克杯、易拉罐共5个类别物体不同角度的跟踪结果，图像中出现的5个物体在训练所用的数据中都是没有出现过的，以此证明本方法在面对新物体时的泛用性。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，包括如下步骤：

S1：在数据预处理模块，通过相机标定方法预先获得相机内参，通过相机内参和深度视频流作为输入，反投影计算每一帧画面对应的三维点云数据，将来自两个不同输入点云分别进行均值化处理，以保证在训练阶段网络能够有效地估计视频帧之间的相对位置和姿态变化；

同时，选择深度视频流的第一帧图像，使用目标检测方法检测目标物体所在区域，得到覆盖该区域的分割掩膜，并使用类级别单帧位姿估计方法估计目标物体的六维姿态作为初始位姿，初始化过程结束；

S2：在帧间姿态估计模块中，使用多层感知机提取S1获得的三维点云数据的几何特征，来自两帧的点云经过多层感知机得到两个特征向量，两个特征向量通过叉乘以计算不同特征向量之间的相似度，该相似度的表示形式为矩阵，称为对应矩阵，表示不同点之间的对应关系；

在训练过程中设计了软对应约束：使用已知的对应关系生成真实的对应矩阵，在真实对应矩阵的约束下训练特征提取网络的参数以估计正确的对应矩阵；

S3：在对应扩张模块中，使用k双向对应来控制对应矩阵中对应关系的分散程度，随着迭代次数增加逐渐缩小k的值，使对应矩阵映射后的原点云逐渐接近目标点云的形状。

2.根据权利要求1所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S1中均值化处理时，将来自两帧点云的所有点作为整体共同计算均值点作为中心点；

S1中，初始位姿初始化结束后，从输入的深度视频流中选择当前需要匹配的两帧深度图像，通过深度图像与分割掩膜得到每个目标像素点的深度值z，由于相机是预先标定过的，相机的内部参数矩阵K为已知，能够获得目标二维像素点(u,v)在相机坐标系下反投影得到的三维点(x,y,z)，如下公式所示：

3.根据权利要求1所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S2中每层感知机由两个线性层与一个ReLU层组成，两个特征向量叉乘后，使用sinkhorn算法迭代进行行列归一化，sinkhorn算法迭代5次，每次按顺序分别对矩阵的行和列进行归一化操作，最终得到表示对应关系的对应矩阵，反映不同点之间的特征的相似度。

4.根据权利要求3所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S1中假设在连续的视频帧之间物体相对于相机的位置忽略不计，首先，使用前一帧，即第i帧的分割掩膜M_i，获取目标物体在前一视频帧深度图像D_t-1上的像素坐标；在当前帧，即第j帧的深度图像上生成一系列不同大小的二维矩形包围盒B_j,k；随后，通过预设的步长逐步扩大矩形包围盒B_i,k，并将其应用于当前帧的图像处理中，通过综合所有生成的分割结果，并选取其中置信度评分最高的分割结果作为当前帧j上最优的分割结果B_j；最后，矩形包围盒B_i,k作为输入数据，配合目标检测方法获得最终用于当前帧的分割掩膜M_j。

5.根据权利要求4所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S2中的几何特征通过特征提取模块实现，使用点对特征来描述点云中每个点周围的邻域，以便以旋转不变的方式表示每个点的局部几何信息；特征提取模块立在PointNet++之上，以点对特征作为输入，并输出每个点云的局部几何特征F_i和F_j，局部几何特征F_i和F_j随后被送入特征匹配模块以估计点云之间的软对应关系。

6.根据权利要求5所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S2中，使用三层感知机提取不同帧的相机坐标系下观测点云P_i和P_j的特征F_i和F_j，通过计算两特征向量的叉乘并行列归一化来得到两组特征之间的相似度得分，最终表示为对应矩阵A，通过对应矩阵A_ij建立第j帧的点云P_j的点通与第i帧的点云P_i的点的软映射关系，反之亦然；为对应矩阵A_ij的转置，如公式所示：

P_j＝A_ijP_j

在训练阶段，归一化对应矩阵A的行列，建立三维空间点之间的一对一、一对多、多对一的“软对应”关系；

在训练阶段，已知的信息包括：每一帧的点云的在标准物体坐标空间下的对应点云N_i和N_j，标准物体坐标空间变换到相机坐标空间的相对位姿变换T_i和T_j，则：

P_i＝T_iN_i

P_j＝T_jN_j

通过位姿变换T_i和T_j建立标准物体坐标空间与相机坐标空间的联系，标准物体坐标空间下的点云N_i和N_j与相机坐标空间下的观测点云P_i和P_j中的点是一一对应的，在标准坐标空间下，点云N_i和N_j的对应来自同一物体的点，且朝向和位置相同，能够获得N_i和N_j之间点的对应关系，进而获得观测点云P_i和P_j点的对应关系。

7.根据权利要求6所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S3中采用k双向对应控制对应矩阵A中对应关系的规模，利用点云N_i和N_j处于同一坐标空间下且属于同一物体的不同部分这一特性，使用最近邻算法为每一个点p_a寻找半径r内的k个点p_a,k,k＝1,2,…,k作为其关联点；当来自点云N_i的点p_a是来自点云N_j的点p_b的关联点，同时p_b也是点p_a的关联点时，这组点(p_a,p_b)被视为相互关联的点对；

生成与对应矩阵A同样大小的矩阵A_gt，将其中相互关联的点对对应的位置设置为1，其他位置设置为0；使用KL散度约束对应矩阵A_gt与估计的对应矩阵A_ij，使帧间位姿估计模块能够估计出表示帧间点云软对应关系的软对应矩阵。

8.根据权利要求7所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，软对应矩阵估计过程中，在跟踪开始时，初始帧的数据被保存为参考帧，在计算连续两帧之间的相对姿态，相邻帧之间的相对变换被应用于参考帧；然后，这一变换被应用于参考帧的点云；随后，参考帧被变换到当前帧的坐标***中；参考帧和当前帧的点云被视为相邻帧，并估计软对应矩阵来确定相对姿态；最后，多个阶段的姿态估计结果被累积，得出当前图像帧中目标物体的最终估计。

9.根据权利要求8所述的基于软对应估计的类级别三维物体六维姿态跟踪方法，其特征在于，S2的训练过程中，通过标准物体坐标空间与相机坐标空间的标注信息，来获得不同点云的点之间的对应关系，并生成真实对应矩阵A_gt，使用对应损失函数L_corr约束网络的训练过程：

L_corr＝KL(A_gt,A_pred)

其中，KL()表示KL散度计算函数，用于衡量两个分布的相似程度；A_pred为估计得到的对应矩阵；

同时，针对两帧点云通过已知的对应关系估计得到的对应矩阵A_ij和A_ji，计算其双向损失，鼓励网络在估计的响应矩阵中聚合权重，在训练阶段，利用了双向对应特性；

式中，P_i和P_j分别为第i帧和第j帧的点云，A_ij为表示从第j帧到第i帧对应关系的对应矩阵，A_ji为表示从第i帧到第j帧对应关系的对应矩阵，表示L1损失函数；

最终阶段，将位姿变换损失函数L_transform分为旋转损失L_rot和平移损失L_t，：

L_transform＝λ_rotL_rot+λ_tL_t

＝λ_rot(R_gtP_i-R_predP_i)+λ_t(t_gt-t_pred)

式中，λ_rot和λ_t分别表示旋转损失L_rot和平移损失L_t在变换损失中所占的比重；T_pred为最终估计得到的六维位姿，其包括三维的旋转和三维的位移；T_gt为目标物体真实的六维位姿；R_pred是位姿T_pred的旋转矩阵，而t_pred是位姿T_pred的平移向量；R_gt是位姿T_gt的旋转矩阵，而t_gt是位姿T_gt的平移向量。