[发明专利]一种物体位姿测量方法、装置及存储介质

权利要求书

1.一种物体位姿测量方法，其特征在于，包括离线建模阶段和在线匹配阶段；

所述离线建模阶段包括：

输入物体的三维模型，所述三维模型包含模型点云坐标和模型点云法向量；

对所述模型点云坐标和模型点云法向量进行采样；

在采样得到的模型点云坐标和模型点云法向量中构建特征集，计算模型点对特征；

将提取到的所述模型点对特征存储到哈希表中；

所述在线匹配阶段包括：

输入深度图像，根据相机内参计算出所述深度图像每个像素点对应的场景点云坐标，并根据所述点云坐标计算出场景点云法向量；

对所述场景点云坐标和场景点云法向量进行采样；

在采样得到的场景点云坐标和场景点云法向量中构建特征集，计算场景点对特征；

对提取到的所述场景点对特征进行量化并与存储在哈希表中的所述模型点对特征匹配，获取多个候选物体位姿；

输入彩色图像提取场景边缘点云；

根据所述场景边缘点云对所述候选物体位姿进行筛选；

将筛选得到的候选物体位姿进行聚类，得到多个初步物体位姿；

使用迭代最近点算法对所述初步物体位姿进行配准，得到最终物体位姿；

所述对提取到的所述场景点对特征进行量化并与存储在哈希表中的所述模型点对特征匹配，获取多个候选物体位姿这一步骤，具体包括：

将提取到的所述场景点对特征进行量化处理；

对量化结果进行扩充，以补偿噪声引起的特征偏移；

将扩充后的多个结果值作为键值，在哈希表中寻找具有相同键值的模型点对特征；

根据所述模型点对特征，获取多个候选物体位姿；

采用如下方式扩充所述量化结果Q：

设其中e_i为第i维量化误差，F_i为F的每一维的值，Δ为量化间隔；当i＝1，当时，Q_new＝(Q₁-1,Q₂,Q₃,Q₄)，当时，Q_new＝(Q₁+1,Q₂,Q₃,Q₄)，当时，不对Q进行扩充；其中，Q_new为扩充后的所述量化结果。

2.根据权利要求1所述的一种物体位姿测量方法，其特征在于，所述对所述模型点云坐标和模型点云法向量进行采样这一步骤，具体包括：

根据模型点云坐标，计算出包围模型点云的边界框，得到模型点云空间；

对所述模型点云空间进行栅格化，得到多个大小相等的栅格，每个所述栅格包含多个点云，每个点云包含对应的模型点云坐标和模型点云法向量；

对每个所述栅格中包含的点云根据模型点云法向量之间的夹角的大小进行聚类；

对每个聚类中的模型点云坐标和模型点云法向量求平均值，得到每个栅格采样后的模型点云坐标和模型点云法向量。

3.根据权利要求1所述的一种物体位姿测量方法，其特征在于，所述在采样得到的模型点云坐标和模型点云法向量中构建特征集，计算模型点对特征这一步骤，具体包括：

对采样得到的模型点云坐标构造K-D树；

选取参考点，所述参考点为采样得到的模型点云坐标中的任意一点；

在所述K-D树中查找目标点，所述目标点为与所述参考点距离小于第一阈值的点；

依次计算所述参考点与目标点构成的模型点对特征。

4.根据权利要求1所述的一种物体位姿测量方法，其特征在于，所述将提取到的所述模型点对特征存储到哈希表中这一步骤，具体包括：

对提取到的所述模型点对特征进行量化处理；

将量化结果通过哈希函数求出一个键值，作为所述点对特征在哈希表中的索引；

将具有相同索引的点对特征存储在哈希表的同一个桶中，不同索引的点对特征存储在哈希表的不同桶中。