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

52
Vision/camera_coordinate_dete_img.py
View File

@ -18,8 +18,8 @@ from Vision.yolo.yolov8_openvino import yolov8_segment_openvino
from Vision.tool.utils import find_position from Vision.tool.utils import find_position
from Vision.tool.utils import class_names from Vision.tool.utils import class_names
from Vision.tool.utils import get_disk_space 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 os import os
class Detection: class Detection:
@ -43,7 +43,7 @@ class Detection:
def get_position(self, Point_isVision=False, save_img_piont=True): def get_position(self, Point_isVision=False, save_img_point=True, seg_distance_threshold = 10):
"" ""
''' '''
:param api: None :param api: None
@ -54,22 +54,22 @@ class Detection:
img = self.img img = self.img
pm = self.point pm = self.point
if ret == 1: if ret == 1:
if save_img_piont == True: if save_img_point == True:
if get_disk_space(path=os.getcwd())<15: # 内存小于15G,停止保存数据 if get_disk_space(path=os.getcwd())<15: # 内存小于15G,停止保存数据
save_img_piont = False save_img_point = False
print('系统内存不足，无法保存数据') print('系统内存不足，无法保存数据')
else: else:
save_path = ''.join([os.getcwd(), '/Vision/model/data/', save_path = ''.join([os.getcwd(), '/Vision/model/data/',
time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))]) time.strftime('%Y_%m_%d_%H_%M_%S', time.localtime(time.time()))])
save_img_name = ''.join([save_path, '.png']) save_img_name = ''.join([save_path, '.png'])
save_piont_name = ''.join([save_path, '.xyz']) save_point_name = ''.join([save_path, '.xyz'])
row_list = list(range(1, 1080, 2)) row_list = list(range(1, img.shape[0], 2))
column_list = list(range(1, 1440, 2)) column_list = list(range(1, img.shape[1], 2))
pm_save = pm.copy() pm_save = pm.copy()
pm_save1 = np.delete(pm_save, row_list, axis=0) pm_save1 = np.delete(pm_save, row_list, axis=0)
point_new = np.delete(pm_save1, column_list, axis=1) point_new = np.delete(pm_save1, column_list, axis=1)
point_new = point_new.reshape(-1, 3) 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: if self.use_openvino_model == False:
@ -133,8 +133,8 @@ class Detection:
# cv2.imshow('bin',binary) # cv2.imshow('bin',binary)
# cv2.waitKey(1) # cv2.waitKey(1)
contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE) contours, hierarchy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
# all_piont_list = contours_in(contours) # all_point_list = contours_in(contours)
# print(len(all_piont_list)) # print(len(all_point_list))
max_contour = None max_contour = None
max_perimeter = 0 max_perimeter = 0
for contour in contours: # 排除小分割区域或干扰区域 for contour in contours: # 排除小分割区域或干扰区域
@ -158,13 +158,13 @@ class Detection:
if pm_seg.size < 300: if pm_seg.size < 300:
print("分割点云数量较少，无法拟合平面") print("分割点云数量较少，无法拟合平面")
continue continue
# cv2.imshow('result', piont_result) # cv2.imshow('result', point_result)
''' '''
拟合平面，计算法向量 拟合平面，计算法向量
''' '''
pcd = o3d.geometry.PointCloud() pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(pm_seg) 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, ransac_n=5,
num_iterations=5000) num_iterations=5000)
[a, b, c, d] = plane_model [a, b, c, d] = plane_model
@ -190,20 +190,26 @@ class Detection:
# 在原图上画出预测的外接矩形 # 在原图上画出预测的外接矩形
box = box.reshape((-1, 1, 2)).astype(np.int32) 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 = 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[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_piont_x3, box_piont_y3, box_piont_z3 = remove_nan(pm, box[2][0][1], box[2][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_piont_x4, box_piont_y4, box_piont_z4 = remove_nan(pm, box[3][0][1], box[3][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) 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[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) # 标出中心点 cv2.circle(img, (x_rotation_center, y_rotation_center), 4, (255, 255, 255), 5) # 标出中心点
if np.isnan(point_x): # 点云值为无效值 if np.isnan(point_x): # 点云值为无效值
continue continue
else: else:
box_list.append([[box_piont_x1, box_piont_y1, box_piont_z1], [box_piont_x2, box_piont_y2, box_piont_z2], box_list.append([[box_point_x1, box_point_y1, box_point_z1], [box_point_x2, box_point_y2, box_point_z2],
[box_piont_x3, box_piont_y3, box_piont_z3], [box_piont_x4, box_piont_y4, box_piont_z4]]) [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]) 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])
@ -222,7 +228,7 @@ class Detection:
if Point_isVision == True: if Point_isVision == True:
pcd = o3d.geometry.PointCloud() pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(seg_point[_idx]) 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, ransac_n=5,
num_iterations=5000) num_iterations=5000)
inlier_cloud = pcd.select_by_index(inliers) # 点云可视化 inlier_cloud = pcd.select_by_index(inliers) # 点云可视化
@ -230,13 +236,13 @@ class Detection:
outlier_cloud = pcd.select_by_index(inliers, invert=True) outlier_cloud = pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([0, 1, 0]) outlier_cloud.paint_uniform_color([0, 1, 0])
o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud, pcd2]) 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)) save_img = cv2.resize(img,(720, 540))
cv2.imwrite(save_img_name, save_img) cv2.imwrite(save_img_name, save_img)
return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx] return 1, img, xyz[_idx], nx_ny_nz[_idx], box_list[_idx]
else: else:
if save_img_piont == True: if save_img_point == True:
save_img = cv2.resize(img, (720, 540)) save_img = cv2.resize(img, (720, 540))
cv2.imwrite(save_img_name, save_img) cv2.imwrite(save_img_name, save_img)

