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-06 10:05:25 +00:00
parent 9fdf488674
commit f3ef203339
1 changed files with 34 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
Vision/camera_coordinate_dete.py
View File

@ -28,7 +28,7 @@ class Detection:
else: else:
print("RVC X Camera is not opened!") print("RVC X Camera is not opened!")
def get_position(self): def get_position(self, Point_isVision=True):
"" ""
''' '''
:param api: None :param api: None
@ -44,6 +44,16 @@ class Detection:
RegionalArea = [] RegionalArea = []
Depth_Z = [] Depth_Z = []
uv = [] uv = []
seg_point = []
if Point_isVision==True:
pm2 = pm.copy()
pm2 = pm2.reshape(-1, 3)
pm2 = pm2[~np.isnan(pm2).all(axis=-1), :]
pm2[:, 2] = pm2[:, 2] + 0.25
pcd2 = o3d.geometry.PointCloud()
pcd2.points = o3d.utility.Vector3dVector(pm2)
# o3d.visualization.draw_geometries([pcd2])
for i, item in enumerate(det_cpu): for i, item in enumerate(det_cpu):
# 画box # 画box
@ -105,12 +115,11 @@ 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")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01, plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
ransac_n=5, ransac_n=5,
num_iterations=1000) num_iterations=5000)
[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")
# inlier_cloud = pcd.select_by_index(inliers) # 点云可视化 # inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
# inlier_cloud.paint_uniform_color([1.0, 0, 0]) # inlier_cloud.paint_uniform_color([1.0, 0, 0])
@ -141,15 +150,28 @@ class Detection:
else: else:
xyz.append([point_x*1000, point_y*1000, point_z*1000]) xyz.append([point_x*1000, point_y*1000, point_z*1000])
Depth_Z.append(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)) 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) cv2.polylines(img, [box], True, (0, 255, 0), 2)
min_value = min(Depth_Z) # 求深度最大值 min_value = min(Depth_Z) # 求深度最小值
min_idx = Depth_Z.index(min_value) # 求最大值对应索引 min_idx = Depth_Z.index(min_value) # 求最小值对应索引
cv2.circle(img, (uv[min_idx][0], uv[min_idx][1]), 20, (255, 0, 0), 5) # 标出中心点 cv2.circle(img, (uv[min_idx][0], uv[min_idx][1]), 30, (255, 0, 0), 20) # 标出中心点
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])
return 1, img, xyz[min_idx], nx_ny_nz[min_idx] return 1, img, xyz[min_idx], nx_ny_nz[min_idx]
else: else:
return 1, img, None, None return 1, img, None, None
@ -162,5 +184,9 @@ class Detection:
print("RVC X Camera is not opened!") print("RVC X Camera is not opened!")
return 0, None, None, None return 0, None, None, None
def release(self):
self.camera_rvc.release()
self.model.clear()