CN109272523B - 基于改进cvfh和crh特征的随机堆放活塞位姿估计方法 - Google Patents

Abstract

基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，包括离线处理以及在线识别：离线处理先通过对Kinect进行离线标定，之后通过活塞的CAD模型自动渲染生成多视角点云，计算其点云特征，最后通过对点云和特征进行索引生成离线模板库；在线识别是对随机堆放的活塞进行位姿估计，首先使用标定好的Kinect对活塞点云进行采集和矫正，然后对点云进行预处理和分割，对于分割后的点云，计算其点云特征并与模板库进行初始配准，之后以该位姿为初值，通过ICP算法对点云进行精确配准，得到点云的精确位姿，对于生成的精确位姿，通过假设验证算法剔除错误匹配，最终完成活塞位姿的估计以及输出；本发明具有时间复杂度低，匹配模版获取方便，位姿估计准确率高的优点。

Description

基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法

技术领域

本发明涉及机器视觉的物体位姿估计技术领域，具体涉及基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法。

背景技术

在机械制造领域内，上料工序一直是较为薄弱的环节，其时间消耗较大、效率较低，而且该工序危险性较高，多数的安全事故均发生在上下料操作中。解决上料自动化的问题，对于减轻工人劳动强度、提高生产效率、实现多机床管理以及保证安全生产具有重要意义。目前，国产活塞生产线的上料工序先由人工将活塞按照要求的位姿放置，机械手再进行抓取，不能满足高效柔性加工生产线的要求。利用机器视觉算法识别工件的位姿，并引导机器人抓取工件，可省去人工环节，提高生产线的柔性，实现该目标的核心是位姿估计技术。现有的位姿估计方法存在以下一些问题：

目标对象结构以及堆放形式简单，并未针对真正的工件以及完全随机的堆放进行研究以及试验，方法验证的条件较为简单，无法实现真正的工厂环境。

匹配模板获取复杂，大多研究无法实现工件的CAD数字模型与传感器采集点云之间的匹配，其匹配模板一般通过人工进行采集。这种采集方式精度低、耗时长，还需要专业设备如三维旋转平台和激光扫描仪等，而且还需相关技术人员配合，扫描后仍需对每个点云的姿态进行标定提取，过程复杂，难以实现系统的快速布置。

方法适应性差，很多相关算法为了实现快速准确的检测识别，其算法的设计与抓取工件特有的特征联系紧密，一旦工件更换，算法效果会大打折扣，参数的调整难度很大。

系统成本高，相关研究及产品使用的传感器以及计算设备十分昂贵，如高精度3D采集系统以及工业级服务器，整套系统成本很高，很大程度上会影响企业进行产品引进的积极性。

综上所述，目前的位姿估计方法，具有实验环境和研究对象不真实、匹配模板获取复杂、方法适应性差以及系统成本高的缺点。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，通过机器视觉的方法实现随机堆放活塞的六自由度位姿估计，具有时间复杂度低，匹配模版获取方便，位姿估计准确率高的优点。

为了达到上述目的，本发明采取的技术方案为：

基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，包括以下步骤：

1)离线处理：离线处理只需在设备安装时进行一次，包括Kinect相机的标定以及活塞离线模版库的自动生成，具体步骤如下：

1.1)对Kinect相机进行标定，并完成随机堆放活塞表面点云的采集：首先通过Kinect对标定板的彩色图像以及视差图像进行采集，之后通过彩色图像和深度图像的角点分别对彩色相机以及深度相机进行初始化标定，然后通过初始标定结果对彩色相机和深度相机的相对位姿进行求解，之后联合彩色相机和深度相机数据对标定参数和视差图畸变系数进行迭代优化，完成整个相机参数的标定；

1.2)、离线模版库的构造，通过活塞的CAD模型自动生成离线匹配模版，具体的步骤如下：

1.2.1)对待抓取的活塞进行CAD建模；

