(19)中华 人民共和国 国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111669299.6 (22)申请日 2021.12.31 (71)申请人 清华大学深圳国际研究生院 地址 518055 广东省深圳市南 山区西丽 街 道深圳大 学城清华校区A栋二楼 (72)发明人 梁斌　刘厚德　黄俊楠　夏崇坤　 王学谦　 (74)专利代理 机构 深圳新创友知识产权代理有 限公司 4 4223 代理人 孟学英 (51)Int.Cl. G06T 7/70(2017.01) G06T 7/66(2017.01) G06K 9/62(2022.01) G06V 10/80(2022.01)G06V 10/764(2022.01) G06V 10/82(2022.01) G06N 3/04(2006.01) G06N 3/08(2006.01) (54)发明名称 一种实时6D位姿估计方法及计算机可读存 储介质 (57)摘要 本发明提供一种实时6D位姿估计方法及计 算机可读存储介质， 方法包括： 从目标物体的三 维模型中选取预设关键点并得到所述预设关键 点在所述三维模型中基于三维模型原点坐标系 下的坐标； 通过网络预测所述目标物体的所述预 设关键点在实际场景中基于相机坐标系下的坐 标； 将所述预设关键点在所述三维模 型原点坐标 系下的坐标和在所述相机坐标系下的坐标进行 拟合计算得到所述目标物体当前的位姿。 使 得网 络具备了优秀的实时性， 实现了同等速度下的最 高位姿估计精度。 权利要求书2页 说明书11页 附图3页 CN 114359377 A 2022.04.15 CN 114359377 A 1.一种实时6D位姿估计方法， 其特 征在于， 包括如下步骤： S1： 从目标物体的三维模型中选取预设关键点并得到所述预设关键点在所述三维模型 中基于三维模型原点 坐标系下的坐标； S2： 通过网络预测所述目标物体的所述预设关键点在实际场景中基于相机坐标系下的 坐标； S3： 将所述预设关键点在所述三维模型原点坐标系下的坐标和在所述相机坐标系下的 坐标进行拟合计算得到所述目标物体当前的位姿。 2.如权利要求1所述的实时6D位姿估计方法， 其特征在于， 通过网络预测所述预设关键 点在实际场景中基于所述相机坐标系下的坐标包括如下步骤： S21： 获取所述目标物体的RGB ‑D图像并将所述RGB ‑D图像中的全局信息转化为局部信 息， 所述局部信息是 所述目标物体的局部RGB图像和所述目标物体的局部点云信息； S22： 对所述目标物体的局部RGB图像进行纹理特征提取、 对所述目标物体的局部点云 信息进行几何特征提取， 利用密集融合得到的逐点密集对应进行逐点特征拼接并基于所述 逐点特征识别出属于所述目标物体的点云； S23： 利用所述目标物体的所述逐点特征预测每个可视点与所述预设关键点之间的偏 移向量； 利用所述逐点特征与所述偏移向量对每个所述可视点与各个所述预设关键点的映 射关系进 行评估并根据评估结果对每个所述可视点的预测信息进 行筛选和融合， 得到所述 预设关键点在所述相机坐标系下的坐标。 3.如权利要求2所述的实时6D位姿估计方法， 其特征在于， 依据 预测的边界框对所述目 标物体的RGB ‑D图像进行截取 得到所述局部信息； 将截取后所述目标物体的局部点云的中心点设置为原点， 方向与相机坐标系相同建立 一个点云中心点坐标系并将所述目标物体在所述三维模型原点坐标系下的坐标转移到所 述点云中心 点坐标系下， 消除所述相机坐标系中相机到所述点云中心点的偏移 量以突出局 部信息。 4.如权利 要求3所述的实时6D位姿估计方法， 其特征在于， 使用ResNet与上采样相结合 的方法对所述目标物体的局部RGB图像进行纹 理特征提取； 利用PointNet对所述目标物体的局部点云信息进行几何特 征提取。 5.如权利要求4所述的实时6D位姿估计方法， 其特征在于， 使用最大池化方法获取全局 特征并将所述全局特征与每个所述可视点的所述逐点特征作为输入， 预测所有所述可视点 的语义标签得分， 识别出属于所述目标物体的点云； 训练时使用softmax交叉熵作为损失函数: 其中， N为点数， cn表示第n个可视点经过softmax 处理后各类别的预测置信度， ln是与cn 相对应的真实值， n是 可视点的数量， 选取 各标签得分最高的类别作为预测的结果。 6.如权利 要求5所述的实时6D位姿估计方法， 其特征在于， 结合多层感知器和ResNet设 计一个偏移向量预测网络来预测各个所述可视点与每个所述预测关键点之间的逐点的偏 移向量；权　利　要　求　书 1/2 页 2 CN 114359377 A 2对于属于所述目标物体的一组可视点 和一组所述预设关键点 所述 偏移向量预测网络以所述逐点特 征fn作为输入输出 所述偏移向量 在训练阶段， 使用Lvec监督 的预测： 其中， M为预设 关键点个数， 为所述偏移向量的真实值； 各个所述可视点预测所述预设关键点坐标 由可视点坐标和所述偏移向量通过如 下公式得到： 7.如权利要求6所述的实时6D位姿估计方法， 其特征在于， 利用所述逐点特征与所述偏 移向量对每个所述可视点与各个所述预设关键点的映射关系进行评估并根据所述评估结 果对每个所述可视点进行 逐点注意力投票进行筛 选； 训练过程中添加Linfl作为影响力预测的损失函数， 其表达式如下： 其中， 根据预测结果与几何空间中的地面真值之间的距离对逐点预测结果进行分类； 低于阈值Tp的被视为正项， 高于 阈值Tn的被视为负项， 其余为忽略项； 忽略的所述可视点不 参与损耗计算； 当点 为正项时ζp＝1， 为负项时ζn＝1， 其余情况下ζp和 ζn为0。 8.如权利要求7所述的实时6D位姿估计方法， 其特征在于， 选取预定比例的所述可视点 并依据所述可视点的影响力分配投票权 重， 投票过程可用下式进行表达： 其中， 表示可视点vpcn对预设关键点kpcm的影响力， δ(x)表示一个权重分布函数， 当 小于所述预定比例时， δ(x)＝0； 当 大于等于所述预定比例时δ(x)输出结果为对所述 预定比例影响力使用softmax函数进行归一 化后的输出； 所述预设 关键点投票网络通过以下损失函数进行监 督训练： 其中， 表示对应预设 关键点坐标的真实值。 9.如权利 要求8所述的实时6D位姿估计方法， 其特征在于， 利用Kabsch算法将预测的所 述预设关键点所述第二坐标与所述已有模型中所述第一坐标进行匹配， 计算出 由旋转R和 平移T组成的6D物体的姿态。 10.一种计算机可读存储介质， 所述计算机可读存储介质存储有计算机程序， 其特征在 于， 所述计算机程序被处 理器执行时实现如权利要求1 ‑9任一所述方法的步骤。权　利　要　求　书 2/2 页 3 CN 114359377 A 3