6D位姿标注方法、系统及存储介质与流程

6d位姿标注方法、系统及存储介质
技术领域
1.本申请涉及机器视觉技术领域，特别涉及一种6d位姿标注方法、系统及存储介质。


背景技术：

2.6d位姿预测是3d视觉中一个关键技术，位姿即位置和姿态，位置可由空间坐标(x,y,z)进行描述，即平移，姿态可由三个方向的旋转(pitch,yaw,roll)来描述，所以物体的位姿可以用6个自由度的数据来描述。6d位姿预测旨在准确测量目标物体的3d旋转值和3d平移值，广泛应用在诸如机器人抓取、增强现实、自动驾驶等场景中。
3.6d位姿预测有很多种方法，深度学习的技术可以大大提高6d位姿预测的准确性，而随着深度学习技术的发展，6d位姿一般通过两种方式进行标注：一是通过人工数据标注，由于深度学习需要大量的数据进行支撑，人工标注6d位姿常常需要大量的时间和工作量，不仅造成较高的人工成本，而且会严重影响标注效率；二是通过采用方阵的方法制作大量的数据以供深度学习网络进行训练，再将训练好的网络针对现实场景进行预测，如此可以节省人工标注的时间，但是由于仿真数据和真实场景下的数据存在差异，使得深度学习训练得出的结果的准确性无法得到保证。


技术实现要素：