1.2.2)在CAD模型外构造一正二十面体，并对正二十面体的每个三角形表面进行两次细化，在细化后的每个顶点放置虚拟相机，相机滚转轴指向中心；

1.2.3)每个虚拟相机依次对CAD模型表面进行点云采样，并对采样结果进行体素化滤波，保证模板点云和匹配点云密度的一致性；

1.2.4)计算每个模版点云的改进聚类视点特征直方图(Clustered ViewpointFeature Histograms，CVFH)以及相机滚转角直方图(Camera Roll Histograms，CRH)特征，对其进行层次聚类，并构造线性搜索索引；

1.2.5)对点云及其对应位姿进行保存，完成离线模版的自动构建；

2)在线识别：具体步骤如下：

2.1)点云采集，通过Kinect对随机堆放活塞的彩色图像以及深度图像进行采集，并根据Kienct相机标定结果将其转化为具有颜色信息的点云，完成点云的采集；

2.2)点云预处理；

2.3)点云分割，采用改进的基于最小割的点云分割算法对活塞点云进行分割，改进算法首先将三维点云转换为二维的深度图像，通过构造的滤波模板对深度图像进行滤波，选取滤波后灰度值较大的点为前景点，实现多前景点的自动选取，以选取的每个前景点作为最小割算法的前景点进行点云分割，由此将最小割算法由交互式的半自动二分割算法扩展为全自动的多分割算法；

2.4)特征计算，针对分割后的每个活塞点云，计算其改进的CVFH特征和CRH特征；

改进的CVFH特征以及CRH特征，在特征原有的基础上，针对其视点位置固定以及聚类中心不稳定的缺点，通过在特征计算前对点云进行自适应的平移操作以及通过在点云最高点进行聚类选取聚类中心点的方法对其缺陷进行了改进；

2.5)初始配准；

2.6)精确配准，位姿的精确配准采用的是迭代最近点(Iterative ClosestPoint)算法；

2.7)假设验证，通过两点云之间的均方误差以及重合度两个指标进行综合验证的假设验证算法；

点云间的均方误差E为ICP变换的返回值，反映了点云精匹配准确度，均方误差值越小，表示其匹配精度越高，设定均方误差的阈值为T1；

定义点云间重合度C，其计算方式如下：

设定搜索半径r，活塞点云为P，模板点云为Q，若点p_i满足p_i∈P,

则定义其为重合点，重合度C定义为式：

C＝n/N

其中，n为重合点个数；N为点云P的大小；

重合度值越接近1表明点云间的重合度越高以及匹配效果越好，设定重合度的阈值为T2；

2.8)位姿输出，将位姿估计成功的活塞位姿进行输出，为活塞的抓取上料提供位姿估计值。

所述的步骤2.2)点云预处理，包括以下步骤：

2.2.1)点云下采样，通过体素化网格滤波算法对点云进行下采样处理；

2.2.2)料箱内壁干扰去除，通过直通滤波结合随机采样一致性平面估计的方法去处料箱内壁的干扰；

2.2.3)离群点去除，通过离群点滤波算法对离群点进行剔除，最终完成点云的预处理。

所述的步骤2.3)点云分割，具体的步骤如下：

2.3.1)深度图转换：

首先将三维点云转换为二维深度图像，先遍历点云，确定点云在x轴以及y轴方向的边界x_min，x_max，y_min，y_max，单位mm，确定深度图像的大小如式：

rows＝ceil(x_max-x_min)+1

cols＝ceil(y_max-y_min)+1

式中，ceil(*)为取整函数；

对于数据点(x,y,z)，其在深度图中的坐标为(ceil(x-x_min),ceil(y-y_min))，该点的灰度值为z，依照该准则对点云向深度图进行映射；

2.3.2)图像滤波：

构造大小为150*150的图像滤波模板，模板内点(x,y)的值v如式：

