CN109785389A - 一种基于哈希描述与迭代最近点的三维物体检测方法 - Google Patents

Abstract

本发明公开了一种基于哈希描述及迭代最近点的三维物体检测方法，通过在三维信息中检测目标物体的位姿数据，可以让机器人等作业工具能完成更丰富的作业任务和进行更复制的作业操作，使得结构化生产向着非结构化转变，采用了哈希描述来对三位数据进行唯一解释，减少了对无效数据的匹配，加快了特征匹配速度。采用了点到切平面的迭代最近点方法，使得匹配更加可靠，精度更高。

Description

一种基于哈希描述与迭代最近点的三维物体检测方法

技术领域

本发明涉及机器视觉中的三维物体识别领域，尤其涉及一种基于哈希描述与迭代最近点的三维物体检测方法。

背景技术

通过机器视觉技术对作业场景进行感知的方法在快递分拣、目标测距、自动化装配等领域有着广泛的应用，特别是在二维图像中的识别与定位技术发展的非常成熟，能满足目前大部分的自动化生产作业需求。随着国家工业4.0的逐步推进，对制造产业自动化程度要求越来越高，基于二维图像的识别与定位技术就显得捉襟见肘。随着三维视觉传感器技术的成熟与价格的下降，越来越多的应用场景开始尝试使用三维视觉传感器来获取作业场景的三维信息，并根据三维信息来对场景中目标进行位置检测和姿态检测，即位姿识别。

现有技术对三维物体的位姿识别方法需要在三维数据中提取大量的特征信息和特征匹配，识别速度慢，定位精度很低，无法实现快速准确的三维目标位姿识别。

发明内容

本发明的目的在于提供一种基于哈希描述与迭代最近点的三维物体检测方法，具有降低三维目标特征搜索的时间复杂度、提高识别定位精度的优点。

本发明的上述目的是通过以下技术方案得以实现的：

一种基于哈希描述及迭代最近点的三维物体检测方法，包括深度相机和PC机，还包括以下步骤：

S1、深度相机采集需要检测的物体的场景数据；

S2、深度相机向场景发射红外射线，深度相机内的红外接收器将接收场景的近红外反射，生成场景的三维点数据，即场景中的每个反射点相对于深度相机坐标系的XYZ信息；

S3、深度相机将采集到的场景数据传到PC机进行保存，保存格式为PLY格式；

S4、利用S3获取的PLY格式的场景数据提取检测物体的目标模型M；

S5、PC机计算三维模型点云的每个点的协方差矩阵，求取协方差矩阵的特征向量，获取每个点的法向量；

S6、根据S5中的法向量生成目标模型M的点对特征；

S7、将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述；

S8、利用深度相机和PC机对场景S进行处理，获得场景S的哈希值，访问目标模型M的哈希描述，快速找到匹配的相似点对，将匹配结果转换为霍夫投票值；

S9、通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果；

S10、通过对迭代最近点算法对初检测结果进行二次矫正，得出精确的目标位姿检测结果。

进一步，所述步骤S4，将场景数据的坐标定义为深度传感器之上，每个数据的坐标值为(x，y，z)，统计所有数据中z值的数量，去除出现最多的z值来剔除数据，进而获取待检测物体的目标模型M。

进一步，所述步骤S5，获取目标模型M中各三维数据点的法向量目标模型M＝{m₀,m₁,m₂,...,m_k-1,m_k}，各三维数据点的法向量可通过各点周围的相邻点来计算，为了计算点的法向量，首先需要获得该点与相邻点之间的协方差矩阵，假设要求取m_i点的法向量协方差矩阵C的计算如式(1)所示，R表示以m_i点为中心的球形空间的半径，距离值d_i(i∈{1,2,...,k})表示相邻点m_j与中心点m_i的欧式距离，通过式(1)计算m_i点与半径R内所有点的协方差矩阵C：

通过协方差矩阵可以计算出m_i点的法向量，对协方差矩阵C的进行特征值分解，可求得协方差矩阵C的特征值和特征向量，对于物体表面的点云数据，其在法线方向的空间分布变化最为微弱，因此中心点m_i的法向量则为协方差矩阵C最小特征值所对应的特征向量，以此类推，获取所有点的法向量

进一步，所述步骤S6中，利用各点的法向量来计算点对特征，对于三维点云数据中的点p_i与除该点以外的任意点p_j，通过两点可以组成点对，利用两点间的法向量与欧式距离可以形成点对的点对特征，点对特征的计算方式如式(2)所示：

