基于GraspNet熟悉点云抓取代码的处理逻辑

基于GraspNet熟悉点云抓取代码的处理逻辑

1.数据读取逻辑

1.1GraspNet数据集的预览

公开数据集下载完全如下，其中scenes为存放的数据集（RGB、D等），grasp_label为抓取的标签，collision_label为碰撞的标签。

scenes中一共有100个场景的文件夹：

每个场景中的文件如下，根据指定相机的品牌，决定使用哪个文件夹。

比如，我这里使用realsense相机，realsense文件夹里面存放的就是rgb、depth（深度）、label（mask）、meta（对应每一帧图片中物体的类别索引、物体的位置姿态、相机内参、深度图像中的像素值进行标准化或缩放的因子）等数据。

1.2xxx

1.3get_item方法读取逻辑

与我们常见的使用opencv读取RGB图不同，在点云处理中，常用PIL库以dtype=np.float32的形式读取RGB、D、mask，并对RGB进行/255.归一化操作。

这里RGB就是float32型，D就是int32型。

mask为uint8型。

随后。使用scipy库对.mat数据进行读取，分别获取对应每一帧图片中物体的类别索引、物体的位置姿态、相机内参、深度图像中的像素值进行标准化或缩放的因子。

对应每一帧图片中物体的类别索引如下，包含场景中每个物体的类别索引。

物体的位置姿态如下，每个维度的含义分别为：
3表示每个物体的姿态信息在三维空间中的三个分量；
4表示每个姿态的四元数表示法中的四个参数，用于描述物体的旋转；
9意味当前帧中对应着场景中的 9 个物体。

intrinsic 是一个 3x3 的矩阵。在计算机视觉中，相机内参通常以矩阵形式表示，这个矩阵通常称为相机的内部参数矩阵。

深度图像中的像素值通常是相机到场景中物体的距离，但这些距离通常以某种方式进行了标准化或缩放，factor_depth表示缩放因子。（这也是在实际场景中相机流的深度图要➗1000的原因）

1.4结合相机内参将depth转点云

其实就是将上述根据数据集中读取出来的相机参数传递 xmap = np.arange(camera.width)
ymap = np.arange(camera.height)
xmap, ymap = np.meshgri给CameraInfo类。再将这个类传递给create_point_cloud_from_depth_image函数。
记住，点云的数据格式其实就是：[N，3]，N表示点的个数，3表示每个点有三个坐标，通常是 (x, y, z)，分别表示点在三维空间中的位置。
以这里的相机分辨率为例，那就是求3个[720， 1280]，再拼接就行。
Z方向最好求，就是将深度图➗1000（缩放因子就行）。
X、Y见下方代码注释。
最后点云数据就得到了，[720， 1280， 3]

class CameraInfo():
    """ Camera intrisics for point cloud creation. """
    def __init__(self, width, height, fx, fy, cx, cy, scale):
        self.width = width
        self.height = height
        self.fx = fx
        self.fy = fy
        self.cx = cx
        self.cy = cy
        self.scale = scale
        
def create_point_cloud_from_depth_image(depth, camera, organized=True):
    assert(depth.shape[0] == camera.height and depth.shape[1] == camera.width)
    #创建一个长度为相机宽度的数组，表示图像中每列的 x 坐标。
    xmap = np.arange(camera.width)
    #创建一个长度为相机高度的数组，表示图像中每列的 y 坐标。
    ymap = np.arange(camera.height)
    # 通过 meshgrid 函数创建网格，得到 (x, y) 坐标的矩阵，其中 xmap 是列索引的重复，ymap 是行索引的重复
    #xmap, ymap的size都是[720， 1280]
    xmap, ymap = np.meshgrid(xmap, ymap)
    #将深度值➗缩放因子
    points_z = depth / camera.scale
    #根据像素 x 坐标、相机的主点 cx 和焦距 fx 计算每个点云点的 x 坐标。
    points_x = (xmap - camera.cx) * points_z / camera.fx
    #根据像素 y 坐标、相机的主点 cy 和焦距 fy 计算每个点云点的 y 坐标。
    points_y = (ymap - camera.cy) * points_z / camera.fy
    cloud = np.stack([points_x, points_y, points_z], axis=-1)
    if not organized:
        cloud = cloud.reshape([-1, 3])
    return cloud

camera = CameraInfo(1280.0, 720.0, intrinsic[0][0], intrinsic[1][1], intrinsic[0][2], intrinsic[1][2], factor_depth)

# generate cloud
cloud = create_point_cloud_from_depth_image(depth, camera, organized=True)

1.5筛选有效的点云数据

获得根据深度图以及相机参数得到的点云数据后，先根据分割的mask获取工作区域的掩码，并结合有效深度值区域综合获得有效工作区域，最后获得效的点云数据和对应的标签以及颜色信息。

def get_workspace_mask(cloud, seg, trans=None, organized=True, outlier=0):
    if organized:
    	#将点云和分割标签转换为一维数组
        h, w, _ = cloud.shape
        cloud = cloud.reshape([h*w, 3])
        seg = seg.reshape(h*w)
    if trans is not None:
    	#将点云数据从相机坐标系转换到工作台
        cloud = transform_point_cloud(cloud, trans)
    #取分割标签为正值（非背景）的点云作为前景点云
    foreground = cloud[seg>0]
    #计算前景点云的最小和最大坐标值，得到一个表示工作空间范围的边界框。
    xmin, ymin, zmin = foreground.min(axis=0)
    xmax, ymax, zmax = foreground.max(axis=0)
    #根据边界框和离群点阈值，生成用于筛选工作空间内点云的掩码
    mask_x = ((cloud[:,0] > xmin-outlier) & (cloud[:,0] < xmax+outlier))
    mask_y = ((cloud[:,1] > ymin-outlier) & (cloud[:,1] < ymax+outlier))
    mask_z = ((cloud[:,2] > zmin-outlier) & (cloud[:,2] < zmax+outlier))
    workspace_mask = (mask_x & mask_y & mask_z)
    if organized:
        workspace_mask = workspace_mask.reshape([h, w])

    return workspace_mask

# get valid points
#选出有效的点云数据和对应的标签以及颜色信息
depth_mask = (depth > 0)
seg_mask = (seg > 0)
if self.remove_outlier:
   camera_poses = np.load(os.path.join(self.root, 'scenes', scene, self.camera, 'camera_poses.npy'))
   align_mat = np.load(os.path.join(self.root, 'scenes', scene, self.camera, 'cam0_wrt_table.npy'))
   trans = np.dot(align_mat, camera_poses[self.frameid[index]])
   workspace_mask = get_workspace_mask(cloud, seg, trans=trans, organized=True, outlier=0.02)
   mask = (depth_mask & workspace_mask)
else:
    mask = depth_mask
cloud_masked = cloud[mask]
color_masked = color[mask]
seg_masked = seg[mask]

1.5随机采样

点云数据的点个数一般会比较多，这里就通过随即采样，选取20000个点。

# sample points
if len(cloud_masked) >= self.num_points:
    idxs = np.random.choice(len(cloud_masked), self.num_points, replace=False)
else:
    idxs1 = np.arange(len(cloud_masked))
    idxs2 = np.random.choice(len(cloud_masked), self.num_points-len(cloud_masked), replace=True)
    idxs = np.concatenate([idxs1, idxs2], axis=0)
cloud_sampled = cloud_masked[idxs]
color_sampled = color_masked[idxs]
seg_sampled = seg_masked[idxs]
objectness_label = seg_sampled.copy()
objectness_label[objectness_label>1] = 1