式中，μ₁＝62.5；σ₁＝12.5；μ₂＝75；σ₂＝6.25；r为点到模板中心的距离

使用构造的模板对深度图进行滤波操作；

2.3.3)提取前景点：

对深度图滤波结果进行阈值化处理，对其像素值进行由高到低排序，并根据以下规则对前景点进行选取：

根据像素值由高到低选取像素点，若像素点与已选择前景点的距离大于设定阈值，则选择该点为前景点，否则选取下一个像素点进行判断；

2.3.4)点云直通滤波：

针对每一个自动提取的前景点，将重新其转化为三维坐标，然后以该三维点坐标为中心，根据设定的偏移量，对活塞点云进行直通滤波；

2.3.5)点云分割：

对于初步分割的点云结果，以前景点为强约束，活塞半径为弱约束，通过最小割算法进行点云分割；

2.3.6)分割结果验证：

通过点云数量对分割结果进行初步验证，设定点云数量阈值T，若分割后点云内点的数量大于T，则点云满足要求，进行下一步处理；若分割点云内点的数量小于T，则认为该点云为错误分割，并对该点云进行剔除。

所述的步骤2.5)初始配准，具体步骤如下：

2.5.1)计算活塞点云的改进CVFH和CRH特征；

2.5.2)将改进的CVFH特征通过特征索引进行搜索，确定匹配度最佳的前N个模板点云；

2.5.3)对于每个模板点云，在模板库中搜索其对应的CRH特征，并与活塞点云的CRH特征做互相关分析，取其峰值点所对应的相位差即为相机滚转角，对于每个CRH特征通常会确定2-3个滚转角；

2.5.4)对于每个滚转角，将模板点云进行相应的旋转，得到初始配准点云，并保存初始配准点云当前的初始位姿。

所述的步骤2.7)假设验证的具体步骤如下：

2.7.1)根据精确配准的均方误差E将精确配准点云进行有小到大的排序；

2.7.2)剔除均方误差大于阈值T₁的精配准点云；

2.7.3)计算剩余精配准点云与活塞点云的重合度C；

2.7.4)逐个对剩余精配准点云的重合度进行检验；

2.7.5)若存在重合度C大于阈值T2，则该精确配准点云为正确匹配点云，最终取重合度最大的精确配准点云对应的位姿为该活塞的位姿，检验结束；若不存在重合度C大于阈值T2，则位姿估计失败。

本发明的有益效果为：

1.本发明可实现工业环境下的随机堆放活塞的位姿识别，具有模版获取方便，算法适应性强，系统成本低的特点。

2.本发明对现有的CVFH特征和CRH特征进行了改进，提高了其特征提取的稳定性以及特征匹配的准确率。

3.本发明对现有的基于最小割的点云分割算法进行了改进，通过自动选取多个前景点的方式将其由交互式的二分割算法改进成为全自动的多分割算法。

附图说明

图1为本发明方法的流程图。

具体实施方式

下面结合附图和实例对本发明作详细描述。

参照图1，基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，包括以下步骤：

1)离线处理：离线处理只需在设备安装时进行一次，包括Kinect相机的标定以及活塞离线模版库的自动生成，具体步骤如下：

1.1)对Kinect相机进行标定，并完成随机堆放活塞表面点云的采集：首先通过Kinect对标定板的彩色图像以及视差图像进行采集，之后通过彩色图像和深度图像的角点分别对彩色相机以及深度相机进行初始化标定，然后通过初始标定结果对彩色相机和深度相机的相对位姿进行求解，之后联合彩色相机和深度相机数据对标定参数和视差图畸变系数进行迭代优化，完成整个相机参数的标定；

1.2)离线模版库的构造，通过活塞的CAD模型自动生成离线匹配模版，具体的步骤如下：

1.2.1)对待抓取的活塞进行CAD建模；

1.2.2)在CAD模型外构造一正二十面体，并对正二十面体的每个三角形表面进行两次细化，在细化后的每个顶点放置虚拟相机，相机滚转轴指向中心；

