#!/usr/bin/env python # -*- coding: UTF-8 -*- ''' @Project ：AutoControlSystem-master @File ：camera_coordinate_dete.py @IDE ：PyCharm @Author ：hjw @Date ：2024/8/27 14:24 ''' import numpy as np import cv2 import open3d as o3d import time import os from Vision.tool.CameraRVC import camera_rvc from Vision.tool.CameraPe_color2depth import camera_pe as camera_pe_color2depth from Vision.tool.CameraPe_depth2color import camera_pe as camera_pe_depth2color from Vision.yolo.yolov8_pt_seg import yolov8_segment from Vision.yolo.yolov8_openvino import yolov8_segment_openvino from Vision.yolo.yolov8_pt_pose import yolov8_pose from Vision.tool.utils import find_position from Vision.tool.utils import class_names from Vision.tool.utils import get_disk_space from Vision.tool.utils import remove_nan_mean_value from Vision.tool.utils import out_bounds_dete, find_closest_point_index from Vision.tool.utils import uv_to_XY, shrink_quadrilateral class Detection: def __init__(self, use_openvino_model=False, use_pose_model=True, use_seg_pt_model=True, cameraType = 'Pe', alignmentType = 'color2depth'): # cameraType = 'RVC' or cameraType = 'Pe' """ 初始化相机及模型 :param use_openvino_model: 加载分割模型 :param use_pose_model: 加载关键点pt模型 :param use_seg_pt_model: 加载分割pt模型 :param use_openvino_model: 选择模型，默认使用openvino :param cameraType: 选择相机 如本相机 'RVC', 图漾相机 'Pe' :param alignmentType: 相机对齐方式 color2depth：彩色图对齐深度图 ；depth2color：深度图对齐彩色图 """ if use_seg_pt_model: # 优先使用pt模型 use_openvino_model = False elif use_openvino_model: use_seg_pt_model = False self.use_openvino_model = use_openvino_model self.cameraType = cameraType self.use_pose_model = use_pose_model self.use_seg_pt_model = use_seg_pt_model self.alignmentType = alignmentType if self.cameraType == 'RVC': self.camera_rvc = camera_rvc() self.seg_distance_threshold = 10 # 1厘米 elif self.cameraType == 'Pe': if self.alignmentType == 'color2depth': self.camera_rvc = camera_pe_color2depth() else: self.camera_rvc = camera_pe_depth2color() self.seg_distance_threshold = 15 # 2厘米 else: print('相机参数错误') return # 加载openvino-seg if self.use_openvino_model: model_path = ''.join([os.getcwd(), './Vision/model/openvino/one_bag.xml']) device = 'CPU' self.model_seg = yolov8_segment_openvino(model_path, device, conf_thres=0.6, iou_thres=0.6) # 加载pt-seg if self.use_seg_pt_model: model_path = ''.join([os.getcwd(), './Vision/model/pt/one_bag.pt']) device = 'cpu' self.model_seg = yolov8_segment() self.model_seg.load_model(model_path, device) # 加载pt-pose if self.use_pose_model: model_path = ''.join([os.getcwd(), './Vision/model/pt/one_bag_pose.pt']) device = 'cpu' self.model_pose = yolov8_pose(model_path, device) def get_position(self, Use_Pose_Model_Pro=False, Point_isVision=False, Box_isPoint=True, First_Depth =True, Iter_Max_Pixel = 30, save_img_point=0, Height_reduce = 80, width_reduce = 60, Xmin =160, Xmax = 1050, Ymin =290 ,Ymax = 780): """ 检测料袋相关信息 :param Use_Pose_Model_Pro: True: 选用关键点推理 False : 选用分割模型推理 :param Point_isVision: 点云可视化 :param Box_isPoint: True 返回点云值； False 返回box相机坐标 :param First_Depth: True 返回料袋中心点深度最小的点云值； False 返回面积最大的料袋中心点云值 :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: 检测框的宽内缩像素 :param Xmin: 限定料袋中心点的范围 :param Xmax: 限定料袋中心点的范围 :param Ymin: 限定料袋中心点的范围 :param Ymax: 限定料袋中心点的范围 :return ret: bool 相机是否正常工作 :return img: ndarray 返回img :return xyz: list 目标中心点云值形如[x,y,z] :return nx_ny_nz: list 拟合平面法向量，形如[a,b,c] :return box_list: list 内缩检测框四顶点，形如[[x1,y1],[],[],[]] """ # ret, img, pm, _depth_align = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及 ret = 1 pm1 = np.loadtxt('D:\pychram_rob\AutoControlSystem-git\Vision\model\data\\2024_11_29_10_05_58.xyz', dtype=np.float32) img = cv2.imread('D:\pychram_rob\AutoControlSystem-git\Vision\model\data\\2024_11_29_10_05_58.png') pm = pm1.reshape((img.shape[0], img.shape[1], 3)) 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_pose_model and Use_Pose_Model_Pro: real_model_pro_isPose = True else: real_model_pro_isPose = False if real_model_pro_isPose: flag, det_cpu, category_names, score_list = self.model_pose.model_inference(img)#用关键点检测模型 else: if self.use_openvino_model == False: flag, det_cpu, dst_img, masks, category_names = self.model_seg.model_inference(img, 0) #用分割模型 else: flag, det_cpu, scores, masks, category_names = self.model_seg.segment_objects(img) if flag == 1: xyz = [] nx_ny_nz = [] RegionalArea = [] Depth_Z = [] uv = [] seg_point = [] box_list = [] 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):#提供检测到的框信息 # 画box if real_model_pro_isPose: label = category_names[i] score = score_list[i] box_x1 = item[0][0] box_y1 = item[0][1] box_x2 = item[3][0] box_y2 = item[3][1] pass else: box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32)#找最近的框的1，3角点坐标 if self.use_openvino_model == False: label = category_names[int(item[5])] score = item[4] else: label = class_names[int(item[4])] score = item[4] rand_color = (0, 255, 255) 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 real_model_pro_isPose: # 创建一个与输入数组相同形状的掩码，初始值全为 0 mask = np.zeros(pm.shape[:2], dtype=np.uint8) # 将四点坐标转换为 numpy 数组 if item[0][0] < item[1][0]: arr = [[item[0][0], item[0][1]], [item[1][0], item[1][1]], [item[3][0], item[3][1]], [item[2][0], item[2][1]]] # new_points.reshape((-1, 1, 2)) else: arr = [[item[3][0], item[3][1]], [item[2][0], item[2][1]], [item[0][0], item[0][1]], [item[1][0], item[1][1]]] box = arr.copy() box_outside = arr.copy() box = shrink_quadrilateral(box, Height_reduce) pts = np.array(box, np.int32) # 将四点构成的四边形区域在掩码上标记为 255 cv2.fillPoly(mask, [pts], 255) # 根据掩码提取对应区域的数据 pm_seg = pm[mask == 255] # box =[[[item[0][0]+width_reduce, item[0][1]+Height_reduce]], # [[item[1][0]-width_reduce, item[1][1]+Height_reduce]], # [[item[3][0]-width_reduce, item[3][1]-Height_reduce]], # [[item[2][0]+width_reduce, item[2][1]-Height_reduce]]] box = box.reshape((-1, 1, 2)) # box = np.array(box) # 内缩 # box_outside = [[[item[0][0], item[0][1]]], # [[item[1][0], item[1][1]]], # [[item[3][0], item[3][1]]], # [[item[2][0], item[2][1]]]]# 外框 box_outside = np.array(box_outside) box_outside = box_outside.reshape((-1, 1, 2)) # box_outside = np.array(box_outside) else: 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)#检测物体轮廓，在灰度化和二值化之后，contours是轮廓信息 # 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 and 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]) # 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) 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]]]#我也当他是锚点 ''' 提取区域范围内的（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])#我为什么要加这个box_y1和box_x1呢？是因为mask取出来不是原图的坐标了，box_y1和box_x1相当于mask在原图的锚点，用来帮助剪切后的形状回到原图的位置 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)#平面分割，平面拟合，plane_model拟合平面的系数 [a, b, c, d] = plane_model#ax+by+cz+d=0，a,b,c就是法向量 # 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]) print(box) 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有没有超过点云范围，pm直接是整个图片的点云，box只是分割模型识别的框 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的坐标能被传回来，如果这个点上的没有，就用旁边的均值 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)#求中心点位置 if x_rotation_centerXmax or y_rotation_centerYmax: continue cv2.circle(img, (x_rotation_center, y_rotation_center), 2, (255, 255, 255), 3) # 标出中心点，只是标出来 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 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) if real_model_pro_isPose: RegionalArea.append(0) else: RegionalArea.append(cv2.contourArea(max_contour))#计算面积 nx_ny_nz.append([a, b, c])#法向量 uv.append([x_rotation_center, y_rotation_center])#中心点x,y 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), 10) # 标出中心点 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 read_data(self, xyz_path, img_path): pm1 = np.loadtxt(xyz_path, dtype=np.float32) img = cv2.imread(img_path) pm = pm1.reshape((img.shape[0], img.shape[1], 3)) return img, pm def save_data(self, img, pm, save_img_point, save_path): if save_img_point == 0: return if not os.path.exists(os.path.dirname(save_path)): os.makedirs(os.path.dirname(save_path)) save_img_name = save_path + '.png' save_point_name = save_path + '.xyz' if save_img_point in (1, 3): cv2.imwrite(save_img_name, img) if save_img_point in (3, 4): 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) np.savetxt(save_point_name, point_new) def model_inference(self, img, Use_Pose_Model_Pro): real_model_pro_isPose = self.use_pose_model and Use_Pose_Model_Pro if real_model_pro_isPose: flag, det_cpu, category_names, score_list = self.model_pose.model_inference(img) return flag, det_cpu, category_names, score_list, real_model_pro_isPose else: if self.use_openvino_model: flag, det_cpu, scores, masks, category_names = self.model_seg.segment_objects(img) else: flag, det_cpu, dst_img, masks, category_names = self.model_seg.model_inference(img, 0) return flag, det_cpu, category_names, masks, real_model_pro_isPose def get_box_3d_points(self, pm, box, Box_isPoint=True, Iter_Max_Pixel=30): """ 输入: box 为 (4, 2) 像素坐标 [[x1,y1], ..., [x4,y4]] 输出: 4个点的3D坐标 [x, y, z] """ box = np.array(box).reshape(-1, 2) # 强制为 (4, 2) pts_3d = [] for pt in box: # 确保 pt 是 [x, y] 结构 x_img, y_img = int(pt[0]), int(pt[1]) if Box_isPoint: x3d, y3d, z3d = remove_nan_mean_value(pm, y_img, x_img, iter_max=Iter_Max_Pixel) else: x3d, y3d, z3d = uv_to_XY(x_img, y_img) pts_3d.append([x3d, y3d, z3d]) return pts_3d def process_mask_and_get_box(self, i,item, masks, pm, box_coords, Height_reduce, width_reduce, real_model_pro_isPose, use_openvino_model): """ 处理mask，提取区域点云和box（内缩和外框） 返回 box (内缩), box_outside(外框), pm_seg(区域点云) """ if real_model_pro_isPose: # 关键点模型的box四点坐标已经给出 mask = np.zeros(pm.shape[:2], dtype=np.uint8) if item[0][0] < item[1][0]: arr = [[item[0][0], item[0][1]], [item[1][0], item[1][1]], [item[3][0], item[3][1]], [item[2][0], item[2][1]]] else: arr = [[item[3][0], item[3][1]], [item[2][0], item[2][1]], [item[0][0], item[0][1]], [item[1][0], item[1][1]]] box = shrink_quadrilateral(arr, Height_reduce) pts = np.array(box, np.int32) cv2.fillPoly(mask, [pts], 255) pm_seg = pm[mask == 255] box = np.array(box).reshape((-1, 1, 2)).astype(np.int32) box_outside = np.array(arr).reshape((-1, 1, 2)).astype(np.int32) else: # 分割模型 box_x1, box_y1, box_x2, box_y2 = box_coords if not use_openvino_model: mask = masks[i].cpu().data.numpy().astype(int) else: mask = masks[i].astype(int) mask = mask[box_y1:box_y2, box_x1:box_x2] 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)] = (0, 255, 255) imgray = cv2.cvtColor(mask_colored, cv2.COLOR_BGR2GRAY) ret, binary = cv2.threshold(imgray, 10, 255, 0) contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) 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 and rect[1][1] - Height_reduce > 30: rect_reduce = (rect[0], (rect[1][0] - width_reduce, rect[1][1] - Height_reduce), rect[2]) else: rect_reduce = rect box_outside = cv2.boxPoints(rect) startidx = box_outside.sum(axis=1).argmin() box_outside = np.roll(box_outside, 4 - startidx, 0).astype(np.int32).reshape((-1, 1, 2)) box_reduce = cv2.boxPoints(rect_reduce) startidx = box_reduce.sum(axis=1).argmin() box_reduce = np.roll(box_reduce, 4 - startidx, 0).astype(np.int32).reshape((-1, 1, 2)) box_outside += np.array([[[box_x1, box_y1]]] * 4) box = box_reduce + np.array([[[box_x1, box_y1]]] * 4) mask_inside = np.zeros(binary.shape, np.uint8) cv2.fillPoly(mask_inside, [box_reduce], (255)) pixel_point2 = cv2.findNonZero(mask_inside) 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]) pm_seg = np.array(select_point).reshape(-1, 3) pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=1), :] return box, box_outside, pm_seg,max_contour def fit_plane_and_get_normal(self, pm_seg): if pm_seg.shape[0] < 100: print("分割点云数量较少，无法拟合平面") return None 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 return [a, b, c] def get_position_test(self, Use_Pose_Model_Pro=False, Point_isVision=False, Box_isPoint=True, First_Depth=True, Iter_Max_Pixel=30, save_img_point=0, Height_reduce=80, width_reduce=60, Xmin=160, Xmax=1050, Ymin=290, Ymax=780): if self.camera_rvc.caminit_isok: print("RVC X Camera is not opened!") return 0, None, None, None, None # 这里示例用固定路径，建议修改为参数输入 xyz_path = 'D:/pychram_rob/AutoControlSystem-git/Vision/model/data/2024_11_29_10_05_58.xyz' img_path = 'D:/pychram_rob/AutoControlSystem-git/Vision/model/data/2024_11_29_10_05_58.png' img, pm = self.read_data(xyz_path, img_path) if save_img_point != 0: free_space = get_disk_space(path=os.getcwd()) if free_space < 15: print('系统内存不足，无法保存数据') save_img_point = 0 else: save_path = os.path.join(os.getcwd(), 'Vision/model/data/', time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime())) self.save_data(img, pm, save_img_point, save_path) flag, det_cpu, category_names, extra, real_model_pro_isPose = self.model_inference(img, Use_Pose_Model_Pro) if flag != 1: print("模型推理失败") return 1, img, None, None, None xyz_list = [] normal_list = [] area_list = [] depth_list = [] uv_list = [] seg_point_list = [] box_list = [] for i, item in enumerate(det_cpu): if real_model_pro_isPose: box_coords = None else: box_coords = item[0:4].astype(np.int32) masks = extra if not real_model_pro_isPose else None box, box_outside, pm_seg,max_contour = self.process_mask_and_get_box(i, item, masks, pm, box_coords, Height_reduce, width_reduce, real_model_pro_isPose, self.use_openvino_model) if pm_seg.shape[0] < 100: continue normal = self.fit_plane_and_get_normal(pm_seg) if normal is None: continue # 计算中心点坐标 if real_model_pro_isPose: x_center = int((item[0][0] + item[1][0] + item[2][0] + item[3][0]) / 4) y_center = int((item[0][1] + item[1][1] + item[2][1] + item[3][1]) / 4) else: x_center = int(np.mean(box[:, 0, 0])) y_center = int(np.mean(box[:, 0, 1])) # 确保中心点坐标在范围内 if x_center < Xmin or x_center > Xmax or y_center < Ymin or y_center > Ymax: continue # 获取中心点点云坐标 point_x, point_y, point_z = remove_nan_mean_value(pm, y_center, x_center, iter_max=Iter_Max_Pixel) if np.isnan(point_x): continue # 计算面积（如果有轮廓） if real_model_pro_isPose: area = 0 else: area = cv2.contourArea(max_contour) if 'max_contour' in locals() else 0 xyz = [point_x, point_y, point_z] if self.cameraType == 'RVC': xyz = [v * 1000 for v in xyz] # 换单位为mm depth_list.append(point_z * 1000) else: depth_list.append(point_z) xyz_list.append(xyz) normal_list.append(normal) area_list.append(area) uv_list.append([x_center, y_center]) seg_point_list.append(pm_seg) box = box.reshape(-1,2) print("box.shape:", box.shape) print("box example:", box) box_3d_points = self.get_box_3d_points(pm, box, Box_isPoint) box_list.append(box_3d_points) # 画图示例 cv2.polylines(img, [box], True, (0, 255, 0), 2) cv2.polylines(img, [box_outside], True, (226, 12, 89), 2) cv2.circle(img, (x_center, y_center), 2, (255, 255, 255), 3) # 选取最终结果索引 idx = find_position(depth_list, area_list, 100, First_Depth) if idx is None: return 1, img, None, None, None # 标记最终中心点 cv2.circle(img, (uv_list[idx][0], uv_list[idx][1]), 30, (0, 0, 255), 10) # 点云可视化示例 if Point_isVision: pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(seg_point_list[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]) # 保存图像 if save_img_point in (2, 4): save_img = cv2.resize(img, (720, 540)) save_path = os.path.join(os.getcwd(), 'Vision/model/data/', time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime())) cv2.imwrite(save_path + '.png', save_img) return 1, img, xyz_list[idx], normal_list[idx], box_list[idx] def get_take_photo_position(self, Height_reduce = 30, width_reduce = 30): """ 检测当前拍照点能否检测到料袋 :param Height_reduce: :param width_reduce: :return ret: bool 相机是否正常工作 :return img: ndarry 返回img :return find_target: bool 是否有目标 :return xyz: list 目标中心点云值，形如[x,y,z] """ ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及 find_target = False if self.camera_rvc.caminit_isok == True: if ret == 1: 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 = [] RegionalArea = [] Depth_Z = [] uv = [] for i, item in enumerate(det_cpu): find_target = True # 画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]) # 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) 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]) 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) cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 if np.isnan(point_x): # 点云值为无效值 continue else: 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) RegionalArea.append(cv2.contourArea(max_contour)) uv.append([x_rotation_center, y_rotation_center]) 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,True) if _idx == None: return 1, img, find_target, None else: cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 return 1, img, find_target, xyz[_idx] else: return 0, None, None else: return 0, None, None pass def get_center_position(self): "" ''' :param api: None :return: ret , img, (x,y,z) 图像中心点位置对应的点云数据 ''' ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及 if self.camera_rvc.caminit_isok == True: if ret: if pm != 'None': pm_shape_y = pm.shape[0] pm_shape_x = pm.shape[1] center_point = [int(pm_shape_y/2), int(pm_shape_x/2)] point_x, point_y, point_z = remove_nan_mean_value(pm, center_point[0], center_point[1]) return img, [point_x, point_y, point_z] else: print('点云值为NAN') return None, None else: return None, None else: return None, None def release(self): self.camera_rvc.release() self.model.clear()