其中表示两点的连线方向，表示两点连线的欧氏距离，表示p_i点的法相向量与两点连线方向的夹角，表示两点法向量之间的夹角，对于向量之间的夹角计算如公式(3)所示：

进一步，步骤S7中，将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述，定义点云集p中的任意点对(p_i，p_j)∈p，点对特征的表达式如式(2)所示，利用式(4)将点对特征转化为哈希值：

其中d_dist表示采样步长，d_angle表示采样角度，d_angle＝2π/N_angle，N_angle表示角度粒度，使用者可以根据实际结果调整N_angle，通常可将其设置为30。通过d_dist、d_angle来对点对特征进行离散化处理，使得相似特征能够转化为相同的特征值，对点对特征F离散化过后，点对特征F(p₁,p₂)_discretized＝F(f₁,f₂,f₃,f₄)的哈希值由(5)式获得：

Index＝P₁*f₁+P₂*f₂+P₃*f₃+f₄ (5)

公式(5)中的P1、P2、P3为3个不同的质数，这是为了使得转化后的哈希值唯一，统计所有点对的哈希值，将相同哈希值的点便可通过哈希表进行集中访问，所有哈希值组成哈希表，该哈希表称为点云数据的哈希描述。

进一步，步骤S8中，利用哈希描述生成霍夫投票值，定义目标模型M，场景的三维点云数据集为S，根据步骤S5生成目标模型点云数据集的哈希描述，通过将场景数据集S的点对转化为哈希值，访问目标模型M生成的哈希描述，进而实现快速的相似特征匹配，再将匹配结果转换为霍夫值，进行全局霍夫投票。

进一步，步骤S9，通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果，霍夫投票值(m_r,α)中的m_r直接由(m_r，m_i)获得，而角度α则由式(6)计算而来,定义世界坐标系W，式(6)中的和描述了将点s_r和m_r移动到世界单位坐标系原点所做的位移运动以及将两点的法向量与同世界单位坐标系的主轴x对齐所做的旋转变换：

对于具有相同哈希值的点对(m_r，m_i)∈M和(s_r，s_i)∈S，首先将点s_r与点m_r的法向量旋转到与世界单位坐标系W的主轴x平行且方向一致，然后移动两点的位置与世界单位坐标系的原点重合，这时点对(m_r，m_i)只需绕着世界单位坐标系的x轴旋转α角度即可与点对(s_r，s_i)重合，通过世界单位坐标系计算出与之后，根据式(6)就可以解出角度α，场景点s_r的霍夫投票值(m_r,α)求解完毕；

接下来通过投票选举最优霍夫投票值,建立了_n×m的全局霍夫投票累加器来收集各个霍夫投票值的票数，二维累加器的行元素为模型点云中所筛选出的各参考点行数n等于参考点的个数，累加器的列数m则与角度粒度N_angle有关，当N_angle＝30时投票累加器的列数为30；

场景参考点s_r会与其他的场景点s_i组成点对，每一组场景点对都可能寻找到若干相似的模型点对(m_r，m_i)，这些点对表示了(s_r，s_i)可能在目标模型M中存在的位置，计算场景点对(s_r，s_i)与相似点对(m_r，m_i)之间的角度α，就可获得场景点s_r的多个霍夫投票值(m_r,α)，对应投票累加器中位置加一，当前场景参考点与其他场景点组成的点对都完成霍夫投票值的计算后，投票累加器中的峰值将作为当前场景点的最优霍夫投票值，将最优霍夫投票结果带入式(7)，即可以获得目标模型M与场景S之间的初位姿检测结果RigidTrans：

进一步，步骤S10，使用迭代最近点算法对初检测结果进行二次修正，初检测结果已经反应了目标模型M与场景S之间的位姿关系，由于初检测结果正好满足相关点姿态相近的条件，可以对迭代最近点算法进行线性化处理，使得迭代最近点算法在保障精度的同时拥有更快的速度；

采用点到切平面的迭代最近点算法来对初检测结果进行二次修正，对于点到切平面的迭代最近点算法，误差函数的优化目标为模型点到对应场景点的切平面的均方误差达到最小；

误差函数如公式(8)所示：

式(8)中m_i＝(m_ix,m_iy,m_iz,T_im)，T_im表示模型点集中的某点，而s_i＝(s_ix,s_iy,s_iz,T_is)，T_is表示m_i在目标点云中的对应点，n_i＝(n_ix,n_iy,n_iz,0)则是点s_i的法向量；