1.2.3)每个虚拟相机依次对CAD模型表面进行点云采样，并对采样结果进行体素化滤波，保证模板点云和匹配点云密度的一致性；

1.2.4)计算每个模版点云的改进CVFH特征和CRH特征，对其进行层次聚类，并构造线性搜索索引；

1.2.5)对点云及其对应位姿进行保存，完成离线模版的自动构建；

2)在线识别：离线处理进行完毕之后，便可通过在线识别对活塞的位姿进行估计，在线识别包含点云采集、点云预处理、点云分割、特征计算、初始配准、精确配准、假设验证以及位姿输出，具体步骤如下：

2.1)点云采集，通过Kinect对随机堆放活塞的彩色图像以及深度图像进行采集，并根据Kienct相机标定结果将其转化为具有颜色信息的点云，完成点云的采集；

2.2)点云预处理，点云采集完成之后，在点云处理前，需要首先对点云进行预处理，通过降低点云密度，去除料箱内壁以及噪声点的干扰进行点云的预处理，目的是降低点云处理的时间以及提高算法的准确性。点云预处理主要包括以下步骤：

2.2.1)点云下采样，下采样的主要目的是降低数据点的数量，加快算法的运行速度，通过体素化网格滤波算法对点云进行下采样处理；

2.2.2)料箱内壁干扰去除，料箱内壁点云的存在会对后续的活塞点云分割以及位姿估计步骤造成干扰，通过直通滤波结合随机采样一致性平面估计的方法去处料箱内壁的干扰；

2.2.3)离群点去除，由于传感器噪声以及平面剔除的影响，点云之中存在一些稀疏的离群点，这些离群点会导致点云局部特征的计算(如法向量)出现较大的偏差，影响之后点云分割、特征计算以及点云配准等处理步骤，通过离群点滤波算法对离群点进行剔除，最终完成点云的预处理；

2.3)点云分割，为实现基于全局特征的活塞位姿估计，需要在进行位姿估计前对散乱堆放的活塞点云进行自动分割，得到单个活塞的表面点云，采用改进的基于最小割的点云分割算法对活塞点云进行分割，改进算法首先将三维点云转换为二维的深度图像，通过构造的滤波模板对深度图像进行滤波，选取滤波后灰度值较大的点为前景点，实现多前景点的自动选取，以选取的每个前景点作为最小割算法的前景点进行点云分割，由此将最小割算法由交互式的半自动二分割算法扩展为全自动的多分割算法，具体的步骤如下：

2.3.1)深度图转换；

相比于三维点云滤波操作，二维图像由于其精简的数据结构(可以在常数时间内获取像素点的邻域)，可以实现更加快速的滤波操作。为了便于数据的快速处理，首先将三维点云转换为二维深度图像，先遍历点云，确定点云在x轴以及y轴方向的边界x_min，x_max，y_min，y_max(单位mm)，确定深度图像的大小如式：

rows＝ceil(x_max-x_min)+1

cols＝ceil(y_max-y_min)+1

式中，ceil(*)为取整函数；

对于数据点(x,y,z)(单位mm)，其在深度图中的坐标为(ceil(x-x_min),ceil(y-y_min))，该点的灰度值为z，依照该准则对点云向深度图进行映射；

2.3.2)图像滤波：

构造大小为150*150的图像滤波模板，模板内点(x,y)的值v如式：

式中，μ₁＝62.5；σ₁＝12.5；μ₂＝75；σ₂＝6.25；r为点到模板中心的距离

使用构造的模板对深度图进行滤波操作；

2.3.3)提取前景点；

对深度图滤波结果进行阈值化处理，对其像素值进行由高到低排序，并根据以下规则对前景点进行选取：

根据像素值由高到低选取像素点，若像素点与已选择前景点的距离大于设定阈值，则选择该点为前景点，否则选取下一个像素点进行判断；

