#!/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 from .tool.CameraRVC import camera from .yolo.yolov8_pt_seg import yolov8_segment class Detection: def __init__(self, model_path, device): self.camera_rvc = camera() self.model = yolov8_segment() self.model.load_model(model_path, device) if self.camera_rvc.caminit_isok == True: print("RVC X Camera is opened!") else: print("RVC X Camera is not opened!") def get_position(self): "" ''' :param api: None :return: ret , img, (x,y,z), (nx, ny, nz) ''' ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及 if self.camera_rvc.caminit_isok == True: if ret == 1: flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0) if flag == 1: xyz = [] nx_ny_nz = [] for i, item in enumerate(det_cpu): # 画box box_x1, box_y1, box_x2, box_y2 = item[0:4].astype(np.int32) label = category_names[int(item[5])] 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) mask = masks[i].cpu().data.numpy().astype(int) # mask = masks[i].numpy().astype(int) bbox_image = img[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)] = 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_piont_list = contours_in(contours) # print(len(all_piont_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 ''' 提取区域范围内的（x, y） ''' mask_inside = np.zeros(binary.shape, np.uint8) cv2.drawContours(mask_inside, [max_contour], 0, 255, -1) pixel_point2 = cv2.findNonZero(mask_inside) # result = np.zeros_like(color_image) select_point = [] for i in range(pixel_point2.shape[0]): # k = pixel_point2[i] select_point.append(pm[pixel_point2[i][0][1], pixel_point2[i][0][0]]) 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 # cv2.imshow('result', piont_result) ''' 拟合平面，计算法向量 ''' pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(pm_seg) # pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz") plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100) [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]) ''' 拟合最小外接矩形，计算矩形中心 ''' rect = cv2.minAreaRect(max_contour) # cv2.boxPoints可以将轮廓点转换为四个角点坐标 box = cv2.boxPoints(rect) # 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针 startidx = box.sum(axis=1).argmin() box = np.roll(box, 4 - startidx, 0) # 在原图上画出预测的外接矩形 box = box.reshape((-1, 1, 2)).astype(np.int32) box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]] 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 =pm[x_rotation_center, y_rotation_center] cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 x1, y1, z1, = pm[x_rotation_center+1, y_rotation_center+1] print('x1 y1 z1 :', x1, y1, z1) #print('像素坐标（x, y）：', x_rotation_center,y_rotation_center) print('x y z :', point_x, point_y, point_z) # point_x=point_x*1000 # point_y=point_y*1000 # point_z=point_z*1000 print('nx ny nz :', a, b, c) # getPosition(x, y, z, a, b, c) if(point_x == np.nan): continue else: xyz.append([x1*1000, y1*1000, z1*1000]) nx_ny_nz.append([a, b, c]) cv2.polylines(img, [box], True, (0, 255, 0), 2) return 1, img, xyz, nx_ny_nz else: return 1, img, None, None else: print("RVC X Camera capture failed!") return 0, None, None, None else: print("RVC X Camera is not opened!") return 0, None, None, None