4.本申请旨在至少解决现有技术中存在的技术问题之一，为此，本申请提出一种6d位姿标注方法、系统及存储介质，能够高效且准确地得到6d位姿。
5.本申请的第一方面，提供了一种6d位姿标注方法，包括：获取目标物体的图像信息；其中，图像信息包括目标掩码和目标物体的深度图片；根据目标掩码，将目标物体深度图片转化为第一点云集合；粗匹配步骤：采用ppf算法对第一点云集合与预设的模型点云集合进行粗匹配，得到目标物体的第一位姿，并根据第一位姿将第一点云集合转换为第二点云集合；第一次精匹配：采用点对面的icp算法将第二点云集合与模型点云集合进行第一次精匹配，得到目标物体的第二位姿，并根据第二位姿将第二点云集合转换为第三点云集合；第二次精匹配：采用点对点的icp算法将第三点云集合与模型点云集合进行第二次精匹配，得到目标物体的第三位姿；根据第一位姿、第二位姿和第三位姿，计算出目标物体的目标位姿。
6.根据本申请第一方面实施例的6d位姿标注方法，至少具有如下有益效果：通过首先将第一点云集合与预设的模型点云集合进行粗匹配，得到目标物体的第一位姿，并由第一位姿将第一点云集合转换为第二点云集合；再将第二点云集合与预设的模型点云集合进行点对面的第一次精匹配，得到目标物体的第二位姿，并由第二位姿将第二点云集合转换为第三点云集合；最后将第三点云集合与预设的模型点云集合进行点对点的第二次精匹配，得到目标物体的第三位姿；最终将三次匹配得到的三个位姿进行计算，得到目标物体的目标位姿。整个过程通过采集目标物体的图像信息，再将目标物体转化的第一点云集合经过一系列的粗匹配以及两次精匹配，可以得到准确的目标位姿，而且由于不再需要进行人
工标注目标物体的位姿，可以有效地提高工作效率。
7.根据本申请的一些实施例，还包括：采用dbscan的点云去噪算法将第一点云集合进行去噪，得到去噪后的第一点云集合，并将去噪后的第一点云集合用于粗匹配步骤。采集到的第一点云集合由于光照等原因会包含有大量的噪点，而常用的基于半径和基于统计的点云去噪方法，对于少量离散的噪点效果很好，但针对大面积聚集型点云噪声效果有限。而基于dbscan的点云去噪方法，能很好的去除大面积的聚集型点云噪声，使得第一点云集合去噪之后再进行匹配能够有效的提高匹配精度。
8.根据本申请的一些实施例，采用dbscan的点云去噪算法将第一点云集合进行去噪，得到去噪后的第一点云集合，并将去噪后的第一点云集合用于粗匹配步骤，包括：获取第一点云集合中的多个核心点；其中，核心点为预设半径范围内的点数大于或者等于预设点数的点，核心点到核心点的预设半径范围内的点称为可达；采用基于密度的聚类算法，获取第一点云集合范围内的多个聚类点集合；其中，若第二核心点由第一核心点可达，第三核心点和第三非核心点由第二核心点可达，则第三核心点和第三非核心点均由第一核心点可达，一个核心点与其他所有可达的点形成一个聚类点集合；获取多个聚类点集合中点数最多的聚类点集合，并将点数最多的聚类点集合作为去噪后的第一点云集合。dbscan(density
‑
based spatial clustering of applications with noise，基于密度的聚类算法)将簇定义为密度相连的点的最大集合，能够把具有足够高密度的区域划分为簇。通过预先设置一个扫描半径即预设距离，则该预设距离为预设半径，若一个点的预设半径范围内的点数超过预设点数，则将该点称为核心点。核心点的预设半径范围内的所有点均由该核心点直接可达。另外，若第二核心点在第一核心点的预设半径范围内，则称为第二核心点由第一核心点可达；若第三核心点和第三非核心点均在第二核心点的预设半径范围内，则称为第三核心点和第三非核心点均由第二核心点可达；第三核心点和第三非核心点也可由第一核心点可达。第一核心点与所有由第一核心点可达的点可以形成一个聚类点集合。据此，在第一点云集合中，可以得到多个聚类点集合。在多个聚类点集合中选取点数最多的聚类点集合，其余点数较少的聚类点集合去掉，则将点数较多的聚类点集合作为去噪后的第一点云集合。
9.根据本申请的一些实施例，粗匹配步骤：采用ppf算法对第一点云集合与预设的模型点云集合进行粗匹配，得到目标物体的第一位姿，并根据第一位姿将第一点云集合转换为第二点云集合，包括：在模型点云集合中，根据每两点之间的点对特征，建立全局模型；将第一点云集合分为第一部分点集合和第二部分点集合，并计算出第二部分点集合与第一部分点集合的多个目标点对特征；将每个目标点对特征与全局模型进行匹配，并得到多个位姿的位姿集合；将位姿集合中个数最多的位姿作为第一位姿；根据第一位姿将第一点云集合转换得到第二点云集合。粗匹配的过程采用了ppf(point pair feature)算法，即基于点对特征的匹配算法。具体为：在离线阶段，在模型点云集合中建立全局模型；在线阶段，将第一点云集合中的点划分为两部分，即第一部分点集合和第二部分点集合，将第一部分点集合中的点作为参考点，并计算第二点集合中的点和第一部分点集合中的点对特征，并将这些点对特征和全局模型进行匹配，得到多个位姿的位姿集合，将位姿集合中个数最多的位姿作为第一位姿，第一点云集合根据第一位姿转换成第二点云集合。粗匹配过程可以将得到初步的位姿即第一位姿。
10.根据本申请的一些实施例，第一次精匹配，采用点对面的icp算法将第二点云集合与模型点云集合进行第一次精匹配，得到目标物体的第二位姿，并根据第二位姿将第二点云集合转换为第三点云集合，包括：获取目标点云集合中的每个点到模型点云集合中的多个点对应的多个切面的第一距离集合，并将第一距离集合中的最短距离对应的两个点作为一个点对；获取目标点云集合中的点与模型点云集合中的点形成的多个点对；获取多个点对之间的第一距离和；其中，第一距离和为目标点云集合中所有点到模型点云集合中对应点的切面的距离之和；将目标点云集合经过第一公式的迭代优化，将第一距离和取得最小值对应的位姿作为第二位姿；根据第二位姿将第二点云集合转换为第三点云集合。由于ppf算法的粗匹配过程得到的位姿结果不能满足作为神经网络的输入，因此需要利用icp(iterative closest points)算法对粗匹配过程得到的第一位姿进行迭代优化。首先采用点对面的icp算法：模型点云集合中的每个点都有对应的切面，而目标点云集合中的点到模型每个点云集合中的多个切面存在一个距离的集合，即第一距离集合，在第一距离集合中获取最短距离对应的两个点将作为一个点对；在目标点云集合中的点和模型点云集合中的多个点形成多个点对；计算多个点对的距离和，将目标点云集合经过第一公式的迭代优化，取得第一距离和最小值的位姿将作为第二位姿，第二点云集合经过第二位姿的转换得到第三点云集合。点对面(point to plane)的icp算法为迭代优化算法，优化目标函数即为第一公式。给定两个点云集合p＝{p1,p2,
…
,pn}，q＝{q1,q2,
…
,qn}，r为它们之间的旋转矩阵，t为平移矩阵，假设pi和qi是一一对应的，首先计算qi点处的法向量ni，再计算两点间距离在法向量上的投影，即点pi到qi处切面的距离，r和t为使得点到面距离和取得最小值的旋转和平移矩阵，旋转和平移矩阵即为所求的第二位姿。其中，第一公式为：
[0011][0012]
根据本申请的一些实施例，第二次精匹配：采用点对点的icp算法将第三点云集合与模型点云集合进行第二次精匹配，得到目标物体的第三位姿，包括：获取目标点云集合中的每个点到模型点云集合中的多个点的第二距离集合，并将第二距离集合中的最短距离对应的两个点作为一个点对；获取目标点云集合中的点与模型点云集合中的点形成的多个点对；获取多个点对的第二距离和；其中，第二距离和为目标点云集合中所有点到模型点云集合中对应点的距离之和；将目标点云集合经过第二公式的迭代优化，将第二距离和取得最小值对应的位姿作为第三位姿。对于特征较多、曲面较多的目标物体，例如工业产品中像法兰盘一样有大片点云在同一平面上的物体，单独的点对面的icp算法无法满足精匹配的要求，因此需要点对点(pointtopoint)的icp算法进行第二次精匹配，优化目标函数即为第二公式。给定两个点云集合p＝{p1,p2,
…
,pn}，q＝{q1,q2,
…
,qn}，r为它们之间的旋转矩阵，t为平移矩阵，假设pi和qi是一一对应的，点对点icp即为计算使得点对距离和取得最小值的旋转和平移矩阵。其中，第二公式为：
[0013][0014]
根据本申请的一些实施例，利用solid works建立目标物体的目标模型，得到目标物体的模型点云集合。通过采用solid works建立目标物体的目标模型，可以便捷的得到目标物体的模型点云集合。
[0015]
根据本申请的一些实施例，目标场景中包括多个目标物体，包括：执行获取目标场
景中目标物体的图像信息；若已获取全部目标物体的目标位姿，则结束位姿标注。由于目标场景中一般包括多个目标物体，可以通过上述方法对每个目标物体进行位姿标注，直到目标场景中的每个目标物体均完成位姿标注。实现了自动标注的目的。从真实场景中采集的数据相比较于现有方法中得到的仿真数据，能够更好地训练网络应对真实场景，从而大幅度提高神经网络在真实任务中的预测精度。
[0016]
本申请的第二方面，提供了一种6d位姿标注系统，包括：至少一个存储器、至少一个处理器及至少一个程序指令，程序指令存储在存储器上并可在处理器上运行，处理器用于执行本申请第一方面的6d位姿标注方法。
[0017]
本申请的第四方面提供了一种存储介质，存储介质上存储有程序指令，程序指令用于执行本申请第一方面的6d位姿标注方法。
[0018]
本申请的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
[0019]
本申请的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0020]
图1为本申请实施例的6d位姿标注方法的流程图；
[0021]
图2为本申请实施例的6d位姿标注方法的位姿标注流程示意图。
具体实施方式
[0022]
下面详细描述本申请的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能理解为对本申请的限制。
[0023]
在本申请的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于或等于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。
[0024]
术语解释：
[0025]
1、6d位姿：6d指6个自由度，包括3个自由度的平移(translation)和3个自由度的旋转(rotation)，2者合起来称为物体的位姿(pose)，通常指2个坐标系间的刚性转换。
[0026]
2、dbscan：density
‑
based spatial clustering of applications with noise，即基于密度的聚类算法。
[0027]
3、ppf：point pair feature,点对特征匹配算法，是一种通过计算点和点之间的特征匹配度进行点云配准的算法
[0028]
4、icp算法：iterative closest point，即迭代最近点算法，是一种基于最小二乘法的最优配准算法。
[0029]
5、rgb
‑
d：深度相机同时采集深度图(depth map)和rgb图，合称rgb
‑
d。
[0030]
参照图1，本申请的第一方面，提供了一种6d位姿标注方法，包括：
[0031]
s101、获取目标物体的图像信息；其中，图像信息包括目标掩码和目标物体的深度
图片；
[0032]
s102、根据目标掩码，将目标物体深度图片转化为第一点云集合；
[0033]
s103、粗匹配步骤：采用ppf算法对第一点云集合与预设的模型点云集合进行粗匹配，得到目标物体的第一位姿，并根据第一位姿将第一点云集合转换为第二点云集合；
[0034]
s104、第一次精匹配：采用点对面的icp算法将第二点云集合与模型点云集合进行第一次精匹配，得到目标物体的第二位姿，并根据第二位姿将第二点云集合转换为第三点云集合；
[0035]
s105、第二次精匹配：采用点对点的icp算法将第三点云集合与模型点云集合进行第二次精匹配，得到目标物体的第三位姿；
[0036]
s106、根据第一位姿、第二位姿和第三位姿，计算出目标物体的目标位姿。
[0037]
参照图2，目标物体位姿的具体标注流程为：
[0038]
s201、通过图像采集模块采集目标场景中的rgb
‑
d数据，其中，rgb
‑
d数据包括rgb图片(或者灰度图片)和深度图片，图像采集模块包括3d相机。
[0039]
s202、通过实例分割模块从采集的rgb图片或者灰度图片中分割目标物体，得到目标物体掩码，然后利用该掩码从深度图片中分割出该目标物体。
[0040]
s203、通过信息转化模块将图像分割模块的得到的目标物体二维的深度图片转化为三维的点云，以供后续处理。
[0041]
s204、由于光照等因素，提取的目标物体的点云含有大量的噪声，点云去噪模块去除提取出来的点云噪声。
[0042]
s205、通过点云匹配模块将去噪后得到的第一点云集合进行匹配，匹配过程包括：基于ppf(point pair feature)算法的点云粗匹配；基于点对面的icp算法的第一次精匹配；基于点对点的icp算法的第二次精匹配。
[0043]
s206、根据一次粗匹配、两次精匹配可以计算得到目标物体的目标位姿。
[0044]
具体的，点云去噪模块是基于dbscan的点云去噪算法进行去噪的。
[0045]
进一步的，dbsacn的点云去噪方法为：通过预先设置一个扫描半径即预设距离，则该预设距离为预设半径，若一个点的预设半径范围内的点数超过预设点数，则将该点称为核心点。核心点的预设半径范围内的所有点均由该核心点直接可达。另外，若第二核心点在第一核心点的预设半径范围内，则称为第二核心点由第一核心点可达；若第三核心点和第三非核心点均在第二核心点的预设半径范围内，则称为第三核心点和第三非核心点均由第二核心点可达；第三核心点和第三非核心点也可由第一核心点可达。第一核心点与所有由第一核心点可达的点可以形成一个聚类点集合。据此，在第一点云集合中，可以得到多个聚类点集合。在多个聚类点集合中选取点数最多的聚类点集合，其余点数较少的聚类点集合去掉，则将点数较多的聚类点集合作为去噪后的第一点云集合，以便应用于后续的粗匹配步骤。
[0046]
在本申请的一些实施例中，采用ppf算法对第一点云集合与预设的模型点云集合进行粗匹配的详细步骤为：在离线阶段，在模型点云集合中，根据每两点之间的点对特征，建立全局模型；在线阶段，将第一点云集合分为第一部分点集合和第二部分点集合，并计算出第一部分点集合与第二部分点集合的多个目标点对特征；将一个点对特征与全局模型进行匹配即可得到一个位姿，所以将全部点对特征与全局模型进行匹配，即可得到一个包含
多个位姿的位姿集合，获取位姿集合中个数最多的位姿，并将其作为第一位姿，再将第一点云集合经过第一位姿的转换即可得到第二点云集合。
[0047]
可以理解的是，也可以在线阶段建立全局模型，但是在线阶段建立模型会导致降低匹配效率。
[0048]
具体的，利用solid works建立目标物体的目标模型，得到目标物体的模型点云集合。
[0049]
在本申请的一些实施例中，采用点对面的icp算法将第二点云集合与模型点云进行第一次精匹配的详细步骤为：模型点云集合中的每个点均对应一个包括该点的切面，而目标点云集合中的一个点与最近的切面对应的模型点云集合中的点形成一个点对，获取目标点云集合中的全部点与模型点云集合找那个的点形成的多个点对，每个点对均形成一个点到切面的距离，全部点对则形成一个第一距离和，而第一距离和为所有点到切面的距离之和。将目标点云集合经过第一公式的迭代优化，取得第一距离和最小值的位姿将作为第二位姿，第二点云集合经过第二位姿的转换得到第三点云集合。
[0050]
具体的，点对面(point to plane)的icp算法为迭代优化算法，优化目标函数即为第一公式。给定两个点云集合p＝{p1,p2,
…
,pn}，q＝{q1,q2,
…
,qn}，r为它们之间的旋转矩阵，t为平移矩阵，假设pi和qi是一一对应的，首先计算qi点处的法向量ni，再计算两点间距离在法向量上的投影，即点pi到qi处切面的距离，r和t为使得点到面距离和取得最小值的旋转和平移矩阵，旋转和平移矩阵即为所求的第二位姿。其中，第一公式为：
[0051][0052]
在本申请的一些实施例中，采用点对点的icp算法将第三点云集合与模型点云集合进行第二次精匹配的详细步骤为:在模型点云集合中选取距离目标点云集合中一个点最近的点，这两个点将形成一个点对，而一个点对存在一个点对距离，目标点云集合中的全部点与模型点云集合中的点形成的多个点对之间的点对距离之和为第二距离和，将目标点云集合经过第二公式的迭代优化，将第二距离和取得最小值对应的位姿作为第三位姿。
[0053]
具体为，给定两个点云集合p＝{p1,p2,
…
,pn}，q＝{q1,q2,
…
,qn}，r为它们之间的旋转矩阵，t为平移矩阵，假设pi和qi是一一对应的，点对点icp即为计算使得点对距离和取得最小值的旋转和平移矩阵。其中，第二公式为：
[0054][0055]
在本申请的一些实施例中，目标场景中一般包含多个目标物体，需要利用前述方法对目标场景中的所有物体进行位姿标注。
[0056]
本申请的第二方面，提供了一种6d位姿标注系统，包括：至少一个存储器、至少一个处理器及至少一个程序指令，程序指令存储在存储器上并可在处理器上运行，处理器用于本申请第一方面的6d位姿标注方法。
[0057]
本申请的第三方面提供了一种存储介质，存储介质上存储有程序指令，程序指令用于执行本申请第一方面的6d位姿标注方法。
[0058]
上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。
[0059]
下面参考图1和图2，以一个具体的实施例详细描述根据本申请实施例的6d位姿标注方法。值得理解的是，下述描述仅是示例性说明，而不是对本申请的具体限制。
[0060]
参照图1至图2，本申请实施例提供了一种6d位姿标注方法，包括：通过3d相机采集目标场景中的rgb
‑
d数据，其中，rgb
‑
d数据包括rgb图片(或者灰度图片)和深度图片；通过实例分割模块从采集的rgb图片或者深度图片中分割目标场景，得到目标物体掩码，然后利用该掩码从深度图片中分割出该目标物体；信息转化模块将目标物体的二维深度图转化为三维点云；再利用基于dbscan的点云去噪算法对得到的三维点云进行去噪的，得到去噪后的第一点云集合；solid works建立目标物体的目标模型，得到目标物体的模型点云集合；采用ppf算法对第一点云集合与预设的模型点云集合进行粗匹配，得到第一位姿，并将第一点云集合经过第一位姿的转换得到第二点云集合；采用点对面的icp算法将第二点云集合与模型点云进行第一次精匹配，得到第二位姿，并将第二点云集合经过第二位姿的转换得到第三点云集合；采用点对点的icp算法将第三点云集合与模型点云集合进行第二次精匹配，得到第三位姿；根据第一位姿、第二位姿和第三位姿，可以计算出目标场景中一个目标物体的目标位姿；利用上述方法，可以得到目标场景中的全部目标物体的位姿。
[0061]
上述对本说明书特定实施例进行了描述。其他实施例在所附权利要求的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
[0062]
程序指令包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或者某些中间形式等。存储介质包括：能够携带计算机程序代码的任何实体或装置、记录介质、计算机存储器、只读存储器(rom)、随机存取存储器(ram)、电载波信号、电信信号以及软件分发介质等。需要说明的是，存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，存储介质不包括电载波信号和电信信号。
[0063]
需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必须的。
[0064]
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
[0065]
上面结合附图对本申请实施例作了详细说明，但是本申请不限于上述实施例，在技术领域普通技术人员所具备的知识范围内，还可以在不脱离本申请宗旨的前提下作出各种变化。