2.3.4)点云直通滤波：

针对每一个自动提取的前景点，将重新其转化为三维坐标，然后以该三维点坐标为中心，根据设定的偏移量，对活塞点云进行直通滤波，目的是将目标活塞点云进行一个初步的分割，降低下一步点云分割算法的所需处理的数据量，加快算法运行速度；

2.3.5)点云分割：

对于初步分割的点云结果，以前景点为强约束，活塞半径为弱约束，通过最小割算法进行点云分割；

2.3.6)分割结果验证：

通过点云数量对分割结果进行简单的初步验证，设定点云数量阈值T，若分割后点云内点的数量大于T，则点云满足要求，可进行下一步处理；若分割点云内点的数量小于T，则认为该点云为错误分割，并对该点云进行剔除；

2.4)特征计算，针对分割后的每个活塞点云，计算其改进的CVFH特征和CRH特征；

改进的CVFH特征以及CRH特征，在特征原有的基础上，针对其视点位置固定以及聚类中心不稳定的缺点，通过在特征计算前对点云进行自适应的平移操作以及通过在点云最高点进行聚类选取聚类中心点的方法对其缺陷进行了改进，提高了特征提取的稳定性以及特征匹配的正确率；

2.5)初始配准，点云初始配准主要是针对CVFH特征以及CRH特征的匹配，目的是通过初始配准得到点云的粗略的位姿，并为下一步的精确配准提供良好的初始值，具体的步骤如下：

2.5.1)计算活塞点云的改进CVFH和CRH特征；

2.5.2)将改进的CVFH特征通过特征索引进行搜索，确定匹配度最佳的前N个模板点云；

2.5.3)对于每个模板点云，在模板库中搜索其对应的CRH特征，并与活塞点云的CRH特征做互相关分析，取其峰值点所对应的相位差即为相机滚转角，对于每个CRH特征通常会确定2-3个滚转角；

2.5.4)对于每个滚转角，将模板点云进行相应的旋转，得到初始配准点云，并保存初始配准点云当前的初始位姿；

2.6)精确配准，由于离线模板库是通过离散的采样提取的点云模板，CRH特征也只有4度的分辨率，而且初始配准并未对点云三个位移自由度进行配准，所以结果具有一定的误差，无法达到机械手抓取的精度要求。在初始匹配之后再进行一步精确的配准，目的是进一步提高位姿的精度，位姿的精确配准采用的是迭代最近点(Iterative Closest Point)算法；

2.7)假设验证，完成精确配准之后，对于每个分割的活塞点云均得到了若干位姿假设，位姿假设的正确性仍需进行最终的假设验证，假设验证的目的是剔除错误的位姿假设，保证最终估计位姿的正确性，通过两点云之间的均方误差以及重合度两个指标进行综合验证的假设验证算法；

点云间的均方误差E为ICP变换的返回值，反映了点云精匹配准确度，均方误差值越小，表示其匹配精度越高，设定均方误差的阈值为T1；

定义点云间重合度C，其计算方式如下：

设定搜索半径r，活塞点云为P，模板点云为Q，若点p_i满足p_i∈P,

则定义其为重合点，重合度C定义为式：

C＝n/N

其中，n为重合点个数；N为点云P的大小；

重合度值越接近1表明点云间的重合度越高以及匹配效果越好，设定重合度的阈值为T2；

假设验证算法的具体步骤如下：

2.7.1)根据精确配准的均方误差E将精确配准点云进行有小到大的排序；

2.7.2)剔除均方误差大于阈值T₁的精配准点云；

2.7.3)计算剩余精配准点云与活塞点云的重合度C；

2.7.4)逐个对剩余精配准点云的重合度进行检验；

2.7.5)若存在重合度C大于阈值T2，则该精确配准点云为正确匹配点云，最终取重合度最大的精确配准点云对应的位姿为该活塞的位姿，检验结束；若不存在重合度C大于阈值T2，则位姿估计失败；