M为4x4的旋转变换矩阵，由式(8)可见，当目标模型M按照变换矩阵M运动之后，移动后的模型点m_i在与场景点s_i做差，得到一个描述位移差的向量，这个向量再与点s_i的法向量做点积，点积的结果可用来估计一个点到另一个点切平面的远近，当所有模型点m_i按照M矩阵进行移动过后，使得式(8)的累计和达到最小，此时的M就为M_opt；

由于模型点集与场景点集的姿态相似，可以采用点到切平面的迭代最近点算法来将非线性问题朝着线性问题进行逼近，从而使得迭代最近点算法的数学形式更加简单，算法运行速度更加快捷；

对于描述位移和旋转的变换矩阵M，其由旋转部分R(α,β,γ)与位移部分T(t_x,t_y,t_z)组成，如式(9)所示：

M＝T(t_x,t_y,t_z)·R(α,β,γ) (9)

其中：

式(11)中的各值为：

r₁₁＝cosγcosβ,

r₁₂＝-sinγcosα+cosγsinβsinα,

r₁₃＝sinγsinα+cosγsinβcosα,

r₂₁＝sinγcosβ,

r₂₂＝cosγcosα+sinγsinβsinα,

r₂₃＝-cosγsinα+sinγsinβcosα,

r₃₁＝-sinβ

r₃₂＝cosβsinα

r₃₃＝cosβcosα

Rx(α)，Ry(β)，Rz(γ)分别表示绕着x，y，z轴的旋转，利用三角函数的等价相似性可知，如果模型点集与场景点集的姿态相似，我们可以将R部分中的三角函数进行等价代换，此时M中的旋转部分R可写为式(12)的形式：

因此，变换矩阵M可以表示为：

此时，式(8)也可转化为：

其中：

对于N组相关的点对，通过式(15)可获得N组线性方程组，这些方程组可写为为Ax-b的形式，其中：

求解M_opt的问题就转化为了求解X_opt的问题，这是一个标准的线性优化问题：

通过SVD分解来完成式(18)的求解，对A进行SVD分解可得到A＝UΣV^T，从而计算A的伪逆A⁺＝VΣ⁺U^T，那么式(18)的线性最小二乘解为：

x_opt＝A⁺·b (19)

三维物体空间位姿初估计的二次校正完成，X_opt即为三维物体空间位姿的最终识别结果。

综上所述，本发明具有以下有益效果：

1)是采用了哈希描述进行查表比较，速度很快；

2)利用了哈希描述和点到切平面的迭代最近点方法各自的优点，弥补了二者的缺点，使得本来比较慢的点到切平面方法通过线性化实现了算法加速。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的测量场景三维数据并提取目标点云模型示意图；

图2为本发明实施例的三维点云的点对特征的示意图；

图3为本发明实施例的点对特征生成哈希描述的流程图；

图4为本发明实施例的世界单位坐标系W的示意图；

图5为本发明实施例的取最优局霍夫投票值的流程图；

图6为本发明实施例的点到切平面的迭代最近点算法的模型图；

图7为本发明实施例的示例的待检测物体；

图8为本发明实施例的示例通过哈希描述进行粗略定位之后的效果图；

图9是本发明实施例的示例经过迭代最近点修正后的效果图。

具体实施方式

在下面的详细描述中，提出了许多具体细节，以便于对本发明的全面理解。但是，对于本领域技术人员来说很明显的是，本发明可以在不需要这些具体细节中的一些细节的情况下实施。下面对实施例的描述仅仅是为了通过示出本发明的示例来提供对本发明的更好地理解。

下面将结合附图，对本发明实施例的技术方案进行描述。

实施例：

一种基于哈希描述及迭代最近点的三维物体检测方法，包括深度相机和PC机，还包括以下步骤：

S1、深度相机采集需要检测的物体的场景数据；

S2、深度相机向场景发射红外射线，深度相机内的红外接收器将接收场景的近红外反射，生成场景的三维点数据，即场景中的每个反射点相对于深度相机坐标系的XYZ信息；

S3、深度相机将采集到的场景数据传到PC机进行保存，保存格式为PLY格式；

S4、利用S3获取的PLY格式的场景数据提取检测物体的目标模型M；

S5、PC机计算三维模型点云的每个点的协方差矩阵，求取协方差矩阵的特征向量，获取每个点的法向量；

S6、根据S5中的法向量生成目标模型M的点对特征；

S7、将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述；

S8、利用深度相机和PC机对场景S进行处理，获得场景S的哈希值，访问目标模型M的哈希描述，快速找到匹配的相似点对，将匹配结果转换为霍夫投票值；

S9、通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果；

