diff --git a/Vision/camera_coordinate_dete.py b/Vision/camera_coordinate_dete.py index 51091fa..b613267 100644 --- a/Vision/camera_coordinate_dete.py +++ b/Vision/camera_coordinate_dete.py @@ -13,6 +13,7 @@ import cv2 import open3d as o3d from Vision.tool.CameraRVC import camera from Vision.yolo.yolov8_pt_seg import yolov8_segment +from Vision.tool.utils import find_position class Detection: @@ -152,27 +153,30 @@ class Detection: 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]) + uv.append([x_rotation_center, y_rotation_center]) seg_point.append(pm_seg) cv2.polylines(img, [box], True, (0, 255, 0), 2) - min_value = min(Depth_Z) # 求深度最小值 - min_idx = Depth_Z.index(min_value) # 求最小值对应索引 - cv2.circle(img, (uv[min_idx][0], uv[min_idx][1]), 30, (255, 0, 0), 20) # 标出中心点 + _idx = find_position(Depth_Z, RegionalArea, 100,True) - if Point_isVision==True: - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(seg_point[min_idx]) - plane_model, inliers = pcd.segment_plane(distance_threshold=0.01, - ransac_n=5, - num_iterations=5000) - 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) - outlier_cloud.paint_uniform_color([0, 1, 0]) - o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2]) + if _idx == None: + return 1, img, None, None + else: + cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (255, 0, 0), 20) # 标出中心点 - return 1, img, xyz[min_idx], nx_ny_nz[min_idx] + if Point_isVision==True: + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(seg_point[_idx]) + plane_model, inliers = pcd.segment_plane(distance_threshold=0.01, + ransac_n=5, + num_iterations=5000) + 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) + outlier_cloud.paint_uniform_color([0, 1, 0]) + o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2]) + + return 1, img, xyz[_idx], nx_ny_nz[_idx] else: return 1, img, None, None