2.8)位姿输出，将位姿估计成功的活塞位姿进行输出，为活塞的抓取上料提供位姿估计值。

通过CVFH和CRH特征改进前以及改进后的算法进行随机堆放活塞位姿估计的对比实验，实验结果如表1所示，从表1可以看出，特征改进后，平均识别时间降低了30.49％，位姿估计准确率提高了3.64％，由此可以验证特征改进的有效性。

表1位姿估计对比实验

Claims (4)

1.基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，其特征在于，包括以下步骤：

1)离线处理：离线处理只需在设备安装时进行一次，包括Kinect相机的标定以及活塞离线模版库的自动生成，具体步骤如下：

1.1)对Kinect相机进行标定，并完成随机堆放活塞表面点云的采集：首先通过Kinect对标定板的彩色图像以及视差图像进行采集，之后通过彩色图像和深度图像的角点分别对彩色相机以及深度相机进行初始化标定，然后通过初始标定结果对彩色相机和深度相机的相对位姿进行求解，之后联合彩色相机和深度相机数据对标定参数和视差图畸变系数进行迭代优化，完成整个相机参数的标定；

1.2)、离线模版库的构造，通过活塞的CAD模型自动生成离线匹配模版，具体的步骤如下：

1.2.1)对待抓取的活塞进行CAD建模；

1.2.2)在CAD模型外构造一正二十面体，并对正二十面体的每个三角形表面进行两次细化，在细化后的每个顶点放置虚拟相机，相机滚转轴指向中心；

1.2.3)每个虚拟相机依次对CAD模型表面进行点云采样，并对采样结果进行体素化滤波，保证模板点云和匹配点云密度的一致性；

1.2.4)计算每个模版点云的改进聚类视点特征直方图(Clustered ViewpointFeature Histograms，CVFH)以及相机滚转角直方图(Camera Roll Histograms，CRH)特征，对其进行层次聚类，并构造线性搜索索引；

1.2.5)对点云及其对应位姿进行保存，完成离线模版的自动构建；

2)在线识别：具体步骤如下：

2.1)点云采集，通过Kinect对随机堆放活塞的彩色图像以及深度图像进行采集，并根据Kienct相机标定结果将其转化为具有颜色信息的点云，完成点云的采集；

2.2)点云预处理；

2.3)点云分割，采用改进的基于最小割的点云分割算法对活塞点云进行分割，改进算法首先将三维点云转换为二维的深度图像，通过构造的滤波模板对深度图像进行滤波，选取滤波后灰度值较大的点为前景点，实现多前景点的自动选取，以选取的每个前景点作为最小割算法的前景点进行点云分割，由此将最小割算法由交互式的半自动二分割算法扩展为全自动的多分割算法；

2.4)特征计算，针对分割后的每个活塞点云，计算其改进的CVFH特征和CRH特征；

改进的CVFH特征以及CRH特征，在特征原有的基础上，针对其视点位置固定以及聚类中心不稳定的缺点，通过在特征计算前对点云进行自适应的平移操作以及通过在点云最高点进行聚类选取聚类中心点的方法对其缺陷进行了改进；

2.5)初始配准；

2.6)精确配准，位姿的精确配准采用的是迭代最近点(Iterative Closest Point)算法；

2.7)假设验证，通过两点云之间的均方误差以及重合度两个指标进行综合验证的假设验证算法；

点云间的均方误差E为ICP变换的返回值，反映了点云精匹配准确度，均方误差值越小，表示其匹配精度越高，设定均方误差的阈值为T1；

定义点云间重合度C，其计算方式如下：

设定搜索半径r，活塞点云为P，模板点云为Q，若点p_i满足

则定义其为重合点，重合度C定义为式：

C＝n/N

其中，n为重合点个数；N为点云P的大小；

重合度值越接近1表明点云间的重合度越高以及匹配效果越好，设定重合度的阈值为T2；