S10、通过对迭代最近点算法对初检测结果进行二次矫正，得出精确的目标位姿检测结果。

其中，步骤S4中，将场景数据的坐标定义为深度传感器之上，如图1所示，每个数据的坐标值为(x，y，z)，统计所有数据中z值的数量，去除出现最多的z值来剔除数据，进而获取待检测物体的目标模型M。

步骤S5，获取目标模型M中各三维数据点的法向量目标模型M＝{m₀,m₁,m₂,...,m_k-1,m_k}，各三维数据点的法向量可通过各点周围的相邻点来计算，为了计算点的法向量，首先需要获得该点与相邻点之间的协方差矩阵，假设要求取m_i点的法向量协方差矩阵C的计算如式(1)所示，R表示以m_i点为中心的球形空间的半径，距离值d_i(i∈{1,2,...,k})表示相邻点m_j与中心点m_i的欧式距离，通过式(1)计算m_i点与半径R内所有点的协方差矩阵C：

通过协方差矩阵可以计算出m_i点的法向量，对协方差矩阵C的进行特征值分解，可求得协方差矩阵C的特征值和特征向量，对于物体表面的点云数据，其在法线方向的空间分布变化最为微弱，因此中心点m_i的法向量则为协方差矩阵C最小特征值所对应的特征向量，以此类推，获取所有点的法向量

如图2所示，步骤S6中，利用各点的法向量来计算点对特征，对于三维点云数据中的点p_i与除该点以外的任意点p_j，通过两点可以组成点对，利用两点间的法向量与欧式距离可以形成点对的点对特征，点对特征的计算方式如式(2)所示：

其中表示两点的连线方向，表示两点连线的欧氏距离，表示p_i点的法相向量与两点连线方向的夹角，表示两点法向量之间的夹角，对于向量之间的夹角计算如公式(3)所示：

步骤S7中，将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述，定义点云集p中的任意点对(p_i，p_j)∈p，点对特征的表达式如式(2)所示，利用式(4)将点对特征转化为哈希值：

其中d_dist表示采样步长，d_angle表示采样角度，d_angle＝2π/N_angle，N_angle表示角度粒度，这个值是使用者自行设定的，N_angle越大，d_angle就越小，也就是说对两个角度的区分度越大，这个值通常为30，若采样角度d_angle为12°，就是说对于0°-12°这个区间内的角度，认为他们是相似的。使用者可以根据实际结果调整N_angle，通常可将其设置为30。通过d_dist、d_angle来对点对特征进行离散化处理，使得相似特征能够转化为相同的特征值，对点对特征F离散化过后，点对特征F(p₁,p₂)_discretized＝F(f₁,f₂,f₃,f₄)的哈希值由(5)式获得：

Index＝P₁*f₁+P₂*f₂+P₃*f₃+f₄ (5)

公式(5)中的P1、P2、P3为3个不同的质数，这是为了使得转化后的哈希值唯一，例如F(1,2,3,4)和F(4,3,2,1)，统计所有点对的哈希值，将相同哈希值的点便可通过哈希表进行集中访问，所有哈希值组成哈希表，该哈希表称为点云数据的哈希描述，如图3所示。

步骤S8中，利用哈希描述生成霍夫投票值，定义目标模型M，场景的三维点云数据集为S，根据步骤S5生成目标模型点云数据集的哈希描述，通过将场景数据集S的点对转化为哈希值，访问目标模型M生成的哈希描述，进而实现快速的相似特征匹配，再将匹配结果转换为霍夫值，进行全局霍夫投票，匹配结果的霍夫值的定义方式如下：

随机的选取S中的部分数据点作为参考点，假定待检测的目标存在与场景之中，存在一个参考点s_r∈S正好位于待检测的目标的表面。此时，模型点云数据集中应该存在一点m_r∈M同s_r∈S相对应。假定模型点云会随着m_r点运动，随着m_r点的法向量旋转。如果将m_r点移动到s_r的位置，同时让m_r点与s_r点的法向量重合，此时模型点云只需要绕着s_r点的法向量旋转α度就可实现场景目标与点云模板的重合。在这样的思路下，通过将场景点s_r和模型点m_r的位置与法向量对齐，点云集M到场景S的位移旋转变换就可用M中的一点m_r和角度α表示。此时，(m_r,α)就称为场景参考点s_r在点云集M中的霍夫投票值。

如果场景中一点s_r正好位于目标物体的表面，并且该点与另一个目标物体表面上的另一个点s_i组成了点对。将点对(s_r，s_i)转化为哈希值，索引目标模型M的哈希描述，将获得与(s_r，s_i)相似点对(m_r，m_i)，利用这些点对(m_r，m_i)就可以计算点s_r的霍夫投票值。

