UPDATE Vision 修复Z轴抓取

Vision/tool/utils.py

@@ -164,19 +164,27 @@ def get_disk_space(path='C:'):
 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]
+        # 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]
+        # 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']
 
@@ -328,6 +336,52 @@ def draw_comparison(img1, img2, name1, name2, fontsize=2.6, text_thickness=3):
 
 
 
+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