#!/usr/bin/env python # -*- coding: UTF-8 -*- ''' @Project ：AutoControlSystem-master @File ：utils.py @IDE ：PyCharm @Author ：hjw @Date ：2024/8/29 15:07 ''' import numpy as np import cv2 import psutil from psutil._common import bytes2human def uv_to_XY(cameraType, u, v): """ 像素坐标转相机坐标 Args: cameraType: u: v: Returns: 如本： ExtrinsicMatrix: [-0.9916700124740601, -0.003792409785091877, 0.12874870002269745, 0.10222162306308746, -0.003501748666167259, 0.9999907612800598, 0.002483875723555684, -0.08221593499183655, -0.12875692546367645, 0.0020123394206166267, -0.9916741251945496, 0.6480034589767456, 0.0, 0.0, 0.0, 1.0] IntrinsicParameters: [2402.101806640625, 0.0, 739.7069091796875, 0.0, 2401.787353515625, 584.73046875, 0.0, 0.0, 1.0] distortion: [-0.04248141124844551, 0.24386045336723328, -0.38333430886268616, -0.0017840253422036767, 0.0007602088153362274] 图漾： depth image format list: 0 -size[640x480] - desc:DEPTH16_640x480 1 -size[1280x960] - desc:DEPTH16_1280x960 2 -size[320x240] - desc:DEPTH16_320x240 delth calib info: calib size :[1280x960] calib intr : (1048.3614501953125, 0.0, 652.146240234375, 0.0, 1048.3614501953125, 500.26397705078125, 0.0, 0.0, 1.0) calib extr : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) calib distortion : (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) """ x = None y = None zc = 1 # 设深度z为1 if cameraType == 'RVC': u0 = 739.70 v0 = 584.73 fx = 2402.10 fy = 2401.78 x = (u-u0)*zc/fx y = (v-v0)*zc/fy elif cameraType == 'Pe': u0 = 652.14 v0 = 500.26 fx = 1048.36 fy = 1048.36 x = (u - u0) * zc / fx y = (v - v0) * zc / fy return x, y, zc def out_bounds_dete(pm_y, pm_x, piont_y, piont_x): if piont_y>=pm_y: piont_y = pm_y-1 print('四坐标点超出点云大小') if piont_y<0: piont_y=0 print('四坐标点超出点云大小') if piont_x>=pm_x: piont_x = pm_x-1 print('四坐标点超出点云大小') if piont_x<0: piont_x=0 print('四坐标点超出点云大小') return piont_y, piont_x def remove_nan_mean_value(pm, y, x, iter_max=50): y, x = out_bounds_dete(pm.shape[0], pm.shape[1], y, x) point_x, point_y, point_z = pm[y, x] if np.isnan(point_x): point_x_list = [] point_y_list = [] point_z_list = [] iter_current = 1 pm_shape_y = pm.shape[0] pm_shape_x = pm.shape[1] remove_nan_isok = False print('Nan值去除') while iter_current < iter_max: # 计算开始点 if y - iter_current > 0: y_start = y - iter_current else: y_start = 0 if x - iter_current > 0: x_start = x - iter_current else: x_start = 0 for idx_y in range(iter_current*2 + 1): y_current = y_start + idx_y if y_current > pm_shape_y-1: continue for idx_x in range(iter_current*2 + 1): x_current = x_start + idx_x if x_current > pm_shape_x-1: continue elif np.isnan(pm[y_current, x_current][0]) == False: point_x_list.append(pm[y_current, x_current][0]) point_y_list.append(pm[y_current, x_current][1]) point_z_list.append(pm[y_current, x_current][2]) len_point_x = len(point_x_list) if len_point_x > 0: point_x = sum(point_x_list)/len_point_x point_y = sum(point_y_list)/len_point_x point_z = sum(point_z_list)/len_point_x remove_nan_isok = True break iter_current += 1 else: remove_nan_isok = True if remove_nan_isok == True: return point_x, point_y, point_z else: print(f'在{iter_max}*{iter_max}范围中未找到有效值，所有点云值为无效值') return np.nan, np.nan, np.nan def remove_nan(pm, y, x): point_x, point_y, point_z = pm[y, x] if np.isnan(point_x): for i in range(10): point_x, point_y, point_z = pm[y+i, x] if np.isnan(point_x)==False: break return point_x, point_y, point_z def get_disk_space(path='C:'): usage = psutil.disk_usage(path) space_free = bytes2human(usage.free) # space_total = bytes2human(usage.total) # space_used = bytes2human(usage.used) # space_free = bytes2human(usage.free) # space_used_percent = bytes2human(usage.percent) space_free = float(space_free[:-1]) return space_free def find_position(Depth_Z, RegionalArea, RegionalArea_Threshold, first_depth=True): if first_depth == True: sorted_id = sorted(range(len(Depth_Z)), key=lambda k: Depth_Z[k], reverse=False) # Depth_Z1 = [Depth_Z[i] for i in sorted_id] # RegionalArea1 = [RegionalArea[i] for i in sorted_id] # for i in range(len(Depth_Z1)): # if RegionalArea1[i] > RegionalArea_Threshold: # return sorted_id[i] if len(sorted_id)>0: return sorted_id[0] else: return else: sorted_id = sorted(range(len(RegionalArea)), key=lambda k: RegionalArea[k], reverse=True) # Depth_Z1 = [Depth_Z[i] for i in sorted_id] # RegionalArea1 = [RegionalArea[i] for i in sorted_id] # for i in range(len(Depth_Z1)): # if RegionalArea1[i] > RegionalArea_Threshold: # return sorted_id[i] if len(sorted_id)>0: return sorted_id[0] else: return class_names = ['box', 'other'] # Create a list of colors for each class where each color is a tuple of 3 integer values rng = np.random.default_rng(3) colors = rng.uniform(0, 255, size=(len(class_names), 3)) def nms(boxes, scores, iou_threshold): # Sort by score sorted_indices = np.argsort(scores)[::-1] keep_boxes = [] while sorted_indices.size > 0: # Pick the last box box_id = sorted_indices[0] keep_boxes.append(box_id) # Compute IoU of the picked box with the rest ious = compute_iou(boxes[box_id, :], boxes[sorted_indices[1:], :]) # Remove boxes with IoU over the threshold keep_indices = np.where(ious < iou_threshold)[0] # print(keep_indices.shape, sorted_indices.shape) sorted_indices = sorted_indices[keep_indices + 1] return keep_boxes def compute_iou(box, boxes): # Compute xmin, ymin, xmax, ymax for both boxes xmin = np.maximum(box[0], boxes[:, 0]) ymin = np.maximum(box[1], boxes[:, 1]) xmax = np.minimum(box[2], boxes[:, 2]) ymax = np.minimum(box[3], boxes[:, 3]) # Compute intersection area intersection_area = np.maximum(0, xmax - xmin) * np.maximum(0, ymax - ymin) # Compute union area box_area = (box[2] - box[0]) * (box[3] - box[1]) boxes_area = (boxes[:, 2] - boxes[:, 0]) * (boxes[:, 3] - boxes[:, 1]) union_area = box_area + boxes_area - intersection_area # Compute IoU iou = intersection_area / union_area return iou def xywh2xyxy(x): # Convert bounding box (x, y, w, h) to bounding box (x1, y1, x2, y2) y = np.copy(x) y[..., 0] = x[..., 0] - x[..., 2] / 2 y[..., 1] = x[..., 1] - x[..., 3] / 2 y[..., 2] = x[..., 0] + x[..., 2] / 2 y[..., 3] = x[..., 1] + x[..., 3] / 2 return y def sigmoid(x): return 1 / (1 + np.exp(-x)) def draw_detections(image, boxes, scores, class_ids, mask_alpha=0.3, mask_maps=None): img_height, img_width = image.shape[:2] size = min([img_height, img_width]) * 0.0006 text_thickness = int(min([img_height, img_width]) * 0.001) mask_img = draw_masks(image, boxes, class_ids, mask_alpha, mask_maps) # Draw bounding boxes and labels of detections for box, score, class_id in zip(boxes, scores, class_ids): color = colors[class_id] x1, y1, x2, y2 = box.astype(int) # Draw rectangle cv2.rectangle(mask_img, (x1, y1), (x2, y2), color, 2) label = class_names[class_id] caption = f'{label} {int(score * 100)}%' (tw, th), _ = cv2.getTextSize(text=caption, fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=size, thickness=text_thickness) th = int(th * 1.2) cv2.rectangle(mask_img, (x1, y1), (x1 + tw, y1 - th), color, -1) cv2.putText(mask_img, caption, (x1, y1), cv2.FONT_HERSHEY_SIMPLEX, size, (255, 255, 255), text_thickness, cv2.LINE_AA) return mask_img def draw_masks(image, boxes, class_ids, mask_alpha=0.3, mask_maps=None): mask_img = image.copy() # Draw bounding boxes and labels of detections for i, (box, class_id) in enumerate(zip(boxes, class_ids)): color = colors[class_id] x1, y1, x2, y2 = box.astype(int) # Draw fill mask image if mask_maps is None: cv2.rectangle(mask_img, (x1, y1), (x2, y2), color, -1) else: crop_mask = mask_maps[i][y1:y2, x1:x2, np.newaxis] crop_mask_img = mask_img[y1:y2, x1:x2] crop_mask_img = crop_mask_img * (1 - crop_mask) + crop_mask * color mask_img[y1:y2, x1:x2] = crop_mask_img return cv2.addWeighted(mask_img, mask_alpha, image, 1 - mask_alpha, 0) def draw_comparison(img1, img2, name1, name2, fontsize=2.6, text_thickness=3): (tw, th), _ = cv2.getTextSize(text=name1, fontFace=cv2.FONT_HERSHEY_DUPLEX, fontScale=fontsize, thickness=text_thickness) x1 = img1.shape[1] // 3 y1 = th offset = th // 5 cv2.rectangle(img1, (x1 - offset * 2, y1 + offset), (x1 + tw + offset * 2, y1 - th - offset), (0, 115, 255), -1) cv2.putText(img1, name1, (x1, y1), cv2.FONT_HERSHEY_DUPLEX, fontsize, (255, 255, 255), text_thickness) (tw, th), _ = cv2.getTextSize(text=name2, fontFace=cv2.FONT_HERSHEY_DUPLEX, fontScale=fontsize, thickness=text_thickness) x1 = img2.shape[1] // 3 y1 = th offset = th // 5 cv2.rectangle(img2, (x1 - offset * 2, y1 + offset), (x1 + tw + offset * 2, y1 - th - offset), (94, 23, 235), -1) cv2.putText(img2, name2, (x1, y1), cv2.FONT_HERSHEY_DUPLEX, fontsize, (255, 255, 255), text_thickness) combined_img = cv2.hconcat([img1, img2]) if combined_img.shape[1] > 3840: combined_img = cv2.resize(combined_img, (3840, 2160)) return combined_img def fit_plane_vision(box_list, normal_vector): plane_x = [] plane_y = [] plane_z = [] print(box_list) plane_x.append(box_list[0][0][0]) plane_x.append(box_list[0][1][0]) plane_x.append(box_list[0][2][0]) plane_x.append(box_list[0][3][0]) plane_y.append(box_list[0][0][1]) plane_y.append(box_list[0][1][1]) plane_y.append(box_list[0][2][1]) plane_y.append(box_list[0][3][1]) plane_z.append(box_list[0][0][2]) plane_z.append(box_list[0][1][2]) plane_z.append(box_list[0][2][2]) plane_z.append(box_list[0][3][2]) # 定义平面方程的参数 a = normal_vector[0] b = normal_vector[1] c = normal_vector[2] d = normal_vector[3] # 定义平面的范围 x_range = (int(min(plane_x)), int(max(plane_x))) y_range = (int(min(plane_y)), int(max(plane_y))) z_range = (int(min(plane_z)), int(max(plane_z))) # 生成平面网格 x = np.linspace(x_range[0], x_range[1], 10) y = np.linspace(y_range[0], y_range[1], 20) X, Y = np.meshgrid(x, y) Z = -(a * X + b * Y + d) / c # 根据平面方程计算 Z 坐标 # 确保 Z 坐标在指定范围内 Z = np.clip(Z, z_range[0], z_range[1]) # 创建 TriangleMesh 对象 import open3d as o3d plane_mesh = o3d.geometry.TriangleMesh() plane_mesh.vertices = o3d.utility.Vector3dVector(np.vstack((X.ravel(), Y.ravel(), Z.ravel())).T) plane_mesh.triangles = o3d.utility.Vector3iVector( np.array([[i, i + 1, i + 100] for i in range(99)] + [[i + 1, i + 101, i + 100] for i in range(99)])) plane_mesh.paint_uniform_color([1, 0.5, 0.5]) # 设置平面颜色 return plane_mesh