步骤S9，通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果，霍夫投票值(m_r,α)中的m_r直接由(m_r，m_i)获得，而角度α则由式(6)计算而来,定义世界坐标系W，如图4所示，图(4)与式(6)中的和描述了将点s_r和m_r移动到世界单位坐标系原点所做的位移运动以及将两点的法向量与同世界单位坐标系的主轴x对齐所做的旋转变换：

对于具有相同哈希值的点对(m_r，m_i)∈M和(s_r，s_i)∈S，首先将点s_r与点m_r的法向量旋转到与世界单位坐标系W的主轴x平行且方向一致，然后移动两点的位置与世界单位坐标系的原点重合，这时点对(m_r，m_i)只需绕着世界单位坐标系的x轴旋转α角度即可与点对(s_r，s_i)重合，通过世界单位坐标系计算出与之后，根据式(6)就可以解出角度α，场景点s_r的霍夫投票值(m_r,α)求解完毕；

接下来通过投票选举最优霍夫投票值,如图5所示，建立了_n×m的全局霍夫投票累加器来收集各个霍夫投票值的票数，二维累加器的行元素为模型点云中所筛选出的各参考点行数n等于参考点的个数，累加器的列数m则与角度粒度N_angle有关，当N_angle＝30时投票累加器的列数为30；

场景参考点s_r会与其他的场景点s_i组成点对，每一组场景点对都可能寻找到若干相似的模型点对(m_r，m_i)，这些点对表示了(s_r，s_i)可能在目标模型M中存在的位置，计算场景点对(s_r，s_i)与相似点对(m_r，m_i)之间的角度α，就可获得场景点s_r的多个霍夫投票值(m_r,α)，对应投票累加器中位置加一，当前场景参考点与其他场景点组成的点对都完成霍夫投票值的计算后，投票累加器中的峰值将作为当前场景点的最优霍夫投票值，将最优霍夫投票结果带入式(7)，即可以获得目标模型M与场景S之间的初位姿检测结果RigidTrans：

步骤S10，使用迭代最近点算法对初检测结果进行二次修正，初检测结果已经反应了目标模型M与场景S之间的位姿关系，由于初检测结果正好满足相关点姿态相近的条件，可以对迭代最近点算法进行线性化处理，使得迭代最近点算法在保障精度的同时拥有更快的速度；

如图6所示，采用点到切平面的迭代最近点算法来对初检测结果进行二次修正，对于点到切平面的迭代最近点算法，误差函数的优化目标为模型点到对应场景点的切平面的均方误差达到最小；

误差函数如公式(8)所示：

式(8)中m_i＝(m_ix,m_iy,m_iz,T_im)，T_im表示模型点集中的某点，而s_i＝(s_ix,s_iy,s_iz,T_is)，T_is表示m_i在目标点云中的对应点，n_i＝(n_ix,n_iy,n_iz,0)则是点s_i的法向量；

M为4x4的旋转变换矩阵，由式(8)可见，当目标模型M按照变换矩阵M运动之后，移动后的模型点m_i在与场景点s_i做差，得到一个描述位移差的向量，这个向量再与点s_i的法向量做点积，点积的结果可用来估计一个点到另一个点切平面的远近，当所有模型点m_i按照M矩阵进行移动过后，使得式(8)的累计和达到最小，此时的M就为M_opt；

由于模型点集与场景点集的姿态相似，可以采用点到切平面的迭代最近点算法来将非线性问题朝着线性问题进行逼近，从而使得迭代最近点算法的数学形式更加简单，算法运行速度更加快捷；

对于描述位移和旋转的变换矩阵M，其由旋转部分R(α,β,γ)与位移部分T(t_x,t_y,t_z)组成，如式(9)所示：

M＝T(t_x,t_y,t_z)·R(α,β,γ) (9)

其中：

式(11)中的各值为：

r₁₁＝cosγcosβ,

r₁₂＝-sinγcosα+cosγsinβsinα,

r₁₃＝sinγsinα+cosγsinβcosα,

r₂₁＝sinγcosβ,

r₂₂＝cosγcosα+sinγsinβsinα,

r₂₃＝-cosγsinα+sinγsinβcosα,

r₃₁＝-sinβ

r₃₂＝cosβsinα

r₃₃＝cosβcosα

