UPDATE Vision box：点云与相机坐标

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)

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)
+                            box_list.append([[x1, y1, z1],
-                        Depth_Z.append(point_z * 1000)
+                                             [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])

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