diff --git a/Vision/camera_coordinate_dete.py b/Vision/camera_coordinate_dete.py index deef23d..8628bb1 100644 --- a/Vision/camera_coordinate_dete.py +++ b/Vision/camera_coordinate_dete.py @@ -330,6 +330,276 @@ class Detection: print("RVC X Camera is not opened!") return 0, None, None, None, None + + def get_position_and_depth(self, Point_isVision=False, Box_isPoint=True, First_Depth =True, Target_pixel_threshold = 200, Iter_Max_Pixel = 30, save_img_point=0, Height_reduce = 30, width_reduce = 30): + """ + 眼在手上，用于料袋顶层抓取，检测料袋相关信息 + :param Point_isVision: 点云可视化 + :param Box_isPoint: True 返回点云值； False 返回box相机坐标 + :param First_Depth: True 返回料袋中心点深度最小的点云值； False 返回面积最大的料袋中心点云值 + :param Target_pixel_threshold: [int] 设定像素阈值，判断是否可以抓取 + :param Iter_Max_Pixel: [int] 点云为NAN时，向该点周围寻找替代值，寻找最大区域（Iter_Max_Pixel×Iter_Max_Pixel） + :param save_img_point: 0不保存 ; 1保存原图 ;2保存处理后的图 ; 3保存点云和原图；4 保存点云和处理后的图; 5 异常数据保存（点云NAN） + :param Height_reduce: 检测框的高内缩像素 + :param width_reduce: 检测框的宽内缩像素 + :return ret: bool 相机是否正常工作 + :return img: ndarry 返回img + :return xyz: list 目标中心点云值形如[x,y,z] + :return nx_ny_nz: list 拟合平面法向量，形如[a,b,c] + :return box_list: list 内缩检测框四顶点，形如[[x1,y1],[],[],[]] + + """ + ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及 + if self.camera_rvc.caminit_isok == True: + if ret == 1: + if save_img_point != 0: + if get_disk_space(path=os.getcwd()) < 15: # 内存小于15G,停止保存数据 + save_img_point = 0 + print('系统内存不足，无法保存数据') + else: + save_path = ''.join([os.getcwd(), '/Vision/model/data/', + time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))]) + save_img_name = ''.join([save_path, '.png']) + save_point_name = ''.join([save_path, '.xyz']) + if save_img_point == 5: + Abnormal_data_img = img.copy() + if save_img_point==1 or save_img_point==3: + cv2.imwrite(save_img_name, img) + if save_img_point==3 or save_img_point==4 or save_img_point==5: + row_list = list(range(1, img.shape[0], 2)) + column_list = list(range(1, img.shape[1], 2)) + pm_save = pm.copy() + pm_save1 = np.delete(pm_save, row_list, axis=0) + point_new = np.delete(pm_save1, column_list, axis=1) + point_new = point_new.reshape(-1, 3) + if save_img_point==5: + Abnormal_data_point = point_new.copy() + else: + np.savetxt(save_point_name, point_new) + if self.use_openvino_model == False: + flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0) + else: + flag, det_cpu, scores, masks, category_names = self.model.segment_objects(img) + if flag == 1: + xyz = [] + nx_ny_nz = [] + RegionalArea = [] + Depth_Z = [] + uv = [] + seg_point = [] + box_list = [] + target_box_area = 0 + if Point_isVision==True: + pm2 = pm.copy() + pm2 = pm2.reshape(-1, 3) + pm2 = pm2[~np.isnan(pm2).all(axis=-1), :] + pm2[:, 2] = pm2[:, 2] + 0.25 + pcd2 = o3d.geometry.PointCloud() + pcd2.points = o3d.utility.Vector3dVector(pm2) + # o3d.visualization.draw_geometries([pcd2]) + + for i, item in enumerate(det_cpu): + target_box_area = 0 + # 画box + box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32) + if self.use_openvino_model == False: + label = category_names[int(item[5])] + else: + label = class_names[int(item[4])] + rand_color = (0, 255, 255) + score = item[4] + org = (int((box_x1 + box_x2) / 2), int((box_y1 + box_y2) / 2)) + x_center = int((box_x1 + box_x2) / 2) + y_center = int((box_y1 + box_y2) / 2) + text = '{}|{:.2f}'.format(label, score) + cv2.putText(img, text, org=org, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.8, + color=rand_color, + thickness=2) + # 画mask + # mask = masks[i].cpu().numpy().astype(int) + if self.use_openvino_model == False: + mask = masks[i].cpu().data.numpy().astype(int) + else: + mask = masks[i].astype(int) + mask = mask[box_y1:box_y2, box_x1:box_x2] + + # mask = masks[i].numpy().astype(int) + h, w = box_y2 - box_y1, box_x2 - box_x1 + mask_colored = np.zeros((h, w, 3), dtype=np.uint8) + mask_colored[np.where(mask)] = rand_color + ################################## + imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY) + # cv2.imshow('mask',imgray) + # cv2.waitKey(1) + # 2、二进制图像 + ret, binary = cv2.threshold(imgray, 10, 255, 0) + # 阈值 二进制图像 + # cv2.imshow('bin',binary) + # cv2.waitKey(1) + contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) + # all_point_list = contours_in(contours) + # print(len(all_point_list)) + max_contour = None + max_perimeter = 0 + for contour in contours: # 排除小分割区域或干扰区域 + perimeter = cv2.arcLength(contour, True) + if perimeter > max_perimeter: + max_perimeter = perimeter + max_contour = contour + + ''' + 拟合最小外接矩形，计算矩形中心 + ''' + + rect = cv2.minAreaRect(max_contour) + if rect[1][0]-width_reduce < 30 or rect[1][1]-Height_reduce < 30: + rect_reduce = ( + (rect[0][0], rect[0][1]), (rect[1][0] - width_reduce, rect[1][1] - Height_reduce), rect[2]) + else: + rect_reduce = ( + (rect[0][0], rect[0][1]), (rect[1][0], rect[1][1]), rect[2]) + target_box_area = rect[1][0] * rect[1][1] + # cv2.boxPoints可以将轮廓点转换为四个角点坐标 + box_outside = cv2.boxPoints(rect) + # 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针 + startidx = box_outside.sum(axis=1).argmin() + box_outside = np.roll(box_outside, 4 - startidx, 0) + box_outside = np.intp(box_outside) + box_outside = box_outside.reshape((-1, 1, 2)).astype(np.int32) + + # cv2.boxPoints可以将轮廓点转换为四个角点坐标 + box_reduce = cv2.boxPoints(rect_reduce) + startidx = box_reduce.sum(axis=1).argmin() + box_reduce = np.roll(box_reduce, 4 - startidx, 0) + box_reduce = np.intp(box_reduce) + box_reduce = box_reduce.reshape((-1, 1, 2)).astype(np.int32) + + ''' + 提取区域范围内的（x, y） + ''' + mask_inside = np.zeros(binary.shape, np.uint8) + cv2.fillPoly(mask_inside, [box_reduce], (255)) + pixel_point2 = cv2.findNonZero(mask_inside) + # result = np.zeros_like(color_image) + select_point = [] + for i in range(pixel_point2.shape[0]): + select_point.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]+box_x1]) + select_point = np.array(select_point) + pm_seg = select_point.reshape(-1, 3) + pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan + if pm_seg.size < 100: + print("分割点云数量较少，无法拟合平面") + continue + + # cv2.imshow('result', point_result) + ''' + 拟合平面，计算法向量 + ''' + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(pm_seg) + plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold, + ransac_n=5, + num_iterations=5000) + [a, b, c, d] = plane_model + # print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0") + # inlier_cloud = pcd.select_by_index(inliers) # 点云可视化 + # inlier_cloud.paint_uniform_color([1.0, 0, 0]) + # outlier_cloud = pcd.select_by_index(inliers, invert=True) + # outlier_cloud.paint_uniform_color([0, 1, 0]) + # o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud]) + + box_outside = box_outside + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]],[[box_x1, box_y1]]] + box = box_reduce + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]] + + box[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0]) + box[1][0][1], box[1][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[1][0][1], box[1][0][0]) + box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0]) + box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0]) + if Box_isPoint == True: + box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0], iter_max=Iter_Max_Pixel) + box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0], iter_max=Iter_Max_Pixel) + box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0], iter_max=Iter_Max_Pixel) + box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0], iter_max=Iter_Max_Pixel) + else: + x1, y1, z1 = uv_to_XY(box[0][0][0], box[0][0][1]) + x2, y2, z2 = uv_to_XY(box[1][0][0], box[1][0][1]) + x3, y3, z3 = uv_to_XY(box[2][0][0], box[2][0][1]) + x4, y4, z4 = uv_to_XY(box[3][0][0], box[3][0][1]) + x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4) + y_rotation_center = int((box[0][0][1] + box[1][0][1] + box[2][0][1] + box[3][0][1]) / 4) + point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center, iter_max=Iter_Max_Pixel) + cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 + if np.isnan(point_x): # 点云值为无效值 + continue + else: + if Box_isPoint == True: + box_list.append( + [[box_point_x1, box_point_y1, box_point_z1], + [box_point_x2, box_point_y2, box_point_z2], + [box_point_x3, box_point_y3, box_point_z3], + [box_point_x4, box_point_y4, box_point_z4]]) + else: + box_list.append([[x1, y1, z1], + [x2, y2, z2], + [x3, y3, z3], + [x4, y4, z4], + ]) + if target_box_area > img.shape[0]*img.shape[1]*(2/3): # Target_pixel_threshold + if self.cameraType == 'RVC': + xyz.append([point_x*1000, point_y*1000, point_z*1000]) + Depth_Z.append(point_z*1000) + elif self.cameraType=='Pe': + xyz.append([point_x, point_y, point_z]) + Depth_Z.append(point_z) + nx_ny_nz.append([a, b, c]) + RegionalArea.append(cv2.contourArea(max_contour)) + uv.append([x_rotation_center, y_rotation_center]) + seg_point.append(pm_seg) + cv2.polylines(img, [box], True, (0, 255, 0), 2) + cv2.polylines(img, [box_outside], True, (226, 12, 89), 2) + + _idx = find_position(Depth_Z, RegionalArea, 100, First_Depth) + + if _idx == None: + if save_img_point == 5: + cv2.imwrite(save_img_name, Abnormal_data_img) + np.savetxt(save_point_name, Abnormal_data_point) + return 1, img, None, None, None + else: + cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 + + if Point_isVision==True: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(seg_point[_idx]) + plane_model, inliers = pcd.segment_plane(distance_threshold=self.seg_distance_threshold, + ransac_n=5, + num_iterations=5000) + inlier_cloud = pcd.select_by_index(inliers) # 点云可视化 + inlier_cloud.paint_uniform_color([1.0, 0, 0]) + outlier_cloud = pcd.select_by_index(inliers, invert=True) + outlier_cloud.paint_uniform_color([0, 0, 1]) + o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2]) + if save_img_point == 2 or save_img_point ==4: + save_img = cv2.resize(img, (720, 540)) + cv2.imwrite(save_img_name, save_img) + return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx] + else: + if save_img_point == 2 or save_img_point ==4: + save_img = cv2.resize(img,(720, 540)) + cv2.imwrite(save_img_name, save_img) + if save_img_point == 5: + cv2.imwrite(save_img_name, Abnormal_data_img) + np.savetxt(save_point_name, Abnormal_data_point) + return 1, img, None, None, None + + else: + print("RVC X Camera capture failed!") + return 0, None, None, None, None + + else: + print("RVC X Camera is not opened!") + return 0, None, None, None, None + def get_take_photo_position(self, Height_reduce = 30, width_reduce = 30): """ 检测当前拍照点能否检测到料袋