Rx(α)，Ry(β)，Rz(γ)分别表示绕着x，y，z轴的旋转，利用三角函数的等价相似性可知，如果模型点集与场景点集的姿态相似，我们可以将R部分中的三角函数进行等价代换，此时M中的旋转部分R可写为式(12)的形式：

因此，变换矩阵M可以表示为：

此时，式(8)也可转化为：

其中：

对于N组相关的点对，通过式(15)可获得N组线性方程组，这些方程组可写为为Ax-b的形式，其中：

求解M_opt的问题就转化为了求解X_opt的问题，这是一个标准的线性优化问题：

通过SVD分解来完成式(18)的求解，对A进行SVD分解可得到A＝UΣV^T，从而计算A的伪逆A⁺＝VΣ⁺U^T，那么式(19)的线性最小二乘解为：

x_opt＝A⁺·b (19)

三维物体空间位姿初估计的二次校正完成，X_opt即为三维物体空间位姿的最终识别结果。

三维物体空间位姿初估计的二次校正完成，X_opt即为三维物体空间位姿的最终识别结果。

定位精度高是因为算法采用了哈希描述这种方法，在算法还没有运行的时候，就形成了目标模型M的哈希表，在算法运行阶段，只需要计算场景的哈希特征，然后直接查目标模型M的哈希表就可以得到快速找到相似的特征，从而定位物体。

在实际中，如图7所示，为一个待测量的场景，通过哈希描述之后匹配的结果如图8所示，可以看到，有一定误差，这是因为在计算哈希描述中，使用d_dist和N_angle进行了离散化处理，精度是有损失的，但是通过查哈希表的方式，能够实现快速的粗定位，也就是能够快速到达目标位置的周围，然后采用迭代最近点的方式进行二次修正，如图9所示，图9就是迭代最近点修正后的结果。同时，由于已知哈希描述方法能够像图8一样定位到目标的大致位置，所以可以采用点到切平面的迭代最近点算法，

特别说明的是，还有一种叫点到点的迭代最近点的方法，点到点的迭代最近点方法是最常用的，一般情况下点到点的迭代最近点方法要比点到切平面的迭代最近点方法要快，是因为在进行定位之前，我们往往会把目标模型M放置到场景中的某个位置，离要定位的位置有一定距离，点到点的迭代最近点方法相对于点到切平面的迭代最近点方法要少计算点的切平面这个步骤。

对于本专利，由于使用哈希描述，已经能够定位到目标位置附近，因此可以对点到切平面的迭代最近点方法中的非线性化部分进行线性化处理，可线性化处理可也是点到切平面的迭代最近点方法的一个独特的地方。相当于本专利整合了两个算法，使他们能够利用二者的优势，相互弥补劣势。

哈希描述的优势是定位速度较快，劣势是定位精度不高，因为其中的离散化过程会造成精度损失。

点到切平面的迭代最近点方法的优势是当初始位置如果接近真实位置的时候，如图8所示，可以将算法中的非线性部分进行线性化，劣势是如果初始位置距离真实位置较远，定位的速度会很慢。

可见，定位精度高体现在使用了迭代最近点的方式对哈希描述的定位结果进行二次校正。速度快体现在两点：

1)是采用了哈希描述进行查表比较，速度很快；

2)利用了哈希描述和点到切平面的迭代最近点方法结合了两种方法各自的优点，弥补了二者的缺点，使得本来比较慢的点到切平面方法通过线性化实现了算法加速。

以上实施例仅用以说明本发明的技术方案，而非对发明的保护范围进行限制。显然，所描述的实施例仅仅是本发明部分实施例，而不是全部实施例。基于这些实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明所要保护的范围。

尽管参照上述实施例对本发明进行了详细的说明，本领域普通技术人员依然可以在不冲突的情况下，不作出创造性劳动对本发明各实施例中的特征根据情况相互组合、增删或作其他调整，从而得到不同的、本质未脱离本发明的构思的其他技术方案，这些技术方案也同样属于本发明所要保护的范围。

Claims (8)

1.一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，包括深度相机和PC机，还包括以下步骤：

S1、深度相机采集需要检测的物体的场景数据；

S2、深度相机向场景发射红外射线，深度相机内的红外接收器将接收场景的近红外反射，生成场景的三维点数据，即场景中的每个反射点相对于深度相机坐标系的XYZ信息；

S3、深度相机将采集到的场景数据传到PC机进行保存，保存格式为PLY格式；

S4、利用S3获取的PLY格式的场景数据提取检测物体的目标模型M；

S5、PC机计算三维模型点云的每个点的协方差矩阵，求取协方差矩阵的特征向量，获取每个点的法向量；