2.8)位姿输出，将位姿估计成功的活塞位姿进行输出，为活塞的抓取上料提供位姿估计值；

所述的步骤2.7)假设验证的具体步骤如下：

2.7.1)根据精确配准的均方误差E将精确配准点云进行有小到大的排序；

2.7.2)剔除均方误差大于阈值T₁的精配准点云；

2.7.3)计算剩余精配准点云与活塞点云的重合度C；

2.7.4)逐个对剩余精配准点云的重合度进行检验；

2.7.5)若存在重合度C大于阈值T2，则该精确配准点云为正确匹配点云，最终取重合度最大的精确配准点云对应的位姿为该活塞的位姿，检验结束；若不存在重合度C大于阈值T2，则位姿估计失败。

2.根据权利要求1所述的基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，其特征在于：所述的步骤2.2)点云预处理，包括以下步骤：

2.2.1)点云下采样，通过体素化网格滤波算法对点云进行下采样处理；

2.2.2)料箱内壁干扰去除，通过直通滤波结合随机采样一致性平面估计的方法去处料箱内壁的干扰；

2.2.3)离群点去除，通过离群点滤波算法对离群点进行剔除，最终完成点云的预处理。

3.根据权利要求1所述的基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，其特征在于：所述的步骤2.3)点云分割，具体的步骤如下：

2.3.1)深度图转换：

首先将三维点云转换为二维深度图像，先遍历点云，确定点云在x轴以及y轴方向的边界x_min，x_max，y_min，y_max，单位mm，确定深度图像的大小如式：

rows＝ceil(x_max-x_min)+1

cols＝ceil(y_max-y_min)+1

式中，ceil(*)为取整函数；

对于数据点(x,y,z)，其在深度图中的坐标为(ceil(x-x_min),ceil(y-y_min))，该点的灰度值为z，依照该准则对点云向深度图进行映射；

2.3.2)图像滤波：

构造大小为150*150的图像滤波模板，模板内点(x,y)的值v如式：

式中，μ₁＝62.5；σ₁＝12.5；μ₂＝75；σ₂＝6.25；r为点到模板中心的距离

使用构造的模板对深度图进行滤波操作；

2.3.3)提取前景点：

对深度图滤波结果进行阈值化处理，对其像素值进行由高到低排序，并根据以下规则对前景点进行选取：

根据像素值由高到低选取像素点，若像素点与已选择前景点的距离大于设定阈值，则选择该点为前景点，否则选取下一个像素点进行判断；

2.3.4)点云直通滤波：

针对每一个自动提取的前景点，将重新其转化为三维坐标，然后以该三维点坐标为中心，根据设定的偏移量，对活塞点云进行直通滤波；

2.3.5)点云分割：

对于初步分割的点云结果，以前景点为强约束，活塞半径为弱约束，通过最小割算法进行点云分割；

2.3.6)分割结果验证：

通过点云数量对分割结果进行初步验证，设定点云数量阈值T，若分割后点云内点的数量大于T，则点云满足要求，进行下一步处理；若分割点云内点的数量小于T，则认为该点云为错误分割，并对该点云进行剔除。

4.根据权利要求1所述的基于改进CVFH和CRH特征的随机堆放活塞位姿估计方法，其特征在于：所述的步骤2.5)初始配准，具体步骤如下：

2.5.1)计算活塞点云的改进CVFH和CRH特征；

2.5.2)将改进的CVFH特征通过特征索引进行搜索，确定匹配度最佳的前N个模板点云；

2.5.3)对于每个模板点云，在模板库中搜索其对应的CRH特征，并与活塞点云的CRH特征做互相关分析，取其峰值点所对应的相位差即为相机滚转角，对于每个CRH特征通常会确定2-3个滚转角；

2.5.4)对于每个滚转角，将模板点云进行相应的旋转，得到初始配准点云，并保存初始配准点云当前的初始位姿。

Images