13
Vision/tool/CameraPe.py
View File

@ -145,7 +145,7 @@ class camera():
:return: ret , img, point_map :return: ret , img, point_map
''' '''
if self.caminit_isok == False or self.event.IsOffline(): if self.caminit_isok == False or self.event.IsOffline():
return 0, None return 0, None, None
else: else:
image_list = self.cl.DeviceStreamRead(self.handle, 2000) image_list = self.cl.DeviceStreamRead(self.handle, 2000)
if len(image_list) == 2: if len(image_list) == 2:
@ -153,19 +153,18 @@ class camera():
frame = image_list[i] frame = image_list[i]
if frame.streamID == PERCIPIO_STREAM_DEPTH: if frame.streamID == PERCIPIO_STREAM_DEPTH:
img_depth = frame img_depth = frame
img_depth2p3d = frame
if frame.streamID == PERCIPIO_STREAM_COLOR: if frame.streamID == PERCIPIO_STREAM_COLOR:
img_color = frame img_color = frame
self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height, # self.cl.DeviceStreamMapDepthImageToColorCoordinate(self.depth_calib, img_depth.width, img_depth.height,
self.scale_unit, img_depth, self.color_calib, img_color.width, # self.scale_unit, img_depth, self.color_calib, img_color.width,
img_color.height, self.img_registration_depth) # img_color.height, self.img_registration_depth)
#
# self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render) # self.cl.DeviceStreamDepthRender(self.img_registration_depth, self.img_registration_render)
# mat_depth_render = self.img_registration_render.as_nparray() # mat_depth_render = self.img_registration_render.as_nparray()
# cv2.imshow('registration', mat_depth_render) # cv2.imshow('registration', mat_depth_render)
self.cl.DeviceStreamMapDepthImageToPoint3D(self.img_registration_depth, self.depth_calib, self.scale_unit, self.cl.DeviceStreamMapDepthImageToPoint3D(img_depth, self.depth_calib, self.scale_unit,
self.pointcloud_data_arr) self.pointcloud_data_arr)
# show p3d arr data # show p3d arr data

82
Vision/tool/utils.py
View File