S6、根据S5中的法向量生成目标模型M的点对特征；

S7、将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述；

S8、利用深度相机和PC机对场景S进行处理，获得场景S的哈希值，访问目标模型M的哈希描述，快速找到匹配的相似点对，将匹配结果转换为霍夫投票值；

S9、通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果；

S10、通过对迭代最近点算法对初检测结果进行二次矫正，得出精确的目标位姿检测结果。

2.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，所述步骤S4，将场景数据的坐标定义为深度传感器之上，每个数据的坐标值为(x，y，z)，统计所有数据中z值的数量，去除出现最多的z值来剔除数据，进而获取待检测物体的目标模型M。

3.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，所述步骤S5，获取目标模型M中各三维数据点的法向量目标模型M＝{m₀,m₁,m₂,...,m_k-1,m_k}，各三维数据点的法向量可通过各点周围的相邻点来计算，为了计算点的法向量，首先需要获得该点与相邻点之间的协方差矩阵，假设要求取m_i点的法向量协方差矩阵C的计算如式(1)所示，R表示以m_i点为中心的球形空间的半径，距离值d_i(i∈{1,2,...,k})表示相邻点m_j与中心点m_i的欧式距离，通过式(1)计算m_i点与半径R内所有点的协方差矩阵C：

通过协方差矩阵可以计算出m_i点的法向量，对协方差矩阵C的进行特征值分解，可求得协方差矩阵C的特征值和特征向量，对于物体表面的点云数据，其在法线方向的空间分布变化最为微弱，因此中心点m_i的法向量则为协方差矩阵C最小特征值所对应的特征向量，以此类推，获取所有点的法向量

4.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，所述步骤S6中，利用各点的法向量来计算点对特征，对于三维点云数据中的点p_i与除该点以外的任意点p_j，通过两点可以组成点对，利用两点间的法向量与欧式距离可以形成点对的点对特征，点对特征的计算方式如式(2)所示：

其中表示两点的连线方向，表示两点连线的欧氏距离，表示p_i点的法相向量与两点连线方向的夹角，表示两点法向量之间的夹角，对于向量之间的夹角计算如公式(3)所示：

5.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，步骤S7中，将目标模型M的所有点对特征转换为哈希值，生成目标模型M的哈希描述，定义点云集p中的任意点对(p_i，p_j)∈p，点对特征的表达式如式(2)所示，利用式(4)将点对特征转化为哈希值：

其中d_dist表示采样步长，d_angle表示采样角度，d_angle＝2π/N_angle，N_angle表示角度粒度，使用者可以根据实际结果调整N_angle，通常可将其设置为30。通过d_dist、d_angle来对点对特征进行离散化处理，使得相似特征能够转化为相同的特征值，对点对特征F离散化过后，点对特征F(p₁,p₂)_discretized＝F(f₁,f₂,f₃,f₄)的哈希值由(5)式获得：

Index＝P₁*f₁+P₂*f₂+P₃*f₃+f₄ (5)

公式(5)中的P1、P2、P3为3个不同的质数，这是为了使得转化后的哈希值唯一，统计所有点对的哈希值，将相同哈希值的点便可通过哈希表进行集中访问，所有哈希值组成哈希表，该哈希表称为点云数据的哈希描述。

6.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，步骤S8中，利用哈希描述生成霍夫投票值，定义目标模型M，场景的三维点云数据集为S，根据步骤S5生成目标模型点云数据集的哈希描述，通过将场景数据集S的点对转化为哈希值，访问目标模型M生成的哈希描述，进而实现快速的相似特征匹配，再将匹配结果转换为霍夫值，进行全局霍夫投票。

7.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，步骤S9，通过全局霍夫投票累加器对霍夫值进行统计，得分最高的霍夫值作为场景S的最优霍夫值，通过最优霍夫值生成对目标模型M的初检测结果，霍夫投票值(m_r,α)中的m_r直接由(m_r，m_i)获得，而角度α则由式(6)计算而来,定义世界坐标系W，式(6)中的和描述了将点s_r和m_r移动到世界单位坐标系原点所做的位移运动以及将两点的法向量与同世界单位坐标系的主轴x对齐所做的旋转变换：

对于具有相同哈希值的点对(m_r，m_i)∈M和(s_r，s_i)∈S，首先将点s_r与点m_r的法向量旋转到与世界单位坐标系W的主轴x平行且方向一致，然后移动两点的位置与世界单位坐标系的原点重合，这时点对(m_r，m_i)只需绕着世界单位坐标系的x轴旋转α角度即可与点对(s_r，s_i)重合，通过世界单位坐标系计算出与之后，根据式(6)就可以解出角度α，场景点s_r的霍夫投票值(m_r,α)求解完毕；

