一 研究步骤

    第一步，用RGB-D相机获取物体的彩色图和深度图；第二步，把RGB彩色图输入mask_rcnn网络，输出零散物体中单个物体的掩码，并通过掩码截取该物体在RGB彩色图和深度图中的相应模块以及像素位置信息，通过该像素位置和深度图中的深度信息生成具有（x，y，z）三维信息的目标点云；第三步，把该物体的目标点云和模板点云信息作为位姿估计算法的输入，分别对其进行ISS关键点检测，然后用FPFH进行特征描述和对应点匹配，之后用SAC_IA粗配准算法算出点对的初始位姿，最后用初始位姿和快速鲁棒的ICP算法求解两个点云之间的转换矩阵。

二 数据准备

    在实例分割阶段，本文选择流传度较广，稳定性较高的MASK-RCNN，抓取对象选择常见的牙刷，通过搭建数据获取平台，采集对齐后的RGB-D数据，然后将RGB彩色图用labelme进行标注。下图为数据获取平台

本文采用的MASK-RCNN借鉴了Bubbliiiing的代码，backbone采用的restnet101+fpn，具体代码见最终成果。其训练效果如下：

三 最优抓取对象选择

def choose(class_ids,masks,depths,thred):
    choose_id_1=[]
    choose_id_2=[]
    dep=[]
    dep_1=[]
    dep_2=[]
    pixses=[]
    for depth in depths:
        depth_i=np.sum(depth,axis=0)
        depth_i=np.sum(depth_i)
        dep.append(depth_i)
    for i in range(len(class_ids)):
        class_id=class_ids[i]
        mask=masks[:,:,i]
        pixs=cv2.countNonZero(mask)
        if class_id==1:
            dep_1.append(dep[i])
            if pixs>=thred[0]:
                choose_id_1.append(i)
            else:
                pix=thred[0]-pixs
                pixses.append(pix)
        if class_id==2:
            dep_2.append(dep[i])
            if pixs>=thred[1]:
                choose_id_2.append(i)
            else:
                pix=thred[1]-pixs
                pixses.append(pix)
    if len(choose_id_1)==0 and len(choose_id_2)==0:
        num=min(pixses)
        id=pixses.index(num)
        return id
    if len(choose_id_1)!=0 and len(choose_id_2)==0:
        num=max(dep_1)
        id=dep.index(num)
        return id
    if len(choose_id_1) == 0 and len(choose_id_2) != 0:
        num = max(dep_2)
        id = dep.index(num)
        return id
    if len(choose_id_1) != 0 and len(choose_id_2) != 0:
        num_1=max(dep_1)
        num_2=max(dep_2)
        if num_1>=num_2:
            id=dep.index(num_1)
            return id
        else:
            id=dep.index(num_2)
            return id

四 点云重建

通过掩膜获取相关对象的深度图，然后重建目标点云。

 asst=False
    thred_1=32000
    thred_2=21000
    save=True
    ply_root_path="C:/Users/1111/Desktop/dataset/riyongpin"
    #相机内参
    camera_intrinsics = [979.03894, 979.03491, 688.8656, 557.68732]#fx="979.03894" fy="979.03491" cx="688.8656" cy="557.68732"
    depth_scale=1000
    imag_path="C:/Users/1111/Desktop/dataset/riyongpin/rgb2.jpg"
    depth_path="C:/Users/1111/Desktop/dataset/riyongpin/depth2.png"
    image=cv2.imread(imag_path)
    depth=cv2.imread(depth_path,-1)
    width =image.shape[1]
    height = image.shape[0]
    #加载模型
    mask_model= MASK_RCNN(confidence = 0.7, nms_iou = 0.5)
    class_ids,masks=mask_model.mask(image)
    depths=[]
    for j in range(len(class_ids)):
        mask = np.array(masks[:, :, j])  # 单个物体的mask
        p=cv2.countNonZero(mask)
        class_id = class_ids[j]
        depth_i = (mask / class_id) * depth  # 单个物体深度图
        depths.append(depth_i)
    pointclouds=[]#图片中物体的点云
    # 选择要进行位姿估计的物体（一次只估计一个物体）
    id=choose(class_ids,masks,depths,[thred_1,thred_2])

    image_i=np.ones((image.shape[0],image.shape[1],image.shape[2]))
    mask=np.array(masks[:,:,id])#单个物体的mask
    class_id=class_ids[id]
    depth_i=depths[id]#单个物体深度图
    for j in range(3):
        image_i[:,:,j]=(mask/class_id)*image[:,:,j]#单个物体的彩色图
        #制作xyz
    Z = (depth_i / depth_scale).T
    fx, fy, cx, cy = camera_intrinsics
    X = np.zeros((width, height))
    Y = np.zeros((width, height))
    for k in range(width):
        X[k, :] = np.full(X.shape[1], k)
    X = ((X - cx ) * Z) / fx
    for m in range(height):
        Y[:, m] = np.full(Y.shape[0], m)
    Y = ((Y - cy ) * Z) / fy
        #
        #彩色点云
    data_ply = np.zeros((6, width * height))
    data_ply[0] = X.T.reshape(-1)
    data_ply[1] = -Y.T.reshape(-1)
    data_ply[2] = -Z.T.reshape(-1)
    image_i = image_i[:, :, ::-1]#把bgr转成rgb
    img= np.array(image_i, dtype=np.uint8)
    data_ply[3] = img[:, :, 0:1].reshape(-1)
    data_ply[4] = img[:, :, 1:2].reshape(-1)
    data_ply[5] = img[:, :, 2:3].reshape(-1)
        #去除位置为0的点
    data_ply=data_ply.T
    list_q=[]
    for q in range(width*height):
        if data_ply[q][0]+data_ply[q][1]+data_ply[q][2]==0:
            list_q.append(q)
    data_ply=np.delete(data_ply, list_q, axis=0)
        # 直通滤波,除去底部的噪音
    if class_id==2:
        means = np.mean(data_ply[:,2])-0.002
        list_p=[]
        for p in range(data_ply.shape[0]):
            if data_ply[p][2]<means:
                list_p.append(p)
        data_ply=np.delete(data_ply,list_p,axis=0)
    data_ply=data_ply.T
    if save==True:
        ply_save_path = ply_root_path +"/"+ "model_" + str(class_id) + ".ply"
        write_ply(data_ply, ply_save_path)