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

UPDATE Vision 新增Nan值判断

Browse Source
This commit is contained in:
HJW
2024-09-24 09:34:59 +08:00
parent 41cf46e71d
commit 5295630001
6 changed files with 165 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
Vision/camera_coordinate_dete.py
View File

@ -11,13 +11,15 @@
import numpy as np
import cv2
import open3d as o3d
from Vision.tool.CameraRVC import camera
# from Vision.tool.CameraRVC import camera
from Vision.tool.CameraPe import camera
from Vision.yolo.yolov8_pt_seg import yolov8_segment
from Vision.yolo.yolov8_openvino import yolov8_segment_openvino
from Vision.tool.utils import find_position
from Vision.tool.utils import class_names
from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan
from Vision.tool.utils import remove_nan_mean_value
from Vision.tool.utils import out_bounds_dete
import time
import os
@ -39,7 +41,7 @@ class Detection:
def get_position(self, Point_isVision=False, save_img_piont=True):
def get_position(self, Point_isVision=False, save_img_point=False, seg_distance_threshold= 10):
""
'''
:param api: None
@ -48,22 +50,22 @@ class Detection:
ret, img, pm = self.camera_rvc.get_img_and_point_map() # 拍照，获取图像及
if self.camera_rvc.caminit_isok == True:
if ret == 1:
if save_img_piont == True:
if save_img_point == True:
if get_disk_space(path=os.getcwd()) < 15: # 内存小于15G,停止保存数据
save_img_piont = False
save_img_point = False
print('系统内存不足，无法保存数据')
else:
save_path = ''.join([os.getcwd(), '/Vision/model/data/',
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
save_img_name = ''.join([save_path, '.png'])
save_piont_name = ''.join([save_path, '.xyz'])
row_list = list(range(1, 1080, 2))
column_list = list(range(1, 1440, 2))
save_point_name = ''.join([save_path, '.xyz'])
row_list = list(range(1, img.shape[0], 2))
column_list = list(range(1, img.shape[1], 2))
pm_save = pm.copy()
pm_save1 = np.delete(pm_save, row_list, axis=0)
point_new = np.delete(pm_save1, column_list, axis=1)
point_new = point_new.reshape(-1, 3)
np.savetxt(save_piont_name, point_new)
np.savetxt(save_point_name, point_new)
if self.use_openvino_model == False:
flag, det_cpu, dst_img, masks, category_names = self.model.model_inference(img, 0)
else:
@ -124,8 +126,8 @@ class Detection:
# cv2.imshow('bin',binary)
# cv2.waitKey(1)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
# all_piont_list = contours_in(contours)
# print(len(all_piont_list))
# all_point_list = contours_in(contours)
# print(len(all_point_list))
max_contour = None
max_perimeter = 0
for contour in contours: # 排除小分割区域或干扰区域
@ -150,13 +152,13 @@ class Detection:
print("分割点云数量较少，无法拟合平面")
continue
# cv2.imshow('result', piont_result)
# cv2.imshow('result', point_result)
'''
拟合平面，计算法向量
'''
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg)
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
plane_model, inliers = pcd.segment_plane(distance_threshold=seg_distance_threshold,
ransac_n=5,
num_iterations=5000)
[a, b, c, d] = plane_model
@ -181,22 +183,28 @@ class Detection:
# 在原图上画出预测的外接矩形
box = box.reshape((-1, 1, 2)).astype(np.int32)
box = box + [[[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]], [[box_x1, box_y1]]]
box_piont_x1, box_piont_y1, box_piont_z1 = remove_nan(pm, box[0][0][1], box[0][0][0])
box_piont_x2, box_piont_y2, box_piont_z2 = remove_nan(pm, box[1][0][1], box[1][0][0])
box_piont_x3, box_piont_y3, box_piont_z3 = remove_nan(pm, box[2][0][1], box[2][0][0])
box_piont_x4, box_piont_y4, box_piont_z4 = remove_nan(pm, box[3][0][1], box[3][0][0])
box[0][0][1], box[0][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[0][0][1], box[0][0][0])
box[1][0][1], box[1][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[1][0][1], box[1][0][0])
box[2][0][1], box[2][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[2][0][1], box[2][0][0])
box[3][0][1], box[3][0][0] = out_bounds_dete(pm.shape[0], pm.shape[1], box[3][0][1], box[3][0][0])
box_point_x1, box_point_y1, box_point_z1 = remove_nan_mean_value(pm, box[0][0][1], box[0][0][0])
box_point_x2, box_point_y2, box_point_z2 = remove_nan_mean_value(pm, box[1][0][1], box[1][0][0])
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])
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 = pm[y_rotation_center, x_rotation_center]
point_x, point_y, point_z = remove_nan_mean_value(pm, y_rotation_center, x_rotation_center)
cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
if np.isnan(point_x): # 点云值为无效值
continue
else:
box_list.append(
[[box_piont_x1, box_piont_y1, box_piont_z1],
[box_piont_x2, box_piont_y2, box_piont_z2],
[box_piont_x3, box_piont_y3, box_piont_z3],
[box_piont_x4, box_piont_y4, box_piont_z4]])
[[box_point_x1, box_point_y1, box_point_z1],
[box_point_x2, box_point_y2, box_point_z2],
[box_point_x3, box_point_y3, box_point_z3],
[box_point_x4, box_point_y4, box_point_z4]])
xyz.append([point_x*1000, point_y*1000, point_z*1000])
Depth_Z.append(point_z*1000)
nx_ny_nz.append([a, b, c])
@ -208,27 +216,27 @@ class Detection:
_idx = find_position(Depth_Z, RegionalArea, 100,True)
if _idx == None:
return 1, img, None, None
return 1, img, None, None, None
else:
cv2.circle(img, (uv[_idx][0], uv[_idx][1]), 30, (0, 0, 255), 20) # 标出中心点
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,
plane_model, inliers = pcd.segment_plane(distance_threshold=seg_distance_threshold,
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])
outlier_cloud.paint_uniform_color([0, 0, 1])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2])
if save_img_piont == True:
if save_img_point == True:
save_img = cv2.resize(img, (720, 540))
cv2.imwrite(save_img_name, save_img)
return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx]
else:
if save_img_piont == True:
if save_img_point == True:
save_img = cv2.resize(img,(720, 540))
cv2.imwrite(save_img_name, save_img)
return 1, img, None, None, None