接下来通过投票选举最优霍夫投票值,建立了_n×m的全局霍夫投票累加器来收集各个霍夫投票值的票数，二维累加器的行元素为模型点云中所筛选出的各参考点行数n等于参考点的个数，累加器的列数m则与角度粒度N_angle有关，当N_angle＝30时投票累加器的列数为30；

场景参考点s_r会与其他的场景点s_i组成点对，每一组场景点对都可能寻找到若干相似的模型点对(m_r，m_i)，这些点对表示了(s_r，s_i)可能在目标模型M中存在的位置，计算场景点对(s_r，s_i)与相似点对(m_r，m_i)之间的角度α，就可获得场景点s_r的多个霍夫投票值(m_r,α)，对应投票累加器中位置加一，当前场景参考点与其他场景点组成的点对都完成霍夫投票值的计算后，投票累加器中的峰值将作为当前场景点的最优霍夫投票值，将最优霍夫投票结果带入式(7)，即可以获得目标模型M与场景S之间的初位姿检测结果RigidTrans：

8.根据权利要求1所述的一种基于哈希描述及迭代最近点的三维物体检测方法，其特征在于，步骤S10，使用迭代最近点算法对初检测结果进行二次修正，初检测结果已经反应了目标模型M与场景S之间的位姿关系，由于初检测结果正好满足相关点姿态相近的条件，可以对迭代最近点算法进行线性化处理，使得迭代最近点算法在保障精度的同时拥有更快的速度；

采用点到切平面的迭代最近点算法来对初检测结果进行二次修正，对于点到切平面的迭代最近点算法，误差函数的优化目标为模型点到对应场景点的切平面的均方误差达到最小；

误差函数如公式(8)所示：

式(8)中m_i＝(m_ix,m_iy,m_iz,T_im)，T_im表示模型点集中的某点，而s_i＝(s_ix,s_iy,s_iz,T_is)，T_is表示m_i在目标点云中的对应点，n_i＝(n_ix,n_iy,n_iz,0)则是点s_i的法向量；

M为4x4的旋转变换矩阵，由式(8)可见，当目标模型M按照变换矩阵M运动之后，移动后的模型点m_i在与场景点s_i做差，得到一个描述位移差的向量，这个向量再与点s_i的法向量做点积，点积的结果可用来估计一个点到另一个点切平面的远近，当所有模型点m_i按照M矩阵进行移动过后，使得式(8)的累计和达到最小，此时的M就为M_opt；

由于模型点集与场景点集的姿态相似，可以采用点到切平面的迭代最近点算法来将非线性问题朝着线性问题进行逼近，从而使得迭代最近点算法的数学形式更加简单，算法运行速度更加快捷；

对于描述位移和旋转的变换矩阵M，其由旋转部分R(α,β,γ)与位移部分T(t_x,t_y,t_z)组成，如式(9)所示：

M＝T(t_x,t_y,t_z)·R(α,β,γ) (9)

其中：

式(11)中的各值为：

r₁₁＝cosγcosβ,

r₁₂＝-sinγcosα+cosγsinβsinα,

r₁₃＝sinγsinα+cosγsinβcosα,

r₂₁＝sinγcosβ,

r₂₂＝cosγcosα+sinγsinβsinα,

r₂₃＝-cosγsinα+sinγsinβcosα,

r₃₁＝-sinβ

r₃₂＝cosβsinα

r₃₃＝cosβcosα

Rx(α)，Ry(β)，Rz(γ)分别表示绕着x，y，z轴的旋转，利用三角函数的等价相似性可知，如果模型点集与场景点集的姿态相似，我们可以将R部分中的三角函数进行等价代换，此时M中的旋转部分R可写为式(12)的形式：

因此，变换矩阵M可以表示为：

此时，式(8)也可转化为：

其中：

对于N组相关的点对，通过式(15)可获得N组线性方程组，这些方程组可写为为Ax-b的形式，其中：

求解M_opt的问题就转化为了求解X_opt的问题，这是一个标准的线性优化问题：

通过SVD分解来完成式(18)的求解，对A进行SVD分解可得到A＝UΣV^T，从而计算A的伪逆A⁺＝VΣ⁺U^T，那么式(18)的线性最小二乘解为：

x_opt＝A⁺·b (19)

三维物体空间位姿初估计的二次校正完成，X_opt即为三维物体空间位姿的最终识别结果。