点云对应检测结果生成抓取物位姿

功能描述

该算子用于结合三维点云信息和二维图像检测结果，为检测到的物体生成抓取位姿。它首先将输入的点云列表与输入的二维检测结果列表进行匹配对齐，然后基于对齐的点云计算抓取点的深度（Z坐标），并结合二维检测结果的几何信息（如最小外接矩形）来确定抓取点的XY坐标和绕Z轴的旋转角度，最终输出基于机器人基坐标系的位姿。

使用场景

  • 机器人抓取 : 在视觉引导的机器人抓取任务中，需要为识别出的物体计算精确的6D抓取位姿，以便机器人能够准确地抓取。

  • 位姿估计 : 结合2D检测的类别、位置信息和3D点云的深度、形状信息，估计物体在空间中的完整位姿。

输入输出

输入项

相机坐标系点云： 输入的点云列表，通常是检测结果提取后的点云经过过滤或聚类后，与待抓取物体对应的点云簇。必须是相机坐标系下的点云，且不含 NaN 值。

图像： 与点云对应的原始图像（彩色或灰度），用于将点云投影回像素坐标。

检测结果： 来自二维检测算子的输出结果列表，每个结果包含物体的边界框/多边形、分数、类别、角度等信息。

相机内参： 3x3的相机内参矩阵。

相机畸变： 相机的畸变系数向量。

手眼标定矩阵： 4x4的从相机坐标系到机器人基坐标系的变换矩阵。

输出项

抓取物位姿信息：

计算得到的抓取位姿列表。每个元素是一个字典，包含：

  • pose：抓取物位姿。

  • score：对应的二维检测结果的置信度分数。

  • class_name：对应的二维检测结果的类别。

  • uuid：对应的二维检测结果的唯一标识符。

  • polygon：对应的二维检测结果的多边形轮廓。

  • object_info：包含物体尺寸、面积等信息的字典，这里面的尺寸和面积是实际尺寸和面积，单位是mm和mm^2。

  • points_number：对应的输入点云在原始列表中的索引。

参数说明

使用检测结果角度

参数说明

决定最终输出位姿的旋转角度（绕Z轴）是直接采用二维检测结果提供的角度，还是根据点云和检测结果的最小外接矩形重新计算。

调参说明

  • 关闭 (默认) : 不直接使用检测结果的角度。算子会计算检测结果最小外接矩形的短边中点连线，将其投影到相机坐标系并转换到机器人基坐标系，计算该连线与基坐标系X轴的夹角作为最终位姿的Rz旋转。

  • 开启 : 直接使用输入“检测结果”中每个实例提供的角度值作为最终位姿的Rz旋转，一般是针对旋转检测才会有这个值，其他检测结果角度都为0。

对齐策略

参数说明

选择将输入的点云簇与二维检测结果进行匹配对齐的方法。

调参说明

  • 中心点对齐（默认）： 计算点云投影到图像后的中心点与检测结果掩膜中心点的距离，选择距离最近的进行匹配。计算速度快，适用于物体间距较大、中心点区分明显的场景。

  • 掩膜交并比对齐： 将点云投影到图像生成掩膜，计算其与检测结果掩膜的交并比（IoU），选择 IoU 最大的进行匹配。考虑了形状重叠，对于物体紧密贴合或部分遮挡的情况可能更鲁棒，但计算量稍大。

抓取点z值计算方法

参数说明

选择如何根据对齐的点云簇计算最终抓取位姿的Z坐标值。

调参说明

  • 均值（默认）： 使用点云簇中所有点的Z坐标平均值。适用于点云质量较好、飞点较少的情况。

  • 中位数： 使用点云簇中所有点的Z坐标中位数。对飞点不敏感，结果更稳健，推荐在点云噪声较多时使用。

  • 掩膜中心圆形区域均值： 在检测结果的最小外接矩形中心，取一个由“圆半径比例”定义的圆形区域，计算该区域内点云的Z坐标均值。

圆半径比例

参数说明

当“抓取点z值计算方法”选择“掩膜中心圆形区域均值”时生效。定义用于计算Z值的圆形区域半径，该半径是相对于检测结果最小外接矩形短边一半长度的比例。

调参说明

此参数控制计算深度Z值时所参考的物体表面区域大小。

  • 较小的比例意味着只考虑物体中心非常小区域的点，可能更稳定但易受噪声影响；

  • 较大的比例考虑更大范围的点，可能更平滑但会包含更多表面起伏。

参数范围

[0, 1]，默认值：0.3