@ -13,14 +13,82 @@ import cv2
import psutil import psutil
from psutil._common import bytes2human from psutil._common import bytes2human
def remove_nan(pm, y, x):
piont_x, piont_y, piont_z = pm[y, x] def out_bounds_dete(pm_y, pm_x, piont_y, piont_x):
if np.isnan(piont_x): if piont_y>pm_y:
for i in range(10): piont_y = pm_y
piont_x, piont_y, piont_z = pm[y+i, x] print('四坐标点超出点云大小')
if np.isnan(piont_x)==False: if piont_y<0:
piont_y=0
print('四坐标点超出点云大小')
if piont_x>pm_x:
piont_x = pm_x
print('四坐标点超出点云大小')
if piont_x<0:
piont_x=0
print('四坐标点超出点云大小')
return piont_y, piont_x
def remove_nan_mean_value(pm, y, x, iter_max=15):
point_x, point_y, point_z = pm[y, x]
if np.isnan(point_x):
print('Nan值去除')
point_x_list = []
point_y_list = []
point_z_list = []
iter_current = 1
pm_shape_y = pm.shape[0]
pm_shape_x = pm.shape[1]
remove_nan_isok = False
while iter_current < iter_max:
# 计算开始点
if y - iter_current > 0:
y_start = y - iter_current
else:
y_start = 0
if x - iter_current > 0:
x_start = x - iter_current
else:
x_start = 0
for idx_y in range(iter_current*2 + 1):
y_current = y_start + idx_y
if y_current > pm_shape_y-1:
continue
for idx_x in range(iter_current*2 + 1):
x_current = x_start + idx_x
if x_current > pm_shape_x-1:
continue
elif np.isnan(pm[y_current, x_current][0]) == False:
point_x_list.append(pm[y_current, x_current][0])
point_y_list.append(pm[y_current, x_current][1])
point_z_list.append(pm[y_current, x_current][2])
len_point_x = len(point_x_list)
if len_point_x>0:
point_x = sum(point_x_list)/len_point_x
point_y = sum(point_y_list)/len_point_x
point_z = sum(point_z_list)/len_point_x
remove_nan_isok = True
break break
return piont_x, piont_y, piont_z iter_current += 1
else:
remove_nan_isok = True
if remove_nan_isok == True:
return point_x, point_y, point_z
else:
print('在15*15范围中未找到有效值，所有点云值为无效值')
return np.nan, np.nan, np.nan
def remove_nan(pm, y, x):
point_x, point_y, point_z = pm[y, x]
if np.isnan(point_x):
for i in range(10):
point_x, point_y, point_z = pm[y+i, x]
if np.isnan(point_x)==False:
break
return point_x, point_y, point_z
def get_disk_space(path='C:'): def get_disk_space(path='C:'):

3
Vision/yolo/yolov8_openvino.py
View File

@ -42,6 +42,9 @@ class yolov8_segment_openvino:
self.boxes, self.scores, self.class_ids, mask_pred = self.process_box_output(outputs[0]) self.boxes, self.scores, self.class_ids, mask_pred = self.process_box_output(outputs[0])
self.mask_maps = self.process_mask_output(mask_pred, outputs[1]) self.mask_maps = self.process_mask_output(mask_pred, outputs[1])
if type(self.boxes) == list:
if len(self.boxes)==0:
return 0, None, None, None, None
self.boxes = self.boxes.tolist() self.boxes = self.boxes.tolist()
self.scores = self.scores.tolist() self.scores = self.scores.tolist()
for t in range(len(self.scores)): for t in range(len(self.scores)):

24
get_position_test.py
View File

@ -5,9 +5,10 @@
# @Author : hjw # @Author : hjw
# @File : get_position_test.py # @File : get_position_test.py
''' '''
from Vision.camera_coordinate_dete_img import Detection from Vision.camera_coordinate_dete import Detection
from Trace.handeye_calibration import * from Trace.handeye_calibration import *
from Vision.tool.utils import get_disk_space from Vision.tool.utils import get_disk_space
from Vision.tool.utils import remove_nan_mean_value
import platform import platform
import cv2 import cv2
@ -19,10 +20,19 @@ while True:
print('xyz点云坐标：', xyz) print('xyz点云坐标：', xyz)
print('nx_ny_nz法向量：', nx_ny_nz) print('nx_ny_nz法向量：', nx_ny_nz)
print('矩形框四顶点：', box) print('矩形框四顶点：', box)
img = cv2.resize(img,(720, 540)) # img = cv2.resize(img,(720, 540))
transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0) # (x, y, z, u, v, w) if xyz!=None:
target_position, noraml_base = getPosition(xyz[0], xyz[1], xyz[2], nx_ny_nz[0], nx_ny_nz[1], nx_ny_nz[2], transformation_matrix, box) transformation_matrix = R_matrix(521.534, 0.705, 850.03, 0.0, 90.0, 0.0) # (x, y, z, u, v, w)
print("target_position:", target_position) target_position, noraml_base = getPosition(xyz[0], xyz[1], xyz[2], nx_ny_nz[0], nx_ny_nz[1], nx_ny_nz[2], transformation_matrix, box)
print("noraml_base", noraml_base) print("target_position:", target_position)
print("noraml_base", noraml_base)
cv2.imshow('img', img) cv2.imshow('img', img)
cv2.waitKey(0) cv2.waitKey(1)
# point = np.loadtxt('./Vision/model/data/test_nan.xyz').reshape((6, 8, 3))
# a = point.copy()
# c = a.tolist()
# x, y, z = remove_nan_mean_value(point,3,3)
# print(x, y, z )
# pass