From 5f093a255101b10a3dfc99423b399346fbea411a Mon Sep 17 00:00:00 2001 From: HJW <1576345902@qq.com> Date: Wed, 18 Sep 2024 18:31:08 +0800 Subject: [PATCH] =?UTF-8?q?UPDATE=20Vision=20=E6=96=B0=E5=A2=9Ebox[]?= =?UTF-8?q?=E8=BE=93=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Vision/camera_coordinate_dete.py | 26 ++++++++++++++---- Vision/camera_coordinate_dete_img.py | 31 +++++++++++++++------ Vision/tool/utils.py | 41 ++++++++++++++++++++++++++++ 3 files changed, 84 insertions(+), 14 deletions(-) diff --git a/Vision/camera_coordinate_dete.py b/Vision/camera_coordinate_dete.py index 642e7f4..f1b151a 100644 --- a/Vision/camera_coordinate_dete.py +++ b/Vision/camera_coordinate_dete.py @@ -17,6 +17,8 @@ from Vision.yolo.yolov8_openvino import yolov8_segment_openvino 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 +from Vision.tool.utils import compute_pca_direction import time import os @@ -74,6 +76,8 @@ class Detection: Depth_Z = [] uv = [] seg_point = [] + box_list = [] + pca_list = [] if Point_isVision==True: pm2 = pm.copy() pm2 = pm2.reshape(-1, 3) @@ -150,6 +154,8 @@ class Detection: # cv2.imshow('result', piont_result) + pca_list.append(compute_pca_direction(pm_seg)) + ''' 拟合平面，计算法向量 ''' @@ -180,7 +186,10 @@ class Detection: # 在原图上画出预测的外接矩形 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]]] - + box_piont_x1, box_piont_y1, box_piont_z1 = remove_nan(pm, box[0][0][1], box[0][0][0]) + box_piont_x2, box_piont_y2, box_piont_z2 = remove_nan(pm, box[1][0][1], box[1][0][0]) + box_piont_x3, box_piont_y3, box_piont_z3 = remove_nan(pm, box[2][0][1], box[2][0][0]) + box_piont_x4, box_piont_y4, box_piont_z4 = remove_nan(pm, 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 = pm[y_rotation_center, x_rotation_center] @@ -188,6 +197,11 @@ class Detection: if np.isnan(point_x): # 点云值为无效值 continue else: + box_list.append( + [[box_piont_x1, box_piont_y1, box_piont_z1], + [box_piont_x2, box_piont_y2, box_piont_z2], + [box_piont_x3, box_piont_y3, box_piont_z3], + [box_piont_x4, box_piont_y4, box_piont_z4]]) xyz.append([point_x*1000, point_y*1000, point_z*1000]) Depth_Z.append(point_z*1000) nx_ny_nz.append([a, b, c]) @@ -201,7 +215,7 @@ class Detection: if _idx == None: return 1, img, None, None else: - cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (255, 0, 0), 20) # 标出中心点 + cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 if Point_isVision==True: pcd = o3d.geometry.PointCloud() @@ -217,20 +231,20 @@ class Detection: if save_img_piont == True: save_img = cv2.resize(img, (720, 540)) cv2.imwrite(save_img_name, save_img) - return 1, img, xyz[_idx], nx_ny_nz[_idx] + return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx], pca_list[_idx] else: if save_img_piont == True: save_img = cv2.resize(img,(720, 540)) cv2.imwrite(save_img_name, save_img) - return 1, img, None, None + return 1, img, None, None, None, None else: print("RVC X Camera capture failed!") - return 0, None, None, None + return 0, None, None, None, None, None else: print("RVC X Camera is not opened!") - return 0, None, None, None + return 0, None, None, None, None, None def release(self): self.camera_rvc.release() diff --git a/Vision/camera_coordinate_dete_img.py b/Vision/camera_coordinate_dete_img.py index 00d6b4c..f977c99 100644 --- a/Vision/camera_coordinate_dete_img.py +++ b/Vision/camera_coordinate_dete_img.py @@ -18,6 +18,10 @@ from Vision.yolo.yolov8_openvino import yolov8_segment_openvino 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 +from Vision.tool.utils import compute_pca_direction +from Vision.tool.utils import pca + import os class Detection: @@ -34,8 +38,8 @@ class Detection: device = 'CPU' self.model = yolov8_segment_openvino(model_path, device, conf_thres=0.3, iou_thres=0.3) - img_path = ''.join([os.getcwd(), '/Vision/model/data/test0910.png']) - point_path = ''.join([os.getcwd(), '/Vision/model/data/test0910.xyz']) + img_path = ''.join([os.getcwd(), '/Vision/model/data/test0911.png']) + point_path = ''.join([os.getcwd(), '/Vision/model/data/test0911.xyz']) self.img = cv2.imread(img_path) self.point = np.loadtxt(point_path).reshape((1080, 1440, 3)) @@ -81,6 +85,9 @@ class Detection: Depth_Z = [] uv = [] seg_point = [] + box_list = [] + pca_list = [] + pca_nx_ny_nz = [] if Point_isVision == True: pm2 = pm.copy() @@ -156,7 +163,8 @@ class Detection: print("分割点云数量较少，无法拟合平面") continue # cv2.imshow('result', piont_result) - + pca_list.append(compute_pca_direction(pm_seg)) + pca_nx_ny_nz.append(pca(pm_seg)) ''' 拟合平面，计算法向量 ''' @@ -181,12 +189,17 @@ class Detection: rect = cv2.minAreaRect(max_contour) # cv2.boxPoints可以将轮廓点转换为四个角点坐标 box = cv2.boxPoints(rect) + # 这一步不影响后面的画图，但是可以保证四个角点坐标为顺时针 startidx = box.sum(axis=1).argmin() - box = np.roll(box, 4 - startidx, 0) + box1 = 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]]] + box_piont_x1, box_piont_y1, box_piont_z1 = remove_nan(pm, box[0][0][1], box[0][0][0]) + box_piont_x2, box_piont_y2, box_piont_z2 = remove_nan(pm, box[1][0][1], box[1][0][0]) + box_piont_x3, box_piont_y3, box_piont_z3 = remove_nan(pm, box[2][0][1], box[2][0][0]) + box_piont_x4, box_piont_y4, box_piont_z4 = remove_nan(pm, 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) @@ -195,6 +208,8 @@ class Detection: if np.isnan(point_x): # 点云值为无效值 continue else: + box_list.append([[box_piont_x1, box_piont_y1, box_piont_z1], [box_piont_x2, box_piont_y2, box_piont_z2], + [box_piont_x3, box_piont_y3, box_piont_z3], [box_piont_x4, box_piont_y4, box_piont_z4]]) xyz.append([point_x * 1000, point_y * 1000, point_z * 1000]) Depth_Z.append(point_z * 1000) nx_ny_nz.append([a, b, c]) @@ -206,7 +221,7 @@ class Detection: _idx = find_position(Depth_Z, RegionalArea, 100, True) if _idx == None: - return 1, img, None, None + return 1, img, None, None, None else: cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点 @@ -225,17 +240,17 @@ class Detection: save_img = cv2.resize(img,(720, 540)) cv2.imwrite(save_img_name, save_img) - return 1, img, xyz[_idx], nx_ny_nz[_idx] + return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx], pca_list[_idx], pca_nx_ny_nz[_idx] else: if save_img_piont == True: save_img = cv2.resize(img, (720, 540)) cv2.imwrite(save_img_name, save_img) - return 1, img, None, None + return 1, img, None, None, None, None, None else: print("RVC X Camera capture failed!") - return 0, None, None, None + return 0, None, None, None, None, None, None def release(self): self.model.clear() diff --git a/Vision/tool/utils.py b/Vision/tool/utils.py index 8908f3d..e9a86e8 100644 --- a/Vision/tool/utils.py +++ b/Vision/tool/utils.py @@ -12,6 +12,47 @@ import numpy as np import cv2 import psutil from psutil._common import bytes2human +from sklearn.decomposition import PCA + +def pca(points): + # 计算PCA + mean = np.mean(points, axis=0) + cov = np.cov(points, rowvar=False) + eigenvalues, eigenvectors = np.linalg.eigh(cov) + + # 对特征值进行排序并找到对应的特征向量 + sorted_indices = np.argsort(eigenvalues)[::-1] + sorted_eigenvalues = eigenvalues[sorted_indices] + sorted_eigenvectors = eigenvectors[:, sorted_indices] + + # # 打印PCA结果 + # print("PCA Mean:", mean) + # print("PCA Eigenvalues:", sorted_eigenvalues) + # print("PCA Eigenvectors:\n", sorted_eigenvectors) + + # 通常，最小特征值对应的特征向量是平面的法线 + normal_vector = sorted_eigenvectors[:, 0] if sorted_eigenvalues[0] < sorted_eigenvalues[ + 1] else sorted_eigenvectors[:, 1] + # print("法向量：", normal_vector) + return normal_vector + + +def compute_pca_direction(points): + # points 是物体点云的坐标 + pca = PCA(n_components=3) + pca.fit(points) + primary_direction = pca.components_[0] # 第一主成分对应的方向向量 + return primary_direction + +def remove_nan(pm, y, x): + piont_x, piont_y, piont_z = pm[y, x] + if np.isnan(piont_x): + for i in range(10): + piont_x, piont_y, piont_z = pm[y+i, x] + if np.isnan(piont_x)==False: + break + return piont_x, piont_y, piont_z + def get_disk_space(path='C:'): usage = psutil.disk_usage(path)