xiongyi/AutoControlSystem-git
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新 Vision/camera_coordinate_dete.py

Browse Source
This commit is contained in:
hjw
2024-09-05 09:25:20 +00:00
parent 7c03cd8234
commit 92f7030306
1 changed files with 164 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
Vision/camera_coordinate_dete.py
View File

@ -11,8 +11,8 @@
import numpy as np import numpy as np
import cv2 import cv2
import open3d as o3d import open3d as o3d
from .tool.CameraRVC import camera from Vision.tool.CameraRVC import camera
from .yolo.yolov8_pt_seg import yolov8_segment from Vision.yolo.yolov8_pt_seg import yolov8_segment
class Detection: class Detection:
@ -39,6 +39,9 @@ class Detection:
if flag == 1: if flag == 1:
xyz = [] xyz = []
nx_ny_nz = [] nx_ny_nz = []
RegionalArea = []
Depth_Z = []
uv = []
for i, item in enumerate(det_cpu): for i, item in enumerate(det_cpu):
# 画box # 画box
@ -89,8 +92,7 @@ class Detection:
# result = np.zeros_like(color_image) # result = np.zeros_like(color_image)
select_point = [] select_point = []
for i in range(pixel_point2.shape[0]): for i in range(pixel_point2.shape[0]):
# k = pixel_point2[i] select_point.append(pm[pixel_point2[i][0][1]+box_y1, pixel_point2[i][0][0]]+box_x1)
select_point.append(pm[pixel_point2[i][0][1], pixel_point2[i][0][0]])
select_point = np.array(select_point) select_point = np.array(select_point)
pm_seg = select_point.reshape(-1, 3) pm_seg = select_point.reshape(-1, 3)
pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan pm_seg = pm_seg[~np.isnan(pm_seg).all(axis=-1), :] # 剔除 nan
@ -102,9 +104,9 @@ class Detection:
pcd = o3d.geometry.PointCloud() pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg) pcd.points = o3d.utility.Vector3dVector(pm_seg)
# pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz") # pcd = o3d.io.read_point_cloud("./Data/seg_point.xyz")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=3, ransac_n=5,
num_iterations=100) num_iterations=1000)
[a, b, c, d] = plane_model [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")
@ -130,25 +132,23 @@ class Detection:
x_rotation_center = int((box[0][0][0] + box[1][0][0] + box[2][0][0] + box[3][0][0]) / 4) 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) 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 =pm[x_rotation_center, y_rotation_center] point_x, point_y, point_z = pm[y_rotation_center, x_rotation_center]
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点 cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
x1, y1, z1, = pm[x_rotation_center+1, y_rotation_center+1] if np.isnan(point_x): # 点云值为无效值
print('x1 y1 z1 :', x1, y1, z1)
#print('像素坐标（x, y）：', x_rotation_center,y_rotation_center)
print('x y z :', point_x, point_y, point_z)
# point_x=point_x*1000
# point_y=point_y*1000
# point_z=point_z*1000
print('nx ny nz :', a, b, c)
# getPosition(x, y, z, a, b, c)
if(point_x == np.nan):
continue continue
else: else:
xyz.append([x1*1000, y1*1000, z1*1000]) xyz.append([point_x*1000, point_y*1000, point_z*1000])
Depth_Z.append(point_z*1000)
nx_ny_nz.append([a, b, c]) nx_ny_nz.append([a, b, c])
RegionalArea.append(cv2.contourArea(max_contour))
uv.append([x_rotation_center,y_rotation_center])
cv2.polylines(img, [box], True, (0, 255, 0), 2) cv2.polylines(img, [box], True, (0, 255, 0), 2)
return 1, img, xyz, nx_ny_nz min_value = min(Depth_Z) # 求深度最大值
min_idx = Depth_Z.index(min_value) # 求最大值对应索引
cv2.circle(img, (uv[min_idx][0], uv[min_idx][1]), 20, (255, 0, 0), 5) # 标出中心点
return 1, img, xyz[min_idx], nx_ny_nz[min_idx]
else: else:
return 1, img, None, None return 1, img, None, None