From 25bb85dca1d33563aa537eed62553960992ae15d Mon Sep 17 00:00:00 2001 From: HJW <1576345902@qq.com> Date: Wed, 9 Oct 2024 17:34:39 +0800 Subject: [PATCH] =?UTF-8?q?UPDATE=20Vision=20box=EF=BC=9A=E7=82=B9?= =?UTF-8?q?=E4=BA=91=E4=B8=8E=E7=9B=B8=E6=9C=BA=E5=9D=90=E6=A0=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Vision/camera_coordinate_dete.py | 13 ++++-- Vision/camera_coordinate_dete_img.py | 24 ++++++++--- Vision/tool/utils.py | 60 ++++++++++++++++++++++++++++ 3 files changed, 89 insertions(+), 8 deletions(-) diff --git a/Vision/camera_coordinate_dete.py b/Vision/camera_coordinate_dete.py index 564f075..1864826 100644 --- a/Vision/camera_coordinate_dete.py +++ b/Vision/camera_coordinate_dete.py @@ -20,6 +20,7 @@ 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 +from Vision.tool.utils import uv_to_XY import time import os @@ -244,7 +245,10 @@ class Detection: box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0]) box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0]) else: - pass + 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) @@ -252,7 +256,6 @@ class Detection: if np.isnan(point_x): # 点云值为无效值 continue else: - if Box_isPoint == True: box_list.append( [[box_point_x1, box_point_y1, box_point_z1], @@ -260,7 +263,11 @@ class Detection: [box_point_x3, box_point_y3, box_point_z3], [box_point_x4, box_point_y4, box_point_z4]]) else: - box_list.append(box) + 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) diff --git a/Vision/camera_coordinate_dete_img.py b/Vision/camera_coordinate_dete_img.py index 11b6f53..d740b86 100644 --- a/Vision/camera_coordinate_dete_img.py +++ b/Vision/camera_coordinate_dete_img.py @@ -21,6 +21,7 @@ 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 +from Vision.tool.utils import uv_to_XY import os class Detection: @@ -45,7 +46,7 @@ class Detection: pass - def get_position(self, Point_isVision=False, Box_isPoint=True, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30): + def get_position(self, Point_isVision=False, Box_isPoint=False, save_img_point=0, seg_distance_threshold = 0.01, Height_reduce = 30, width_reduce = 30): """ 检测料袋相关信息 :param Point_isVision: 点云可视化 @@ -209,8 +210,8 @@ class Detection: 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") + # 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) @@ -230,7 +231,10 @@ class Detection: box_point_x3, box_point_y3, box_point_z3 = remove_nan_mean_value(pm, box[2][0][1], box[2][0][0]) box_point_x4, box_point_y4, box_point_z4 = remove_nan_mean_value(pm, box[3][0][1], box[3][0][0]) else: - pass + x1, y1, z1 = uv_to_XY(self.cameraType, box[0][0][0], box[0][0][1]) + x2, y2, z2 = uv_to_XY(self.cameraType, box[1][0][0], box[1][0][1]) + x3, y3, z3 = uv_to_XY(self.cameraType, box[2][0][0], box[2][0][1]) + x4, y4, z4 = uv_to_XY(self.cameraType, 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) @@ -245,8 +249,18 @@ class Detection: [box_point_x3, box_point_y3, box_point_z3], [box_point_x4, box_point_y4, box_point_z4]]) else: - box_list.append(box) - Depth_Z.append(point_z * 1000) + 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) + #Depth_Z.append(point_z * 1000) nx_ny_nz.append([a, b, c]) RegionalArea.append(cv2.contourArea(max_contour)) uv.append([x_rotation_center, y_rotation_center]) diff --git a/Vision/tool/utils.py b/Vision/tool/utils.py index 028a009..81248b8 100644 --- a/Vision/tool/utils.py +++ b/Vision/tool/utils.py @@ -14,6 +14,66 @@ 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